turtlebot_arm_arm_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==0x1000004a);

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

#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
#ifndef isinf
#define isinf _isinf
#endif
//#ifndef isfinite
//#define isfinite _isfinite
//#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. has to be smaller than thresholds used for branch conds and evaluation
#ifndef IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
#endif

// used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
#ifndef IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
#endif

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

// there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate
#ifndef IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
#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 IKatan2Simple(float fy, float fx) {
    return atan2f(fy,fx);
}
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 IKatan2Simple(double fy, double fx) {
    return atan2(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);
}

template <typename T>
struct CheckValue
{
    T value;
    bool valid;
};

template <typename T>
inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
{
    CheckValue<T> ret;
    ret.valid = false;
    ret.value = 0;
    if( !isnan(fy) && !isnan(fx) ) {
        if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
            ret.value = IKatan2Simple(fy,fx);
            ret.valid = true;
        }
    }
    return ret;
}

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;
}

template <typename T>
inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
{
    CheckValue<T> ret;
    ret.valid = true;
    if( n == 0 ) {
        ret.value = 1.0;
        return ret;
    }
    else if( n == 1 )
    {
        ret.value = f;
        return ret;
    }
    else if( n < 0 )
    {
        if( f == 0 )
        {
            ret.valid = false;
            ret.value = (T)1.0e30;
            return ret;
        }
        if( n == -1 ) {
            ret.value = T(1.0)/f;
            return ret;
        }
    }

    int num = n > 0 ? n : -n;
    if( num == 2 ) {
        ret.value = f*f;
    }
    else if( num == 3 ) {
        ret.value = f*f*f;
    }
    else {
        ret.value = 1.0;
        while(num>0) {
            if( num & 1 ) {
                ret.value *= f;
            }
            num >>= 1;
            f *= f;
        }
    }
    
    if( n < 0 ) {
        ret.value = T(1.0)/ret.value;
    }
    return ret;
}

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,x21,x22,x23,x24;
x0=IKcos(j[0]);
x1=IKsin(j[2]);
x2=IKcos(j[1]);
x3=IKsin(j[1]);
x4=IKcos(j[2]);
x5=IKsin(j[0]);
x6=IKcos(j[3]);
x7=IKsin(j[3]);
x8=IKcos(j[4]);
x9=IKsin(j[4]);
x10=((0.112)*x1);
x11=((0.112)*x4);
x12=((0.106)*x4);
x13=((1.0)*x6);
x14=((1.0)*x4);
x15=((1.0)*x7);
x16=((1.1e-7)*x4);
x17=((1.1e-7)*x1);
x18=(x0*x3);
x19=(x3*x5);
x20=(x1*x2);
x21=(x0*x2);
x22=(x2*x5);
x23=(x1*x3);
x24=((1.1e-7)*x21);
eetrans[0]=((-3.0e-8)+((x7*((((x11*x21))+(((-1.0)*x10*x18))))))+(((0.106)*x18))+(((-1.0)*x24))+(((-1.0)*x16*x21))+((x17*x18))+((x12*x18))+(((0.106)*x0*x20))+((x6*((((x11*x18))+((x10*x21)))))));
eetrans[1]=(((x6*((((x11*x19))+((x10*x22))))))+(((0.106)*x20*x5))+(((0.106)*x19))+(((-1.0)*x16*x22))+(((-1.1e-7)*x22))+((x17*x19))+((x12*x19))+((x7*((((x11*x22))+(((-1.0)*x10*x19)))))));
IkReal x25=((1.0)*x3);
eetrans[2]=((0.0605)+(((0.106)*x2))+(((1.1e-7)*x3))+(((-0.106)*x23))+((x17*x2))+((x16*x3))+((x7*(((((-1.0)*x10*x2))+(((-1.0)*x11*x25))))))+((x6*(((((-1.0)*x10*x25))+((x11*x2))))))+((x12*x2)));
eerot[0]=(((x5*x9))+((x8*((((x7*((((x18*x4))+((x0*x20))))))+((x6*((((x1*x18))+(((-1.0)*x14*x21)))))))))));
IkReal x26=((1.0)*x19);
eerot[1]=((((-1.0)*x8*((((x13*(((((-1.0)*x1*x26))+((x22*x4))))))+((x15*(((((-1.0)*x14*x26))+(((-1.0)*x20*x5))))))))))+(((-1.0)*x0*x9)));
IkReal x27=((1.0)*x14);
eerot[2]=((-1.0)*x8*((((x13*(((((-1.0)*x20))+(((-1.0)*x27*x3))))))+((x15*((x23+(((-1.0)*x2*x27)))))))));
}

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,j0mul,j1,cj1,sj1,htj1,j1mul,j2,cj2,sj2,htj2,j2mul,j3,cj3,sj3,htj3,j3mul,j5,cj5,sj5,htj5,j5mul,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,_ij5[2], _nj5;

IkReal j100, cj100, sj100;
unsigned char _ij100[2], _nj100;
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; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -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=r01;
new_px=py;
new_r01=((-1.0)*r00);
new_py=((-3.0e-8)+(((-1.0)*px)));
new_r02=r02;
new_pz=((-0.0605)+pz);
r00 = new_r00; r01 = new_r01; r02 = new_r02; px = new_px; py = new_py; pz = new_pz;

pp=((px*px)+(py*py)+(pz*pz));
{
IkReal j0eval[1];
j0eval[0]=((IKabs(px))+(IKabs(py)));
if( IKabs(j0eval[0]) < 0.0000010000000000  )
{
continue; // no branches [j0]

} else
{
{
IkReal j0array[2], cj0array[2], sj0array[2];
bool j0valid[2]={false};
_nj0 = 2;
CheckValue<IkReal> x29 = IKatan2WithCheck(IkReal(((-1.0)*px)),IkReal(((-1.0)*py)),IKFAST_ATAN2_MAGTHRESH);
if(!x29.valid){
continue;
}
IkReal x28=x29.value;
j0array[0]=((-1.0)*x28);
sj0array[0]=IKsin(j0array[0]);
cj0array[0]=IKcos(j0array[0]);
j0array[1]=((3.14159265358979)+(((-1.0)*x28)));
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]=((((4.4e-7)*cj0*py*(px*px)*(r01*r01)))+(((-1.0)*(cj0*cj0)*(py*py*py*py)*(r01*r01)))+(((0.005318656)*pz*(cj0*cj0)*(r01*r01)))+(((1.17010432e-9)*r00*r02*sj0))+(((-8.0)*px*pz*r00*r02*(cj0*cj0)*(py*py)))+(((2.0)*(cj0*cj0)*(py*py)*(pz*pz)*(r01*r01)))+(((-4.4e-7)*cj0*py*(px*px)*(r02*r02)))+(((-8.8e-7)*pz*r00*r02*sj0*(py*py)))+(((-4.0)*cj0*pz*r01*r02*sj0*(px*px*px)))+(((0.039712)*cj0*r00*r01*sj0*(pz*pz)))+(((2.0)*cj0*r00*r01*sj0*(pz*pz*pz*pz)))+(((4.0)*py*r00*r01*(cj0*cj0)*(px*px*px)))+(((8.0)*cj0*px*py*sj0*(pz*pz)*(r02*r02)))+(((-5.51936e-9)*cj0*py*(r01*r01)))+(((-0.140064)*py*pz*r01*r02))+(((-8.0)*py*pz*r01*r02*(px*px)*(sj0*sj0)))+(((1.32e-6)*py*r00*r01*sj0*(px*px)))+(((4.0)*cj0*py*sj0*(px*px*px)*(r00*r00)))+(((1.696)*cj0*px*py*pz*sj0*(r02*r02)))+(((-4.0)*(px*px)*(pz*pz)*(r02*r02)*(sj0*sj0)))+(((-4.0)*(px*px)*(pz*pz)*(r00*r00)))+(((0.848)*px*py*pz*r00*r01*(sj0*sj0)))+(((0.848)*px*py*pz*r00*r01*(cj0*cj0)))+(((-0.424)*r01*r02*(cj0*cj0)*(py*py*py)))+(((-0.005318656)*pz*(r02*r02)))+(((-4.0)*px*r00*r01*(cj0*cj0)*(py*py*py)))+(((-0.039712)*cj0*py*pz*r00*r02*sj0))+(((2.0)*(px*px)*(py*py)*(r00*r00)*(sj0*sj0)))+(((-1.0)*(px*px*px*px)*(r00*r00)*(sj0*sj0)))+(((-8.8e-7)*cj0*py*(px*px)*(r00*r00)))+(((-0.005318656)*py*r01*r02))+(((-1.0)*(cj0*cj0)*(pz*pz*pz*pz)*(r01*r01)))+(((-4.0)*cj0*px*r01*r02*sj0*(pz*pz*pz)))+(((-4.0)*py*r00*r01*(px*px*px)*(sj0*sj0)))+(((4.4e-7)*px*sj0*(pz*pz)*(r02*r02)))+(((0.005318656)*cj0*px*r01*r02*sj0))+(((4.0)*px*py*r00*r01*(cj0*cj0)*(pz*pz)))+(((-0.848)*pz*(px*px)*(r02*r02)*(sj0*sj0)))+(((-0.424)*r00*r02*(px*px*px)*(sj0*sj0)))+(((-0.848)*cj0*px*py*pz*sj0*(r01*r01)))+(((0.848)*cj0*r00*r01*sj0*(pz*pz*pz)))+(((0.0250879999999516)*(px*px)*(r02*r02)))+(((-0.019856)*(cj0*cj0)*(px*px)*(r01*r01)))+(((0.424)*py*r01*r02*(px*px)))+(((-0.424)*(pz*pz*pz)*(r02*r02)))+(((12.0)*cj0*px*pz*r01*r02*sj0*(py*py)))+(((-5.51936e-9)*px*sj0*(r02*r02)))+(((-4.0)*cj0*py*r00*r02*sj0*(pz*pz*pz)))+(((-0.019856)*(cj0*cj0)*(pz*pz)*(r01*r01)))+(((5.51936e-9)*py*r00*r01*sj0))+(((-2.0)*(px*px)*(py*py)*(r02*r02)))+(((-4.0)*cj0*px*py*sj0*(pz*pz)*(r01*r01)))+(((0.424)*py*r01*r02*(cj0*cj0)*(px*px)))+(((-8.0)*px*py*r00*r01*(pz*pz)))+(((2.0)*(cj0*cj0)*(px*px)*(py*py)*(r01*r01)))+(((-0.039712)*cj0*px*py*sj0*(r01*r01)))+(((1.103872e-8)*pz*r00*r02*sj0))+(((-0.000157351935999393)*(r02*r02)))+(((-0.005318656)*px*r00*r02*(sj0*sj0)))+(((0.019856)*(cj0*cj0)*(py*py)*(r01*r01)))+(((5.51936e-9)*cj0*py*(r02*r02)))+(((-0.019856)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-4.0)*cj0*px*sj0*(py*py*py)*(r00*r00)))+(((-2.0)*cj0*r00*r01*sj0*(py*py*py*py)))+(((2.0)*(px*px)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-0.039712)*cj0*px*py*sj0*(r00*r00)))+(((0.039712)*py*pz*r01*r02*(cj0*cj0)))+(((-9.328e-8)*cj0*px*py*r00*r02))+(((1.272)*py*r01*r02*(cj0*cj0)*(pz*pz)))+(((-0.039712)*cj0*px*pz*r01*r02*sj0))+(((4.0)*pz*r01*r02*(py*py*py)))+(((-0.424)*(pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((1.272)*cj0*py*r00*r02*sj0*(px*px)))+(((-0.848)*px*r00*r02*(cj0*cj0)*(py*py)))+(((-1.32e-6)*cj0*px*r00*r01*(py*py)))+(((-4.4e-7)*cj0*py*(pz*pz)*(r02*r02)))+(((-0.848)*pz*(cj0*cj0)*(py*py)*(r02*r02)))+(((-4.0)*py*r01*r02*(pz*pz*pz)))+(((-0.424)*pz*(py*py)*(r00*r00)*(sj0*sj0)))+(((-4.0)*cj0*pz*r00*r02*sj0*(py*py*py)))+(((-1.272)*cj0*py*r00*r02*sj0*(pz*pz)))+(((-4.0)*cj0*py*sj0*(px*px*px)*(r01*r01)))+(((-0.424)*cj0*r01*r02*sj0*(px*px*px)))+(((-1.17010432e-9)*cj0*r01*r02))+(((4.4e-7)*sj0*(px*px*px)*(r02*r02)))+(((4.0)*px*r00*r01*(py*py*py)*(sj0*sj0)))+(((4.0)*py*pz*r01*r02*(px*px)))+(((9.328e-8)*py*pz*r00*r01*sj0))+(((-4.4e-7)*px*sj0*(py*py)*(r00*r00)))+(((-4.4e-7)*cj0*py*(pz*pz)*(r01*r01)))+(((0.424)*px*r00*r02*(py*py)))+(((-4.4e-7)*cj0*px*r00*r01*(pz*pz)))+(((4.4e-7)*px*sj0*(py*py)*(r02*r02)))+(((-9.328e-8)*r00*r02*sj0*(py*py)))+(((9.328e-8)*px*pz*sj0*(r00*r00)))+(((4.0)*cj0*px*sj0*(py*py*py)*(r01*r01)))+(((4.0)*pz*r00*r02*(px*px*px)))+(((0.0250879999999516)*(py*py)*(r02*r02)))+(((-0.424)*pz*(cj0*cj0)*(px*px)*(r01*r01)))+(((4.4e-7)*cj0*r00*r01*(px*px*px)))+(((-1.272)*cj0*px*r01*r02*sj0*(pz*pz)))+(((-0.848)*pz*(px*px)*(r00*r00)))+(((1.272)*px*r00*r02*(pz*pz)*(sj0*sj0)))+(((0.424)*r00*r02*(px*px*px)))+(((5.51936e-9)*px*sj0*(r00*r00)))+(((-4.0)*pz*r01*r02*(cj0*cj0)*(py*py*py)))+(((-4.4e-7)*r00*r01*sj0*(py*py*py)))+(((-0.140064)*px*pz*r00*r02))+(((-4.0)*cj0*px*py*sj0*(pz*pz)*(r00*r00)))+(((-4.0)*pz*r00*r02*(px*px*px)*(sj0*sj0)))+(((-1.696)*px*py*pz*r00*r01))+(((-2.0)*(py*py)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-0.848)*pz*(py*py)*(r01*r01)))+(((-0.848)*cj0*px*py*pz*sj0*(r00*r00)))+(((-9.328e-8)*cj0*py*pz*(r01*r01)))+(((2.0)*(px*px)*(pz*pz)*(r02*r02)))+(((4.4e-7)*sj0*(px*px*px)*(r00*r00)))+(((0.424)*pz*(px*px)*(r02*r02)))+(((-0.0008128512)*cj0*r00*r01*sj0))+(((-1.0)*(py*py*py*py)*(r02*r02)))+(((0.019856)*(px*px)*(r00*r00)*(sj0*sj0)))+(((-2.0)*(cj0*cj0)*(px*px)*(pz*pz)*(r01*r01)))+(((4.0)*px*r00*r02*(pz*pz*pz)*(sj0*sj0)))+(((-0.010637312)*cj0*pz*r00*r01*sj0))+(((-0.070032)*(pz*pz)*(r02*r02)))+(((0.005318656)*pz*(r00*r00)*(sj0*sj0)))+(((-0.424)*(cj0*cj0)*(pz*pz*pz)*(r01*r01)))+(((-2.0)*cj0*r00*r01*sj0*(px*px*px*px)))+(((0.0004064256)*(cj0*cj0)*(r01*r01)))+(((-1.0)*(pz*pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((-1.272)*px*r00*r02*(pz*pz)))+(((4.0)*px*pz*r00*r02*(py*py)*(sj0*sj0)))+(((9.328e-8)*px*pz*sj0*(r02*r02)))+(((-4.4e-7)*cj0*(py*py*py)*(r02*r02)))+(((-8.8e-7)*cj0*px*py*pz*r00*r02))+(((0.039712)*px*py*r00*r01*(sj0*sj0)))+(((-0.424)*cj0*r00*r02*sj0*(py*py*py)))+(((0.424)*pz*(cj0*cj0)*(py*py)*(r01*r01)))+(((4.0)*py*r01*r02*(cj0*cj0)*(pz*pz*pz)))+(((9.328e-8)*px*py*r01*r02*sj0))+(((-4.0)*(cj0*cj0)*(px*px)*(py*py)*(r00*r00)))+(((9.328e-8)*cj0*r01*r02*(px*px)))+(((-4.0)*(py*py)*(pz*pz)*(r01*r01)))+(((-1.0)*(pz*pz*pz*pz)*(r02*r02)))+(((0.424)*pz*(px*px)*(r00*r00)*(sj0*sj0)))+(((12.0)*cj0*r00*r01*sj0*(px*px)*(py*py)))+(((-0.005318656)*px*r00*r02))+(((-4.0)*px*r00*r02*(pz*pz*pz)))+(((0.0004064256)*(r00*r00)*(sj0*sj0)))+(((-9.328e-8)*cj0*px*pz*r00*r01))+(((4.4e-7)*px*sj0*(pz*pz)*(r00*r00)))+(((-0.0449440000000484)*(py*py)*(r01*r01)))+(((0.424)*r01*r02*(py*py*py)))+(((8.8e-7)*cj0*pz*r01*r02*(px*px)))+(((4.0)*px*py*r00*r01*(pz*pz)*(sj0*sj0)))+(((-0.005318656)*py*r01*r02*(cj0*cj0)))+(((-1.272)*py*r01*r02*(pz*pz)))+(((8.8e-7)*px*sj0*(py*py)*(r01*r01)))+(((0.039712)*px*pz*r00*r02*(sj0*sj0)))+(((4.0)*py*pz*r01*r02*(cj0*cj0)*(px*px)))+(((1.272)*cj0*px*r01*r02*sj0*(py*py)))+(((0.424)*px*r00*r02*(py*py)*(sj0*sj0)))+(((4.0)*px*pz*r00*r02*(py*py)))+(((2.0)*(py*py)*(pz*pz)*(r02*r02)))+(((-1.103872e-8)*cj0*pz*r01*r02))+(((0.424)*pz*(py*py)*(r02*r02)))+(((-5.51936e-9)*cj0*px*r00*r01))+(((-4.0)*(px*px)*(py*py)*(r01*r01)*(sj0*sj0)))+(((4.4e-7)*py*r00*r01*sj0*(pz*pz)))+(((-1.0)*(cj0*cj0)*(px*px*px*px)*(r01*r01)))+(((-1.0)*(py*py*py*py)*(r00*r00)*(sj0*sj0)))+(((0.039712)*px*py*r00*r01*(cj0*cj0)))+(((8.8e-7)*px*py*pz*r01*r02*sj0))+(((-9.328e-8)*cj0*py*pz*(r02*r02)))+(((-4.4e-7)*cj0*(py*py*py)*(r01*r01)))+(((0.005318656)*cj0*py*r00*r02*sj0))+(((-0.019856)*(py*py)*(r00*r00)*(sj0*sj0)))+(((-1.0)*(px*px*px*px)*(r02*r02)))+(((-4.0)*(cj0*cj0)*(py*py)*(pz*pz)*(r02*r02)))+(((12.0)*cj0*py*pz*r00*r02*sj0*(px*px)))+(((-0.0449440000000484)*(px*px)*(r00*r00)))+(((-0.848)*py*r01*r02*(px*px)*(sj0*sj0)))+(((-0.0898880000000968)*px*py*r00*r01)));
op[1]=((((1.696)*px*sj0*(py*py)*(r01*r01)))+(((1.696)*cj0*pz*r01*r02*(px*px)))+(((0.179775999999806)*px*py*r01*r02*sj0))+(((-8.8e-7)*r00*r02*(px*px*px)))+(((-8.8e-7)*pz*(px*px)*(r02*r02)))+(((-0.179775999999806)*cj0*px*pz*r00*r01))+(((-0.848)*cj0*py*(pz*pz)*(r01*r01)))+(((-0.848)*cj0*px*r00*r01*(pz*pz)))+(((-0.848)*cj0*(py*py*py)*(r01*r01)))+(((1.8656e-7)*(py*py)*(r00*r00)*(sj0*sj0)))+(((-1.696)*cj0*px*py*pz*r00*r02))+(((-8.8e-7)*px*r00*r02*(py*py)))+(((-0.010637312)*cj0*py*(r01*r01)))+(((-8.8e-7)*py*r01*r02*(cj0*cj0)*(px*px)))+(((-0.848)*px*sj0*(py*py)*(r00*r00)))+(((1.103872e-8)*px*r00*r02*(sj0*sj0)))+(((-2.64e-6)*cj0*py*r00*r02*sj0*(px*px)))+(((2.207744e-8)*cj0*pz*r00*r01*sj0))+(((-2.34020864e-9)*(r00*r00)*(sj0*sj0)))+(((-1.8656e-7)*(cj0*cj0)*(py*py)*(r01*r01)))+(((2.544)*py*r00*r01*sj0*(px*px)))+(((-1.76e-6)*px*py*pz*r00*r01*(cj0*cj0)))+(((0.848)*sj0*(px*px*px)*(r00*r00)))+(((0.00225511014399757)*r00*r02*sj0))+(((-0.179775999999806)*r00*r02*sj0*(py*py)))+(((1.8656e-7)*(cj0*cj0)*(px*px)*(r01*r01)))+(((1.76e-6)*pz*(px*px)*(r02*r02)*(sj0*sj0)))+(((0.010637312)*py*r00*r01*sj0))+(((8.8e-7)*(pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((-3.7312e-7)*py*pz*r01*r02*(cj0*cj0)))+(((-0.179775999999806)*cj0*py*pz*(r01*r01)))+(((-1.696)*pz*r00*r02*sj0*(py*py)))+(((-2.64e-6)*py*r01*r02*(cj0*cj0)*(pz*pz)))+(((1.76e-6)*pz*(px*px)*(r00*r00)))+(((0.179775999999806)*px*pz*sj0*(r02*r02)))+(((1.76e-6)*cj0*px*py*pz*sj0*(r01*r01)))+(((-3.7312e-7)*px*pz*r00*r02*(sj0*sj0)))+(((3.7312e-7)*cj0*px*py*sj0*(r00*r00)))+(((-1.103872e-8)*pz*(cj0*cj0)*(r01*r01)))+(((-3.52e-6)*cj0*px*py*pz*sj0*(r02*r02)))+(((3.7312e-7)*cj0*py*pz*r00*r02*sj0))+(((-1.8656e-7)*(px*px)*(r02*r02)))+(((8.8e-7)*pz*(py*py)*(r00*r00)*(sj0*sj0)))+(((-0.848)*cj0*py*(px*px)*(r02*r02)))+(((0.848)*sj0*(px*px*px)*(r02*r02)))+(((1.8656e-7)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((3.7312e-7)*cj0*px*py*sj0*(r01*r01)))+(((-8.8e-7)*pz*(py*py)*(r02*r02)))+(((8.8e-7)*cj0*r01*r02*sj0*(px*px*px)))+(((-1.76e-6)*cj0*r00*r01*sj0*(pz*pz*pz)))+(((-1.103872e-8)*pz*(r00*r00)*(sj0*sj0)))+(((1.103872e-8)*py*r01*r02))+(((-1.103872e-8)*cj0*px*r01*r02*sj0))+(((2.64e-6)*cj0*px*r01*r02*sj0*(pz*pz)))+(((-3.7312e-7)*cj0*r00*r01*sj0*(pz*pz)))+(((3.7312e-7)*cj0*px*pz*r01*r02*sj0))+(((2.64e-6)*py*r01*r02*(pz*pz)))+(((0.179775999999806)*cj0*r01*r02*(px*px)))+(((0.010637312)*px*sj0*(r00*r00)))+(((-2.34020864e-9)*(cj0*cj0)*(r01*r01)))+(((1.76e-6)*pz*(cj0*cj0)*(py*py)*(r02*r02)))+(((8.8e-7)*(cj0*cj0)*(pz*pz*pz)*(r01*r01)))+(((-0.00225511014399757)*cj0*r01*r02))+(((2.64e-6)*px*r00*r02*(pz*pz)))+(((1.76e-6)*pz*(py*py)*(r01*r01)))+(((0.848)*py*r00*r01*sj0*(pz*pz)))+(((2.64e-6)*cj0*py*r00*r02*sj0*(pz*pz)))+(((0.848)*px*sj0*(py*py)*(r02*r02)))+(((3.52e-6)*px*py*pz*r00*r01))+(((8.8e-7)*(pz*pz*pz)*(r02*r02)))+(((0.021274624)*pz*r00*r02*sj0))+(((-0.179775999999806)*cj0*px*py*r00*r02))+(((1.8656e-7)*(cj0*cj0)*(pz*pz)*(r01*r01)))+(((-2.64e-6)*px*r00*r02*(pz*pz)*(sj0*sj0)))+(((-0.848)*cj0*py*(pz*pz)*(r02*r02)))+(((-0.848)*cj0*(py*py*py)*(r02*r02)))+(((0.179775999999806)*px*pz*sj0*(r00*r00)))+(((1.8656e-7)*(pz*pz)*(r02*r02)))+(((1.696)*px*py*pz*r01*r02*sj0))+(((-1.696)*cj0*py*(px*px)*(r00*r00)))+(((-8.8e-7)*px*r00*r02*(py*py)*(sj0*sj0)))+(((-1.103872e-8)*cj0*py*r00*r02*sj0))+(((0.848)*cj0*py*(px*px)*(r01*r01)))+(((-1.76e-6)*px*py*pz*r00*r01*(sj0*sj0)))+(((1.103872e-8)*px*r00*r02))+(((-8.8e-7)*pz*(px*px)*(r00*r00)*(sj0*sj0)))+(((4.68041728e-9)*cj0*r00*r01*sj0))+(((-0.021274624)*cj0*pz*r01*r02))+(((2.34020864e-9)*(r02*r02)))+(((8.8e-7)*r00*r02*(px*px*px)*(sj0*sj0)))+(((-0.848)*r00*r01*sj0*(py*py*py)))+(((-8.8e-7)*py*r01*r02*(px*px)))+(((-2.64e-6)*cj0*px*r01*r02*sj0*(py*py)))+(((-3.7312e-7)*px*py*r00*r01*(cj0*cj0)))+(((3.7312e-7)*px*pz*r00*r02))+(((-1.8656e-7)*(py*py)*(r02*r02)))+(((1.76e-6)*cj0*px*py*pz*sj0*(r00*r00)))+(((-8.8e-7)*r01*r02*(py*py*py)))+(((-0.010637312)*cj0*px*r00*r01))+(((0.179775999999806)*py*pz*r00*r01*sj0))+(((8.8e-7)*cj0*r00*r02*sj0*(py*py*py)))+(((0.848)*px*sj0*(pz*pz)*(r00*r00)))+(((1.76e-6)*py*r01*r02*(px*px)*(sj0*sj0)))+(((0.848)*px*sj0*(pz*pz)*(r02*r02)))+(((8.8e-7)*pz*(cj0*cj0)*(px*px)*(r01*r01)))+(((1.103872e-8)*pz*(r02*r02)))+(((1.103872e-8)*py*r01*r02*(cj0*cj0)))+(((3.7312e-7)*py*pz*r01*r02))+(((0.010637312)*cj0*py*(r02*r02)))+(((-0.179775999999806)*cj0*py*pz*(r02*r02)))+(((-0.010637312)*px*sj0*(r02*r02)))+(((-3.7312e-7)*px*py*r00*r01*(sj0*sj0)))+(((-1.8656e-7)*(px*px)*(r00*r00)*(sj0*sj0)))+(((-2.544)*cj0*px*r00*r01*(py*py)))+(((0.848)*cj0*r00*r01*(px*px*px)))+(((-8.8e-7)*pz*(cj0*cj0)*(py*py)*(r01*r01)))+(((1.76e-6)*px*r00*r02*(cj0*cj0)*(py*py)))+(((8.8e-7)*r01*r02*(cj0*cj0)*(py*py*py))));
op[2]=((((-8.0)*cj0*px*py*sj0*(pz*pz)*(r01*r01)))+(((0.0794239999996128)*px*pz*r00*r02))+(((7.02062592e-9)*cj0*r01*r02))+(((-2.0)*(px*px*px*px)*(r00*r00)*(sj0*sj0)))+(((-8.0)*px*r00*r02*(pz*pz*pz)))+(((-2.0)*(pz*pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((-2.0)*(cj0*cj0)*(pz*pz*pz*pz)*(r01*r01)))+(((-8.0)*cj0*pz*r01*r02*sj0*(px*px*px)))+(((8.0)*px*py*r00*r01*(pz*pz)*(sj0*sj0)))+(((0.00288451788799514)*cj0*r00*r01*sj0))+(((-2.0)*(cj0*cj0)*(px*px*px*px)*(r01*r01)))+(((-2.0)*(px*px*px*px)*(r02*r02)))+(((-0.140063999999806)*(px*px)*(r00*r00)*(sj0*sj0)))+(((0.280127999999613)*cj0*py*pz*r00*r02*sj0))+(((0.00194040627199879)*(r02*r02)))+(((5.5968e-7)*cj0*px*py*r00*r02))+(((4.0)*(px*px)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((8.0)*px*r00*r02*(pz*pz*pz)*(sj0*sj0)))+(((4.0)*(px*px)*(pz*pz)*(r02*r02)))+(((-2.0)*(py*py*py*py)*(r00*r00)*(sj0*sj0)))+(((24.0)*cj0*px*pz*r01*r02*sj0*(py*py)))+(((-0.00144225894399757)*(r00*r00)*(sj0*sj0)))+(((0.140063999999806)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-8.0)*(px*px)*(pz*pz)*(r00*r00)))+(((-8.0)*(px*px)*(pz*pz)*(r02*r02)*(sj0*sj0)))+(((8.0)*pz*r01*r02*(py*py*py)))+(((0.280127999999613)*cj0*px*py*sj0*(r00*r00)))+(((-4.0)*cj0*r00*r01*sj0*(py*py*py*py)))+(((-0.140063999999806)*(cj0*cj0)*(py*py)*(r01*r01)))+(((-8.0)*py*r01*r02*(pz*pz*pz)))+(((4.0)*(cj0*cj0)*(px*px)*(py*py)*(r01*r01)))+(((-2.0)*(pz*pz*pz*pz)*(r02*r02)))+(((-8.0)*px*r00*r01*(cj0*cj0)*(py*py*py)))+(((-8.0)*(cj0*cj0)*(py*py)*(pz*pz)*(r02*r02)))+(((8.0)*px*pz*r00*r02*(py*py)*(sj0*sj0)))+(((24.0)*cj0*py*pz*r00*r02*sj0*(px*px)))+(((-5.5968e-7)*px*pz*sj0*(r02*r02)))+(((-8.0)*cj0*pz*r00*r02*sj0*(py*py*py)))+(((-5.5968e-7)*py*pz*r00*r01*sj0))+(((-8.0)*pz*r00*r02*(px*px*px)*(sj0*sj0)))+(((8.0)*py*r00*r01*(cj0*cj0)*(px*px*px)))+(((-0.129599999999903)*(py*py)*(r02*r02)))+(((8.0)*px*r00*r01*(py*py*py)*(sj0*sj0)))+(((-4.0)*(px*px)*(py*py)*(r02*r02)))+(((-16.0)*px*pz*r00*r02*(cj0*cj0)*(py*py)))+(((8.0)*cj0*px*sj0*(py*py*py)*(r01*r01)))+(((-0.129599999999903)*(px*px)*(r02*r02)))+(((-8.0)*(cj0*cj0)*(px*px)*(py*py)*(r00*r00)))+(((0.280127999999613)*cj0*px*pz*r01*r02*sj0))+(((0.140063999999806)*(cj0*cj0)*(pz*pz)*(r01*r01)))+(((4.0)*cj0*r00*r01*sj0*(pz*pz*pz*pz)))+(((-5.5968e-7)*px*pz*sj0*(r00*r00)))+(((8.0)*px*py*r00*r01*(cj0*cj0)*(pz*pz)))+(((0.0794239999996128)*py*pz*r01*r02))+(((-0.280127999999613)*px*py*r00*r01*(sj0*sj0)))+(((-4.0)*(cj0*cj0)*(px*px)*(pz*pz)*(r01*r01)))+(((-0.280127999999613)*px*pz*r00*r02*(sj0*sj0)))+(((24.0)*cj0*r00*r01*sj0*(px*px)*(py*py)))+(((4.0)*(cj0*cj0)*(py*py)*(pz*pz)*(r01*r01)))+(((8.0)*cj0*py*sj0*(px*px*px)*(r00*r00)))+(((0.140063999999806)*(cj0*cj0)*(px*px)*(r01*r01)))+(((-0.280127999999613)*px*py*r00*r01*(cj0*cj0)))+(((-16.0)*py*pz*r01*r02*(px*px)*(sj0*sj0)))+(((8.0)*py*pz*r01*r02*(cj0*cj0)*(px*px)))+(((8.0)*py*r01*r02*(cj0*cj0)*(pz*pz*pz)))+(((0.280127999999613)*cj0*px*py*sj0*(r01*r01)))+(((-8.0)*pz*r01*r02*(cj0*cj0)*(py*py*py)))+(((16.0)*cj0*px*py*sj0*(pz*pz)*(r02*r02)))+(((-8.0)*py*r00*r01*(px*px*px)*(sj0*sj0)))+(((0.0397119999998064)*(pz*pz)*(r02*r02)))+(((0.140063999999806)*(py*py)*(r00*r00)*(sj0*sj0)))+(((8.0)*py*pz*r01*r02*(px*px)))+(((-16.0)*px*py*r00*r01*(pz*pz)))+(((5.5968e-7)*cj0*py*pz*(r01*r01)))+(((4.0)*(px*px)*(py*py)*(r00*r00)*(sj0*sj0)))+(((-8.0)*cj0*px*py*sj0*(pz*pz)*(r00*r00)))+(((-0.280127999999613)*cj0*r00*r01*sj0*(pz*pz)))+(((-2.0)*(py*py*py*py)*(r02*r02)))+(((-5.5968e-7)*cj0*r01*r02*(px*px)))+(((-2.0)*(cj0*cj0)*(py*py*py*py)*(r01*r01)))+(((-8.0)*(px*px)*(py*py)*(r01*r01)*(sj0*sj0)))+(((5.5968e-7)*cj0*py*pz*(r02*r02)))+(((-8.0)*cj0*py*r00*r02*sj0*(pz*pz*pz)))+(((8.0)*pz*r00*r02*(px*px*px)))+(((-5.5968e-7)*px*py*r01*r02*sj0))+(((-4.0)*cj0*r00*r01*sj0*(px*px*px*px)))+(((8.0)*px*pz*r00*r02*(py*py)))+(((-0.0898880000000968)*(py*py)*(r01*r01)))+(((-8.0)*cj0*px*r01*r02*sj0*(pz*pz*pz)))+(((5.5968e-7)*cj0*px*pz*r00*r01))+(((-8.0)*(py*py)*(pz*pz)*(r01*r01)))+(((4.0)*(py*py)*(pz*pz)*(r02*r02)))+(((-0.179776000000194)*px*py*r00*r01))+(((-0.00144225894399757)*(cj0*cj0)*(r01*r01)))+(((-7.02062592e-9)*r00*r02*sj0))+(((-0.280127999999613)*py*pz*r01*r02*(cj0*cj0)))+(((-8.0)*cj0*px*sj0*(py*py*py)*(r00*r00)))+(((-8.0)*cj0*py*sj0*(px*px*px)*(r01*r01)))+(((5.5968e-7)*r00*r02*sj0*(py*py)))+(((-4.0)*(py*py)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-0.0898880000000968)*(px*px)*(r00*r00))));
op[3]=((((1.696)*px*sj0*(py*py)*(r01*r01)))+(((1.696)*cj0*pz*r01*r02*(px*px)))+(((-3.7312e-7)*px*pz*r00*r02))+(((-2.34020864e-9)*(r02*r02)))+(((0.179775999999806)*r00*r02*sj0*(py*py)))+(((-3.7312e-7)*cj0*px*py*sj0*(r01*r01)))+(((-8.8e-7)*r00*r02*(px*px*px)))+(((-8.8e-7)*pz*(px*px)*(r02*r02)))+(((-0.179775999999806)*px*py*r01*r02*sj0))+(((-0.848)*cj0*py*(pz*pz)*(r01*r01)))+(((2.34020864e-9)*(r00*r00)*(sj0*sj0)))+(((-0.848)*cj0*px*r00*r01*(pz*pz)))+(((-3.7312e-7)*cj0*px*pz*r01*r02*sj0))+(((-0.848)*cj0*(py*py*py)*(r01*r01)))+(((-1.696)*cj0*px*py*pz*r00*r02))+(((-8.8e-7)*px*r00*r02*(py*py)))+(((-0.010637312)*cj0*py*(r01*r01)))+(((-8.8e-7)*py*r01*r02*(cj0*cj0)*(px*px)))+(((-0.848)*px*sj0*(py*py)*(r00*r00)))+(((1.103872e-8)*px*r00*r02*(sj0*sj0)))+(((-2.64e-6)*cj0*py*r00*r02*sj0*(px*px)))+(((3.7312e-7)*px*py*r00*r01*(cj0*cj0)))+(((-1.8656e-7)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((2.207744e-8)*cj0*pz*r00*r01*sj0))+(((1.8656e-7)*(px*px)*(r00*r00)*(sj0*sj0)))+(((2.544)*py*r00*r01*sj0*(px*px)))+(((-4.68041728e-9)*cj0*r00*r01*sj0))+(((-0.00225511014399757)*r00*r02*sj0))+(((-1.76e-6)*px*py*pz*r00*r01*(cj0*cj0)))+(((0.848)*sj0*(px*px*px)*(r00*r00)))+(((3.7312e-7)*px*pz*r00*r02*(sj0*sj0)))+(((0.179775999999806)*cj0*py*pz*(r02*r02)))+(((1.76e-6)*pz*(px*px)*(r02*r02)*(sj0*sj0)))+(((0.010637312)*py*r00*r01*sj0))+(((8.8e-7)*(pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((2.34020864e-9)*(cj0*cj0)*(r01*r01)))+(((-1.8656e-7)*(cj0*cj0)*(px*px)*(r01*r01)))+(((-1.696)*pz*r00*r02*sj0*(py*py)))+(((-2.64e-6)*py*r01*r02*(cj0*cj0)*(pz*pz)))+(((3.7312e-7)*cj0*r00*r01*sj0*(pz*pz)))+(((-0.179775999999806)*py*pz*r00*r01*sj0))+(((1.76e-6)*pz*(px*px)*(r00*r00)))+(((1.76e-6)*cj0*px*py*pz*sj0*(r01*r01)))+(((-1.103872e-8)*pz*(cj0*cj0)*(r01*r01)))+(((-3.52e-6)*cj0*px*py*pz*sj0*(r02*r02)))+(((8.8e-7)*pz*(py*py)*(r00*r00)*(sj0*sj0)))+(((-0.848)*cj0*py*(px*px)*(r02*r02)))+(((0.179775999999806)*cj0*px*pz*r00*r01))+(((0.848)*sj0*(px*px*px)*(r02*r02)))+(((1.8656e-7)*(py*py)*(r02*r02)))+(((-0.179775999999806)*cj0*r01*r02*(px*px)))+(((-8.8e-7)*pz*(py*py)*(r02*r02)))+(((8.8e-7)*cj0*r01*r02*sj0*(px*px*px)))+(((-1.76e-6)*cj0*r00*r01*sj0*(pz*pz*pz)))+(((-1.8656e-7)*(py*py)*(r00*r00)*(sj0*sj0)))+(((-1.103872e-8)*pz*(r00*r00)*(sj0*sj0)))+(((1.103872e-8)*py*r01*r02))+(((-1.103872e-8)*cj0*px*r01*r02*sj0))+(((2.64e-6)*cj0*px*r01*r02*sj0*(pz*pz)))+(((2.64e-6)*py*r01*r02*(pz*pz)))+(((0.010637312)*px*sj0*(r00*r00)))+(((1.76e-6)*pz*(cj0*cj0)*(py*py)*(r02*r02)))+(((-1.8656e-7)*(cj0*cj0)*(pz*pz)*(r01*r01)))+(((8.8e-7)*(cj0*cj0)*(pz*pz*pz)*(r01*r01)))+(((2.64e-6)*px*r00*r02*(pz*pz)))+(((1.76e-6)*pz*(py*py)*(r01*r01)))+(((0.848)*py*r00*r01*sj0*(pz*pz)))+(((-3.7312e-7)*cj0*px*py*sj0*(r00*r00)))+(((-0.179775999999806)*px*pz*sj0*(r02*r02)))+(((2.64e-6)*cj0*py*r00*r02*sj0*(pz*pz)))+(((0.848)*px*sj0*(py*py)*(r02*r02)))+(((3.52e-6)*px*py*pz*r00*r01))+(((8.8e-7)*(pz*pz*pz)*(r02*r02)))+(((0.021274624)*pz*r00*r02*sj0))+(((1.8656e-7)*(cj0*cj0)*(py*py)*(r01*r01)))+(((-2.64e-6)*px*r00*r02*(pz*pz)*(sj0*sj0)))+(((-0.848)*cj0*py*(pz*pz)*(r02*r02)))+(((-0.848)*cj0*(py*py*py)*(r02*r02)))+(((1.696)*px*py*pz*r01*r02*sj0))+(((-1.696)*cj0*py*(px*px)*(r00*r00)))+(((3.7312e-7)*px*py*r00*r01*(sj0*sj0)))+(((-8.8e-7)*px*r00*r02*(py*py)*(sj0*sj0)))+(((-1.103872e-8)*cj0*py*r00*r02*sj0))+(((-3.7312e-7)*cj0*py*pz*r00*r02*sj0))+(((0.848)*cj0*py*(px*px)*(r01*r01)))+(((-1.76e-6)*px*py*pz*r00*r01*(sj0*sj0)))+(((1.103872e-8)*px*r00*r02))+(((1.8656e-7)*(px*px)*(r02*r02)))+(((-8.8e-7)*pz*(px*px)*(r00*r00)*(sj0*sj0)))+(((-0.021274624)*cj0*pz*r01*r02))+(((8.8e-7)*r00*r02*(px*px*px)*(sj0*sj0)))+(((-0.848)*r00*r01*sj0*(py*py*py)))+(((-8.8e-7)*py*r01*r02*(px*px)))+(((-1.8656e-7)*(pz*pz)*(r02*r02)))+(((-2.64e-6)*cj0*px*r01*r02*sj0*(py*py)))+(((-0.179775999999806)*px*pz*sj0*(r00*r00)))+(((0.179775999999806)*cj0*py*pz*(r01*r01)))+(((1.76e-6)*cj0*px*py*pz*sj0*(r00*r00)))+(((-8.8e-7)*r01*r02*(py*py*py)))+(((-0.010637312)*cj0*px*r00*r01))+(((8.8e-7)*cj0*r00*r02*sj0*(py*py*py)))+(((3.7312e-7)*py*pz*r01*r02*(cj0*cj0)))+(((0.00225511014399757)*cj0*r01*r02))+(((0.848)*px*sj0*(pz*pz)*(r00*r00)))+(((0.179775999999806)*cj0*px*py*r00*r02))+(((1.76e-6)*py*r01*r02*(px*px)*(sj0*sj0)))+(((0.848)*px*sj0*(pz*pz)*(r02*r02)))+(((8.8e-7)*pz*(cj0*cj0)*(px*px)*(r01*r01)))+(((-3.7312e-7)*py*pz*r01*r02))+(((1.103872e-8)*pz*(r02*r02)))+(((1.103872e-8)*py*r01*r02*(cj0*cj0)))+(((0.010637312)*cj0*py*(r02*r02)))+(((-0.010637312)*px*sj0*(r02*r02)))+(((-2.544)*cj0*px*r00*r01*(py*py)))+(((0.848)*cj0*r00*r01*(px*px*px)))+(((-8.8e-7)*pz*(cj0*cj0)*(py*py)*(r01*r01)))+(((1.76e-6)*px*r00*r02*(cj0*cj0)*(py*py)))+(((8.8e-7)*r01*r02*(cj0*cj0)*(py*py*py))));
op[4]=((((1.32e-6)*cj0*px*r00*r01*(py*py)))+(((-1.0)*(cj0*cj0)*(py*py*py*py)*(r01*r01)))+(((4.4e-7)*cj0*px*r00*r01*(pz*pz)))+(((8.8e-7)*cj0*py*(px*px)*(r00*r00)))+(((1.17010432e-9)*r00*r02*sj0))+(((-8.0)*px*pz*r00*r02*(cj0*cj0)*(py*py)))+(((2.0)*(cj0*cj0)*(py*py)*(pz*pz)*(r01*r01)))+(((-4.0)*cj0*pz*r01*r02*sj0*(px*px*px)))+(((0.039712)*cj0*r00*r01*sj0*(pz*pz)))+(((2.0)*cj0*r00*r01*sj0*(pz*pz*pz*pz)))+(((4.0)*py*r00*r01*(cj0*cj0)*(px*px*px)))+(((8.0)*cj0*px*py*sj0*(pz*pz)*(r02*r02)))+(((-0.140064)*py*pz*r01*r02))+(((-8.0)*py*pz*r01*r02*(px*px)*(sj0*sj0)))+(((4.0)*cj0*py*sj0*(px*px*px)*(r00*r00)))+(((-1.272)*cj0*px*r01*r02*sj0*(py*py)))+(((-4.0)*(px*px)*(pz*pz)*(r02*r02)*(sj0*sj0)))+(((-4.0)*(px*px)*(pz*pz)*(r00*r00)))+(((-0.848)*px*py*pz*r00*r01*(sj0*sj0)))+(((-4.4e-7)*cj0*py*(px*px)*(r01*r01)))+(((-4.4e-7)*py*r00*r01*sj0*(pz*pz)))+(((-4.0)*px*r00*r01*(cj0*cj0)*(py*py*py)))+(((0.424)*cj0*r01*r02*sj0*(px*px*px)))+(((-0.424)*py*r01*r02*(cj0*cj0)*(px*px)))+(((-0.039712)*cj0*py*pz*r00*r02*sj0))+(((2.0)*(px*px)*(py*py)*(r00*r00)*(sj0*sj0)))+(((-1.0)*(px*px*px*px)*(r00*r00)*(sj0*sj0)))+(((1.272)*cj0*px*r01*r02*sj0*(pz*pz)))+(((-1.0)*(cj0*cj0)*(pz*pz*pz*pz)*(r01*r01)))+(((0.848)*py*r01*r02*(px*px)*(sj0*sj0)))+(((-4.0)*cj0*px*r01*r02*sj0*(pz*pz*pz)))+(((-1.272)*px*r00*r02*(pz*pz)*(sj0*sj0)))+(((-4.0)*py*r00*r01*(px*px*px)*(sj0*sj0)))+(((-1.32e-6)*py*r00*r01*sj0*(px*px)))+(((4.0)*px*py*r00*r01*(cj0*cj0)*(pz*pz)))+(((0.0250879999999516)*(px*px)*(r02*r02)))+(((-0.019856)*(cj0*cj0)*(px*px)*(r01*r01)))+(((4.4e-7)*cj0*py*(pz*pz)*(r02*r02)))+(((12.0)*cj0*px*pz*r01*r02*sj0*(py*py)))+(((-4.0)*cj0*py*r00*r02*sj0*(pz*pz*pz)))+(((-0.019856)*(cj0*cj0)*(pz*pz)*(r01*r01)))+(((0.424)*cj0*r00*r02*sj0*(py*py*py)))+(((-4.4e-7)*px*sj0*(pz*pz)*(r02*r02)))+(((-2.0)*(px*px)*(py*py)*(r02*r02)))+(((-4.0)*cj0*px*py*sj0*(pz*pz)*(r01*r01)))+(((-8.0)*px*py*r00*r01*(pz*pz)))+(((0.848)*cj0*px*py*pz*sj0*(r01*r01)))+(((2.0)*(cj0*cj0)*(px*px)*(py*py)*(r01*r01)))+(((-0.039712)*cj0*px*py*sj0*(r01*r01)))+(((-0.424)*py*r01*r02*(px*px)))+(((0.424)*(pz*pz*pz)*(r02*r02)))+(((-5.51936e-9)*cj0*py*(r02*r02)))+(((0.424)*(pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((-0.000157351935999393)*(r02*r02)))+(((0.019856)*(cj0*cj0)*(py*py)*(r01*r01)))+(((0.424)*(cj0*cj0)*(pz*pz*pz)*(r01*r01)))+(((-0.019856)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((0.424)*r00*r02*(px*px*px)*(sj0*sj0)))+(((-0.005318656)*pz*(r00*r00)*(sj0*sj0)))+(((-4.0)*cj0*px*sj0*(py*py*py)*(r00*r00)))+(((-2.0)*cj0*r00*r01*sj0*(py*py*py*py)))+(((2.0)*(px*px)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-0.039712)*cj0*px*py*sj0*(r00*r00)))+(((0.039712)*py*pz*r01*r02*(cj0*cj0)))+(((-9.328e-8)*cj0*px*py*r00*r02))+(((-1.272)*cj0*py*r00*r02*sj0*(px*px)))+(((4.4e-7)*px*sj0*(py*py)*(r00*r00)))+(((-0.039712)*cj0*px*pz*r01*r02*sj0))+(((4.0)*pz*r01*r02*(py*py*py)))+(((-0.005318656)*cj0*px*r01*r02*sj0))+(((0.010637312)*cj0*pz*r00*r01*sj0))+(((0.848)*cj0*px*py*pz*sj0*(r00*r00)))+(((5.51936e-9)*px*sj0*(r02*r02)))+(((-0.848)*px*py*pz*r00*r01*(cj0*cj0)))+(((1.103872e-8)*cj0*pz*r01*r02))+(((-0.424)*pz*(px*px)*(r02*r02)))+(((0.424)*pz*(cj0*cj0)*(px*px)*(r01*r01)))+(((-4.4e-7)*cj0*r00*r01*(px*px*px)))+(((-4.0)*py*r01*r02*(pz*pz*pz)))+(((-4.0)*cj0*pz*r00*r02*sj0*(py*py*py)))+(((-0.424)*px*r00*r02*(py*py)))+(((-4.0)*cj0*py*sj0*(px*px*px)*(r01*r01)))+(((-1.17010432e-9)*cj0*r01*r02))+(((4.0)*px*r00*r01*(py*py*py)*(sj0*sj0)))+(((4.0)*py*pz*r01*r02*(px*px)))+(((9.328e-8)*py*pz*r00*r01*sj0))+(((1.272)*py*r01*r02*(pz*pz)))+(((5.51936e-9)*cj0*py*(r01*r01)))+(((4.4e-7)*cj0*py*(pz*pz)*(r01*r01)))+(((-9.328e-8)*r00*r02*sj0*(py*py)))+(((0.005318656)*px*r00*r02*(sj0*sj0)))+(((-0.424)*r01*r02*(py*py*py)))+(((9.328e-8)*px*pz*sj0*(r00*r00)))+(((4.0)*cj0*px*sj0*(py*py*py)*(r01*r01)))+(((4.0)*pz*r00*r02*(px*px*px)))+(((0.005318656)*px*r00*r02))+(((4.4e-7)*cj0*(py*py*py)*(r02*r02)))+(((0.0250879999999516)*(py*py)*(r02*r02)))+(((0.424)*pz*(py*py)*(r00*r00)*(sj0*sj0)))+(((1.272)*cj0*py*r00*r02*sj0*(pz*pz)))+(((-1.103872e-8)*pz*r00*r02*sj0))+(((-4.0)*pz*r01*r02*(cj0*cj0)*(py*py*py)))+(((0.005318656)*pz*(r02*r02)))+(((-0.005318656)*pz*(cj0*cj0)*(r01*r01)))+(((-0.140064)*px*pz*r00*r02))+(((0.005318656)*py*r01*r02))+(((-4.0)*cj0*px*py*sj0*(pz*pz)*(r00*r00)))+(((-4.0)*pz*r00*r02*(px*px*px)*(sj0*sj0)))+(((-2.0)*(py*py)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-0.424)*px*r00*r02*(py*py)*(sj0*sj0)))+(((-9.328e-8)*cj0*py*pz*(r01*r01)))+(((2.0)*(px*px)*(pz*pz)*(r02*r02)))+(((-0.848)*cj0*r00*r01*sj0*(pz*pz*pz)))+(((-0.0008128512)*cj0*r00*r01*sj0))+(((-1.0)*(py*py*py*py)*(r02*r02)))+(((-0.005318656)*cj0*py*r00*r02*sj0))+(((0.019856)*(px*px)*(r00*r00)*(sj0*sj0)))+(((-2.0)*(cj0*cj0)*(px*px)*(pz*pz)*(r01*r01)))+(((4.0)*px*r00*r02*(pz*pz*pz)*(sj0*sj0)))+(((-4.4e-7)*px*sj0*(py*py)*(r02*r02)))+(((-1.696)*cj0*px*py*pz*sj0*(r02*r02)))+(((-0.070032)*(pz*pz)*(r02*r02)))+(((-8.8e-7)*px*sj0*(py*py)*(r01*r01)))+(((8.8e-7)*cj0*px*py*pz*r00*r02))+(((-2.0)*cj0*r00*r01*sj0*(px*px*px*px)))+(((0.0004064256)*(cj0*cj0)*(r01*r01)))+(((-0.424)*r00*r02*(px*px*px)))+(((-1.0)*(pz*pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((4.0)*px*pz*r00*r02*(py*py)*(sj0*sj0)))+(((-0.424)*pz*(px*px)*(r00*r00)*(sj0*sj0)))+(((9.328e-8)*px*pz*sj0*(r02*r02)))+(((0.848)*pz*(py*py)*(r01*r01)))+(((-8.8e-7)*cj0*pz*r01*r02*(px*px)))+(((0.039712)*px*py*r00*r01*(sj0*sj0)))+(((0.424)*r01*r02*(cj0*cj0)*(py*py*py)))+(((-4.4e-7)*sj0*(px*px*px)*(r02*r02)))+(((4.0)*py*r01*r02*(cj0*cj0)*(pz*pz*pz)))+(((9.328e-8)*px*py*r01*r02*sj0))+(((-4.0)*(cj0*cj0)*(px*px)*(py*py)*(r00*r00)))+(((9.328e-8)*cj0*r01*r02*(px*px)))+(((-4.0)*(py*py)*(pz*pz)*(r01*r01)))+(((-1.0)*(pz*pz*pz*pz)*(r02*r02)))+(((-4.4e-7)*sj0*(px*px*px)*(r00*r00)))+(((12.0)*cj0*r00*r01*sj0*(px*px)*(py*py)))+(((-4.0)*px*r00*r02*(pz*pz*pz)))+(((0.0004064256)*(r00*r00)*(sj0*sj0)))+(((-9.328e-8)*cj0*px*pz*r00*r01))+(((0.848)*px*r00*r02*(cj0*cj0)*(py*py)))+(((-5.51936e-9)*py*r00*r01*sj0))+(((-5.51936e-9)*px*sj0*(r00*r00)))+(((-4.4e-7)*px*sj0*(pz*pz)*(r00*r00)))+(((-0.0449440000000484)*(py*py)*(r01*r01)))+(((-0.424)*pz*(cj0*cj0)*(py*py)*(r01*r01)))+(((0.848)*pz*(px*px)*(r02*r02)*(sj0*sj0)))+(((8.8e-7)*pz*r00*r02*sj0*(py*py)))+(((4.0)*px*py*r00*r01*(pz*pz)*(sj0*sj0)))+(((-8.8e-7)*px*py*pz*r01*r02*sj0))+(((0.039712)*px*pz*r00*r02*(sj0*sj0)))+(((4.0)*py*pz*r01*r02*(cj0*cj0)*(px*px)))+(((4.0)*px*pz*r00*r02*(py*py)))+(((2.0)*(py*py)*(pz*pz)*(r02*r02)))+(((0.848)*pz*(px*px)*(r00*r00)))+(((1.696)*px*py*pz*r00*r01))+(((0.005318656)*py*r01*r02*(cj0*cj0)))+(((0.848)*pz*(cj0*cj0)*(py*py)*(r02*r02)))+(((1.272)*px*r00*r02*(pz*pz)))+(((-1.272)*py*r01*r02*(cj0*cj0)*(pz*pz)))+(((-4.0)*(px*px)*(py*py)*(r01*r01)*(sj0*sj0)))+(((-1.0)*(cj0*cj0)*(px*px*px*px)*(r01*r01)))+(((-1.0)*(py*py*py*py)*(r00*r00)*(sj0*sj0)))+(((4.4e-7)*cj0*py*(px*px)*(r02*r02)))+(((0.039712)*px*py*r00*r01*(cj0*cj0)))+(((-9.328e-8)*cj0*py*pz*(r02*r02)))+(((-0.424)*pz*(py*py)*(r02*r02)))+(((-0.019856)*(py*py)*(r00*r00)*(sj0*sj0)))+(((5.51936e-9)*cj0*px*r00*r01))+(((-1.0)*(px*px*px*px)*(r02*r02)))+(((-4.0)*(cj0*cj0)*(py*py)*(pz*pz)*(r02*r02)))+(((12.0)*cj0*py*pz*r00*r02*sj0*(px*px)))+(((4.4e-7)*cj0*(py*py*py)*(r01*r01)))+(((4.4e-7)*r00*r01*sj0*(py*py*py)))+(((-0.0449440000000484)*(px*px)*(r00*r00)))+(((-0.0898880000000968)*px*py*r00*r01)));
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]=((2.0)*(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 j5array[2], cj5array[2], sj5array[2];
bool j5valid[2]={false};
_nj5 = 2;
sj5array[0]=((((-1.0)*r01*sj0))+(((-1.0)*cj0*r00)));
if( sj5array[0] >= -1-IKFAST_SINCOS_THRESH && sj5array[0] <= 1+IKFAST_SINCOS_THRESH )
{
    j5valid[0] = j5valid[1] = true;
    j5array[0] = IKasin(sj5array[0]);
    cj5array[0] = IKcos(j5array[0]);
    sj5array[1] = sj5array[0];
    j5array[1] = j5array[0] > 0 ? (IKPI-j5array[0]) : (-IKPI-j5array[0]);
    cj5array[1] = -cj5array[0];
}
else if( isnan(sj5array[0]) )
{
    // probably any value will work
    j5valid[0] = true;
    cj5array[0] = 1; sj5array[0] = 0; j5array[0] = 0;
}
for(int ij5 = 0; ij5 < 2; ++ij5)
{
if( !j5valid[ij5] )
{
    continue;
}
_ij5[0] = ij5; _ij5[1] = -1;
for(int iij5 = ij5+1; iij5 < 2; ++iij5)
{
if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
{
    j5valid[iij5]=false; _ij5[1] = iij5; break; 
}
}
j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];

{
IkReal j3eval[2];
j3eval[0]=cj5;
j3eval[1]=IKsign(cj5);
if( IKabs(j3eval[0]) < 0.0000010000000000  || IKabs(j3eval[1]) < 0.0000010000000000  )
{
{
IkReal evalcond[1];
bool bgotonextstatement = true;
do
{
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
if( IKabs(evalcond[0]) < 0.0000050000000000  )
{
bgotonextstatement=false;
{
IkReal j3array[2], cj3array[2], sj3array[2];
bool j3valid[2]={false};
_nj3 = 2;
IkReal x30=((8.92857142856662)*sj1);
IkReal x31=(px*sj0);
IkReal x32=(cj0*py);
IkReal x33=((9.26549865228612e-6)*cj1);
if( (((-0.528301886792168)+(((42.1159029649369)*(py*py)))+(((-8.92857142856662)*cj1*pz))+(((42.1159029649369)*(px*px)))+(((-9.26549865228612e-6)*pz*sj1))+(((-1.0)*x30*x31))+(((-1.0)*x32*x33))+(((42.1159029649369)*(pz*pz)))+((x30*x32))+((x31*x33)))) < -1-IKFAST_SINCOS_THRESH || (((-0.528301886792168)+(((42.1159029649369)*(py*py)))+(((-8.92857142856662)*cj1*pz))+(((42.1159029649369)*(px*px)))+(((-9.26549865228612e-6)*pz*sj1))+(((-1.0)*x30*x31))+(((-1.0)*x32*x33))+(((42.1159029649369)*(pz*pz)))+((x30*x32))+((x31*x33)))) > 1+IKFAST_SINCOS_THRESH )
    continue;
IkReal x34=((-1.0)*(IKasin(((-0.528301886792168)+(((42.1159029649369)*(py*py)))+(((-8.92857142856662)*cj1*pz))+(((42.1159029649369)*(px*px)))+(((-9.26549865228612e-6)*pz*sj1))+(((-1.0)*x30*x31))+(((-1.0)*x32*x33))+(((42.1159029649369)*(pz*pz)))+((x30*x32))+((x31*x33))))));
j3array[0]=((-1.57079736453075)+(((-1.0)*x34)));
sj3array[0]=IKsin(j3array[0]);
cj3array[0]=IKcos(j3array[0]);
j3array[1]=((1.57079528905905)+x34);
sj3array[1]=IKsin(j3array[1]);
cj3array[1]=IKcos(j3array[1]);
if( j3array[0] > IKPI )
{
    j3array[0]-=IK2PI;
}
else if( j3array[0] < -IKPI )
{    j3array[0]+=IK2PI;
}
j3valid[0] = true;
if( j3array[1] > IKPI )
{
    j3array[1]-=IK2PI;
}
else if( j3array[1] < -IKPI )
{    j3array[1]+=IK2PI;
}
j3valid[1] = true;
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 x35=px*px;
IkReal x36=py*py;
IkReal x37=pz*pz;
IkReal x38=((0.212)*py);
IkReal x39=(r00*sj1);
IkReal x40=(cj0*r00);
IkReal x41=(px*r01);
IkReal x42=((2.2e-7)*cj1);
IkReal x43=(cj1*r02);
IkReal x44=((2.0)*px);
IkReal x45=(pz*r02);
IkReal x46=(py*sj0);
IkReal x47=(r01*sj0);
IkReal x48=((2.2e-7)*sj1);
IkReal x49=(cj0*px);
IkReal x50=((1.0)*x40);
IkReal x51=((0.212)*cj1*pz);
evalcond[0]=((-0.0237800000000121)+(((2.0)*x45*x46))+(((2.0)*cj0*py*x41))+(((-1.0)*sj0*x38*x43))+(((-0.212)*x43*x49))+(((-1.0)*x38*x39))+((x35*x40))+((cj0*x44*x45))+(((2.464e-8)*(IKsin(j3))))+((x36*x47))+(((0.212)*sj1*x41))+((py*r00*x42))+(((-1.0)*x41*x42))+(((-1.0)*x37*x50))+(((-1.0)*x37*x47))+(((-0.023744)*(IKcos(j3))))+((pz*x47*x48))+(((-1.0)*r02*x48*x49))+((x40*x51))+(((-1.0)*x36*x50))+((x47*x51))+((r00*x44*x46))+(((-0.0112360000000121)*x47))+(((-0.0112360000000121)*x40))+(((-1.0)*r02*x46*x48))+(((2.2e-7)*cj0*pz*x39))+(((-1.0)*x35*x47)));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
{
continue;
}
}

{
IkReal j2eval[2];
sj5=1.0;
cj5=0;
j5=1.5707963267949;
j2eval[0]=((965097.402597894)+(((-1.0)*sj3))+(((963636.363636364)*cj3)));
j2eval[1]=IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3))));
if( IKabs(j2eval[0]) < 0.0000010000000000  || IKabs(j2eval[1]) < 0.0000010000000000  )
{
continue; // no branches [j2]

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x52=((0.112)*cj1);
IkReal x53=(cj0*py);
IkReal x54=((0.106)*cj1);
IkReal x55=(px*sj0);
IkReal x56=(cj3*pz);
IkReal x57=((1.1e-7)*cj1);
IkReal x58=((0.106)*sj1);
IkReal x59=((1.1e-7)*sj1);
IkReal x60=((0.112)*sj1);
IkReal x61=((0.112)*pz*sj3);
CheckValue<IkReal> x62=IKPowWithIntegerCheck(IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3)))),-1);
if(!x62.valid){
continue;
}
CheckValue<IkReal> x63 = IKatan2WithCheck(IkReal(((((-1.0)*x53*x59))+(((-1.0)*x53*x54))+(((-1.0)*x56*x60))+(((1.232e-8)*cj3))+((cj3*x52*x55))+(((-1.0)*pz*sj3*x52))+(((-1.0)*sj3*x55*x60))+(((-1.0)*pz*x58))+((sj3*x53*x60))+((pz*x57))+(((-1.0)*cj3*x52*x53))+(((0.011872)*sj3))+((x55*x59))+((x54*x55)))),IkReal(((-0.0112360000000121)+(((-1.0)*x53*x58))+(((-1.0)*sj3*x52*x53))+(((-1.0)*pz*sj3*x60))+((cj3*x55*x60))+(((1.232e-8)*sj3))+(((-1.0)*x55*x57))+((x53*x57))+((x52*x56))+(((-0.011872)*cj3))+((pz*x54))+((pz*x59))+((sj3*x52*x55))+((x55*x58))+(((-1.0)*cj3*x53*x60)))),IKFAST_ATAN2_MAGTHRESH);
if(!x63.valid){
continue;
}
j2array[0]=((-1.5707963267949)+(((1.5707963267949)*(x62.value)))+(x63.value));
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 x64=IKsin(j2);
IkReal x65=IKcos(j2);
IkReal x66=((1.0)*r01);
IkReal x67=(cj1*pz);
IkReal x68=(r00*sj1);
IkReal x69=(cj0*r00);
IkReal x70=(px*sj1);
IkReal x71=(cj0*r02);
IkReal x72=(cj1*py);
IkReal x73=(r02*sj0);
IkReal x74=(r01*sj0);
IkReal x75=(cj1*px);
IkReal x76=(pz*sj1);
IkReal x77=((1.1e-7)*x64);
IkReal x78=((0.106)*x65);
IkReal x79=((1.1e-7)*x65);
IkReal x80=((0.106)*x64);
IkReal x81=((0.112)*x65);
IkReal x82=((0.112)*x64);
IkReal x83=((1.0)*py*sj1);
IkReal x84=(sj3*x82);
IkReal x85=(cj3*x81);
IkReal x86=(cj3*x82);
IkReal x87=(sj3*x81);
IkReal x88=(x77+x78+x85);
IkReal x89=(x80+x86+x87);
evalcond[0]=((-1.1e-7)+(((-1.0)*x79))+((cj0*x72))+x76+x89+(((-1.0)*sj0*x75)));
evalcond[1]=((-0.106)+(((-1.0)*cj0*x83))+x67+x84+((sj0*x70))+(((-1.0)*x88)));
evalcond[2]=((((-1.0)*sj0*x66*x67))+(((-1.0)*x66*x70))+(((0.106)*x74))+(((0.106)*x69))+x84+(((-1.0)*x67*x69))+((x72*x73))+(((-1.0)*x88))+((py*x68))+((x71*x75)));
evalcond[3]=((((-1.0)*x73*x83))+(((-1.0)*x70*x71))+(((-1.0)*x66*x75))+((cj0*pz*x68))+(((-1.1e-7)*x74))+x79+(((-1.1e-7)*x69))+(((-1.0)*x89))+((x74*x76))+((r00*x72)));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
{
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 = j5;
vinfos[4].indices[0] = _ij5[0];
vinfos[4].indices[1] = _ij5[1];
vinfos[4].maxsolutions = _nj5;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}
}
}

}
} while(0);
if( bgotonextstatement )
{
bool bgotonextstatement = true;
do
{
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
if( IKabs(evalcond[0]) < 0.0000050000000000  )
{
bgotonextstatement=false;
{
IkReal j3array[2], cj3array[2], sj3array[2];
bool j3valid[2]={false};
_nj3 = 2;
IkReal x90=((8.92857142856662)*sj1);
IkReal x91=(px*sj0);
IkReal x92=(cj0*py);
IkReal x93=((9.26549865228612e-6)*cj1);
if( (((-0.528301886792168)+(((42.1159029649369)*(py*py)))+(((-8.92857142856662)*cj1*pz))+(((42.1159029649369)*(px*px)))+(((-9.26549865228612e-6)*pz*sj1))+(((-1.0)*x90*x91))+((x90*x92))+(((42.1159029649369)*(pz*pz)))+(((-1.0)*x92*x93))+((x91*x93)))) < -1-IKFAST_SINCOS_THRESH || (((-0.528301886792168)+(((42.1159029649369)*(py*py)))+(((-8.92857142856662)*cj1*pz))+(((42.1159029649369)*(px*px)))+(((-9.26549865228612e-6)*pz*sj1))+(((-1.0)*x90*x91))+((x90*x92))+(((42.1159029649369)*(pz*pz)))+(((-1.0)*x92*x93))+((x91*x93)))) > 1+IKFAST_SINCOS_THRESH )
    continue;
IkReal x94=((-1.0)*(IKasin(((-0.528301886792168)+(((42.1159029649369)*(py*py)))+(((-8.92857142856662)*cj1*pz))+(((42.1159029649369)*(px*px)))+(((-9.26549865228612e-6)*pz*sj1))+(((-1.0)*x90*x91))+((x90*x92))+(((42.1159029649369)*(pz*pz)))+(((-1.0)*x92*x93))+((x91*x93))))));
j3array[0]=((-1.57079736453075)+(((-1.0)*x94)));
sj3array[0]=IKsin(j3array[0]);
cj3array[0]=IKcos(j3array[0]);
j3array[1]=((1.57079528905905)+x94);
sj3array[1]=IKsin(j3array[1]);
cj3array[1]=IKcos(j3array[1]);
if( j3array[0] > IKPI )
{
    j3array[0]-=IK2PI;
}
else if( j3array[0] < -IKPI )
{    j3array[0]+=IK2PI;
}
j3valid[0] = true;
if( j3array[1] > IKPI )
{
    j3array[1]-=IK2PI;
}
else if( j3array[1] < -IKPI )
{    j3array[1]+=IK2PI;
}
j3valid[1] = true;
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 x95=px*px;
IkReal x96=py*py;
IkReal x97=pz*pz;
IkReal x98=((0.212)*py);
IkReal x99=(r00*sj1);
IkReal x100=(cj0*r00);
IkReal x101=(px*r01);
IkReal x102=((2.2e-7)*cj1);
IkReal x103=(cj1*r02);
IkReal x104=((2.0)*px);
IkReal x105=(pz*r02);
IkReal x106=(py*sj0);
IkReal x107=(r01*sj0);
IkReal x108=((2.2e-7)*sj1);
IkReal x109=(cj0*px);
IkReal x110=((1.0)*x100);
IkReal x111=((0.212)*cj1*pz);
evalcond[0]=((0.0237800000000121)+(((2.2e-7)*cj0*pz*x99))+((x100*x111))+(((-1.0)*x98*x99))+((x100*x95))+(((-1.0)*x101*x102))+(((2.0)*x105*x106))+((x107*x96))+(((-1.0)*r02*x106*x108))+((pz*x107*x108))+(((-1.0)*r02*x108*x109))+(((-2.464e-8)*(IKsin(j3))))+(((-1.0)*x107*x97))+(((-1.0)*x107*x95))+(((-1.0)*x110*x96))+(((-1.0)*x110*x97))+(((2.0)*cj0*py*x101))+(((-0.212)*x103*x109))+(((0.212)*sj1*x101))+(((0.023744)*(IKcos(j3))))+(((-1.0)*sj0*x103*x98))+((cj0*x104*x105))+((r00*x104*x106))+(((-0.0112360000000121)*x100))+(((-0.0112360000000121)*x107))+((py*r00*x102))+((x107*x111)));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
{
continue;
}
}

{
IkReal j2eval[2];
sj5=-1.0;
cj5=0;
j5=-1.5707963267949;
j2eval[0]=((965097.402597894)+(((-1.0)*sj3))+(((963636.363636364)*cj3)));
j2eval[1]=IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3))));
if( IKabs(j2eval[0]) < 0.0000010000000000  || IKabs(j2eval[1]) < 0.0000010000000000  )
{
continue; // no branches [j2]

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x112=((0.112)*cj1);
IkReal x113=(cj0*py);
IkReal x114=((0.106)*cj1);
IkReal x115=(px*sj0);
IkReal x116=(cj3*pz);
IkReal x117=((1.1e-7)*cj1);
IkReal x118=((0.106)*sj1);
IkReal x119=((1.1e-7)*sj1);
IkReal x120=((0.112)*sj1);
IkReal x121=((0.112)*pz*sj3);
CheckValue<IkReal> x122=IKPowWithIntegerCheck(IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3)))),-1);
if(!x122.valid){
continue;
}
CheckValue<IkReal> x123 = IKatan2WithCheck(IkReal((((cj3*x112*x115))+(((-1.0)*sj3*x115*x120))+(((1.232e-8)*cj3))+((pz*x117))+((x114*x115))+(((-1.0)*x113*x119))+(((-1.0)*x113*x114))+(((-1.0)*x116*x120))+(((-1.0)*cj3*x112*x113))+(((-1.0)*pz*sj3*x112))+(((0.011872)*sj3))+((x115*x119))+((sj3*x113*x120))+(((-1.0)*pz*x118)))),IkReal(((-0.0112360000000121)+((sj3*x112*x115))+((x112*x116))+(((-1.0)*cj3*x113*x120))+((pz*x119))+((pz*x114))+(((-1.0)*x113*x118))+(((1.232e-8)*sj3))+((x113*x117))+((cj3*x115*x120))+(((-1.0)*x115*x117))+(((-1.0)*pz*sj3*x120))+(((-1.0)*sj3*x112*x113))+(((-0.011872)*cj3))+((x115*x118)))),IKFAST_ATAN2_MAGTHRESH);
if(!x123.valid){
continue;
}
j2array[0]=((-1.5707963267949)+(((1.5707963267949)*(x122.value)))+(x123.value));
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 x124=IKsin(j2);
IkReal x125=IKcos(j2);
IkReal x126=((1.0)*r01);
IkReal x127=(cj1*pz);
IkReal x128=(r00*sj1);
IkReal x129=(cj0*r00);
IkReal x130=(px*sj1);
IkReal x131=(cj0*r02);
IkReal x132=(cj1*py);
IkReal x133=(r02*sj0);
IkReal x134=(r01*sj0);
IkReal x135=(cj1*px);
IkReal x136=(pz*sj1);
IkReal x137=((1.1e-7)*x124);
IkReal x138=((0.106)*x125);
IkReal x139=((0.106)*x124);
IkReal x140=((1.1e-7)*x125);
IkReal x141=((0.112)*x125);
IkReal x142=((0.112)*x124);
IkReal x143=((1.0)*py*sj1);
IkReal x144=(cj3*x141);
IkReal x145=(sj3*x142);
IkReal x146=(cj3*x142);
IkReal x147=(sj3*x141);
IkReal x148=(x144+x137+x138);
IkReal x149=(x146+x147+x139);
evalcond[0]=((-1.1e-7)+(((-1.0)*x140))+((cj0*x132))+x149+x136+(((-1.0)*sj0*x135)));
evalcond[1]=((-0.106)+(((-1.0)*x148))+(((-1.0)*cj0*x143))+x145+x127+((sj0*x130)));
evalcond[2]=((((-1.0)*x145))+(((0.106)*x134))+(((0.106)*x129))+(((-1.0)*x127*x129))+((py*x128))+(((-1.0)*x126*x130))+x148+((x131*x135))+(((-1.0)*sj0*x126*x127))+((x132*x133)));
evalcond[3]=(((cj0*pz*x128))+(((-1.0)*x140))+(((-1.0)*x130*x131))+(((-1.0)*x133*x143))+(((-1.1e-7)*x129))+(((-1.0)*x126*x135))+(((-1.1e-7)*x134))+((r00*x132))+x149+((x134*x136)));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
{
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 = j5;
vinfos[4].indices[0] = _ij5[0];
vinfos[4].indices[1] = _ij5[1];
vinfos[4].maxsolutions = _nj5;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}
}
}

}
} while(0);
if( bgotonextstatement )
{
bool bgotonextstatement = true;
do
{
if( 1 )
{
bgotonextstatement=false;
continue; // branch miss [j2, j3]

}
} while(0);
if( bgotonextstatement )
{
}
}
}
}

} else
{
{
IkReal j3array[1], cj3array[1], sj3array[1];
bool j3valid[1]={false};
_nj3 = 1;
IkReal x150=((1100000000.0)*px);
IkReal x151=(r02*sj0);
IkReal x152=((112360000000121.0)*r02);
IkReal x153=((1.06e+15)*r00);
IkReal x154=(pz*sj0);
IkReal x155=((112360000000121.0)*sj1);
IkReal x156=(cj0*r01);
IkReal x157=(py*r01);
IkReal x158=(r00*sj0);
IkReal x159=((112360000000121.0)*cj1);
IkReal x160=(pz*r02);
IkReal x161=(cj0*py*r02);
CheckValue<IkReal> x162 = IKatan2WithCheck(IkReal(((((1100000000.0)*r00*x154))+(((1.06e+15)*x160))+(((1.06e+15)*x157))+(((1100000000.0)*x161))+(((123200000.0)*cj5))+(((-1.0)*x150*x151))+((x155*x156))+(((-1100000000.0)*pz*x156))+((px*x153))+(((-1.0)*cj1*x152))+(((-1.0)*x155*x158)))),IkReal(((((-1.06e+15)*x161))+(((1100000000.0)*x157))+(((1100000000.0)*x160))+(((-1.0)*x153*x154))+((r00*x150))+(((-118720000000000.0)*cj5))+(((1.06e+15)*px*x151))+(((1.06e+15)*pz*x156))+(((-1.0)*x156*x159))+((x158*x159))+(((-1.0)*sj1*x152)))),IKFAST_ATAN2_MAGTHRESH);
if(!x162.valid){
continue;
}
CheckValue<IkReal> x163=IKPowWithIntegerCheck(IKsign(cj5),-1);
if(!x163.valid){
continue;
}
j3array[0]=((-1.5707963267949)+(x162.value)+(((1.5707963267949)*(x163.value))));
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 x164=IKcos(j3);
IkReal x165=IKsin(j3);
IkReal x166=px*px;
IkReal x167=py*py;
IkReal x168=pz*pz;
IkReal x169=(py*r00);
IkReal x170=((2.0)*px);
IkReal x171=((1.1e-7)*cj1);
IkReal x172=(cj0*r01);
IkReal x173=((0.212)*sj1);
IkReal x174=(cj0*r00);
IkReal x175=(r00*sj0);
IkReal x176=((0.212)*cj1);
IkReal x177=(cj0*py);
IkReal x178=((1.1e-7)*sj1);
IkReal x179=((2.2e-7)*cj1);
IkReal x180=(px*r01);
IkReal x181=((0.106)*cj1);
IkReal x182=(cj0*r02);
IkReal x183=(px*sj0);
IkReal x184=((1.0)*pz);
IkReal x185=(r01*sj0);
IkReal x186=((2.2e-7)*sj1);
IkReal x187=((0.106)*sj1);
IkReal x188=(cj5*x165);
IkReal x189=((1.0)*x167);
IkReal x190=(py*r02*sj0);
IkReal x191=((0.023744)*x164);
IkReal x192=((2.464e-8)*x165);
IkReal x193=(pz*x186);
IkReal x194=(cj5*x164);
IkReal x195=((1.0)*x168);
IkReal x196=((1.0)*x166);
evalcond[0]=((((-1.0)*px*r00))+(((1.1e-7)*x194))+(((-1.0)*r02*x184))+(((0.106)*x188))+((x175*x187))+((x171*x172))+(((-1.0)*x171*x175))+(((-1.0)*py*r01))+((r02*x181))+(((-1.0)*x172*x187))+((r02*x178)));
evalcond[1]=((0.012544)+((pz*x176))+((x173*x183))+(((-1.0)*x189))+(((-1.0)*x195))+(((-1.0)*x196))+(((-1.0)*x192))+x193+x191+(((-1.0)*x179*x183))+(((-1.0)*x173*x177))+((x177*x179)));
evalcond[2]=(((pz*x175))+(((-1.0)*x175*x178))+(((-1.0)*r02*x183))+(((0.112)*cj5))+((x172*x181))+(((-1.0)*r02*x171))+((x172*x178))+(((0.106)*x194))+(((-1.1e-7)*x188))+(((-1.0)*x175*x181))+((r02*x187))+(((-1.0)*x172*x184))+((r02*x177)));
evalcond[3]=((((-1.0)*px*x182*x186))+((pz*x176*x185))+((pz*x170*x182))+(((-0.0112360000000121)*x185))+((py*x170*x172))+((x173*x180))+(((-1.0)*x186*x190))+(((-0.0112360000000121)*x174))+((x169*x179))+(((-1.0)*px*x176*x182))+((x174*x193))+((pz*x174*x176))+((x185*x193))+(((2.0)*pz*x190))+((x167*x185))+((x166*x174))+(((-0.0237800000000121)*sj5))+(((-1.0)*x176*x190))+(((-1.0)*sj5*x191))+(((-1.0)*x185*x196))+(((-1.0)*x185*x195))+(((-1.0)*x169*x173))+(((-1.0)*x179*x180))+(((-1.0)*x174*x195))+(((-1.0)*x174*x189))+((sj0*x169*x170))+((sj5*x192)));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
{
continue;
}
}

{
IkReal j2eval[2];
j2eval[0]=cj5;
j2eval[1]=IKsign(cj5);
if( IKabs(j2eval[0]) < 0.0000010000000000  || IKabs(j2eval[1]) < 0.0000010000000000  )
{
{
IkReal j2eval[2];
IkReal x197=(cj3*cj5);
IkReal x198=(cj5*sj3);
j2eval[0]=((((-1.0)*x197))+(((-963636.363636364)*x198)));
j2eval[1]=IKsign(((((-1.1e-7)*x197))+(((-0.106)*x198))));
if( IKabs(j2eval[0]) < 0.0000010000000000  || IKabs(j2eval[1]) < 0.0000010000000000  )
{
{
IkReal j2eval[2];
IkReal x199=(cj3*cj5);
IkReal x200=(cj5*sj3);
j2eval[0]=((((1018181.81818182)*cj5))+(((963636.363636364)*x199))+(((-1.0)*x200)));
j2eval[1]=IKsign(((((0.112)*cj5))+(((0.106)*x199))+(((-1.1e-7)*x200))));
if( IKabs(j2eval[0]) < 0.0000010000000000  || IKabs(j2eval[1]) < 0.0000010000000000  )
{
{
IkReal evalcond[1];
bool bgotonextstatement = true;
do
{
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
if( IKabs(evalcond[0]) < 0.0000050000000000  )
{
bgotonextstatement=false;
{
IkReal j2eval[2];
sj5=1.0;
cj5=0;
j5=1.5707963267949;
j2eval[0]=((965097.402597894)+(((-1.0)*sj3))+(((963636.363636364)*cj3)));
j2eval[1]=IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3))));
if( IKabs(j2eval[0]) < 0.0000010000000000  || IKabs(j2eval[1]) < 0.0000010000000000  )
{
continue; // no branches [j2]

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x201=((0.112)*cj1);
IkReal x202=(cj0*py);
IkReal x203=((0.106)*cj1);
IkReal x204=(px*sj0);
IkReal x205=(cj3*pz);
IkReal x206=((1.1e-7)*cj1);
IkReal x207=((0.106)*sj1);
IkReal x208=((1.1e-7)*sj1);
IkReal x209=((0.112)*sj1);
IkReal x210=((0.112)*pz*sj3);
CheckValue<IkReal> x211 = IKatan2WithCheck(IkReal(((((1.232e-8)*cj3))+((cj3*x201*x204))+(((-1.0)*pz*sj3*x201))+((sj3*x202*x209))+(((-1.0)*pz*x207))+(((-1.0)*x202*x203))+(((-1.0)*x202*x208))+(((-1.0)*sj3*x204*x209))+((pz*x206))+((x204*x208))+(((-1.0)*cj3*x201*x202))+(((-1.0)*x205*x209))+((x203*x204))+(((0.011872)*sj3)))),IkReal(((-0.0112360000000121)+(((-1.0)*cj3*x202*x209))+(((-1.0)*x204*x206))+(((-1.0)*sj3*x201*x202))+((x201*x205))+(((-1.0)*pz*sj3*x209))+(((1.232e-8)*sj3))+(((-1.0)*x202*x207))+((pz*x203))+((pz*x208))+((x204*x207))+((cj3*x204*x209))+((x202*x206))+(((-0.011872)*cj3))+((sj3*x201*x204)))),IKFAST_ATAN2_MAGTHRESH);
if(!x211.valid){
continue;
}
CheckValue<IkReal> x212=IKPowWithIntegerCheck(IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3)))),-1);
if(!x212.valid){
continue;
}
j2array[0]=((-1.5707963267949)+(x211.value)+(((1.5707963267949)*(x212.value))));
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 x213=IKsin(j2);
IkReal x214=IKcos(j2);
IkReal x215=((1.0)*r01);
IkReal x216=(cj1*pz);
IkReal x217=(r00*sj1);
IkReal x218=(cj0*r00);
IkReal x219=(px*sj1);
IkReal x220=(cj0*r02);
IkReal x221=(cj1*py);
IkReal x222=(r02*sj0);
IkReal x223=(r01*sj0);
IkReal x224=(cj1*px);
IkReal x225=(pz*sj1);
IkReal x226=((1.1e-7)*x213);
IkReal x227=((0.106)*x214);
IkReal x228=((1.1e-7)*x214);
IkReal x229=((0.106)*x213);
IkReal x230=((0.112)*x214);
IkReal x231=((0.112)*x213);
IkReal x232=((1.0)*py*sj1);
IkReal x233=(sj3*x231);
IkReal x234=(cj3*x230);
IkReal x235=(cj3*x231);
IkReal x236=(sj3*x230);
IkReal x237=(x227+x226+x234);
IkReal x238=(x229+x236+x235);
evalcond[0]=((-1.1e-7)+(((-1.0)*sj0*x224))+((cj0*x221))+x225+x238+(((-1.0)*x228)));
evalcond[1]=((-0.106)+(((-1.0)*cj0*x232))+(((-1.0)*x237))+x216+x233+((sj0*x219)));
evalcond[2]=((((0.106)*x223))+((py*x217))+(((-1.0)*x216*x218))+(((0.106)*x218))+(((-1.0)*x237))+x233+((x220*x224))+((x221*x222))+(((-1.0)*x215*x219))+(((-1.0)*sj0*x215*x216)));
evalcond[3]=((((-1.1e-7)*x223))+((cj0*pz*x217))+(((-1.0)*x215*x224))+(((-1.1e-7)*x218))+((r00*x221))+((x223*x225))+(((-1.0)*x238))+x228+(((-1.0)*x219*x220))+(((-1.0)*x222*x232)));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
{
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 = j5;
vinfos[4].indices[0] = _ij5[0];
vinfos[4].indices[1] = _ij5[1];
vinfos[4].maxsolutions = _nj5;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

}
} while(0);
if( bgotonextstatement )
{
bool bgotonextstatement = true;
do
{
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
if( IKabs(evalcond[0]) < 0.0000050000000000  )
{
bgotonextstatement=false;
{
IkReal j2eval[2];
sj5=-1.0;
cj5=0;
j5=-1.5707963267949;
j2eval[0]=((965097.402597894)+(((-1.0)*sj3))+(((963636.363636364)*cj3)));
j2eval[1]=IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3))));
if( IKabs(j2eval[0]) < 0.0000010000000000  || IKabs(j2eval[1]) < 0.0000010000000000  )
{
continue; // no branches [j2]

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x239=((0.112)*cj1);
IkReal x240=(cj0*py);
IkReal x241=((0.106)*cj1);
IkReal x242=(px*sj0);
IkReal x243=(cj3*pz);
IkReal x244=((1.1e-7)*cj1);
IkReal x245=((0.106)*sj1);
IkReal x246=((1.1e-7)*sj1);
IkReal x247=((0.112)*sj1);
IkReal x248=((0.112)*pz*sj3);
CheckValue<IkReal> x249=IKPowWithIntegerCheck(IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3)))),-1);
if(!x249.valid){
continue;
}
CheckValue<IkReal> x250 = IKatan2WithCheck(IkReal(((((-1.0)*pz*x245))+((cj3*x239*x242))+(((1.232e-8)*cj3))+(((-1.0)*pz*sj3*x239))+(((-1.0)*x243*x247))+((x241*x242))+(((-1.0)*sj3*x242*x247))+((x242*x246))+((sj3*x240*x247))+(((-1.0)*x240*x241))+(((-1.0)*x240*x246))+(((0.011872)*sj3))+(((-1.0)*cj3*x239*x240))+((pz*x244)))),IkReal(((-0.0112360000000121)+(((-1.0)*cj3*x240*x247))+(((-1.0)*sj3*x239*x240))+(((-1.0)*x242*x244))+((x240*x244))+(((1.232e-8)*sj3))+((cj3*x242*x247))+((x239*x243))+((sj3*x239*x242))+((x242*x245))+(((-1.0)*x240*x245))+(((-0.011872)*cj3))+((pz*x246))+((pz*x241))+(((-1.0)*pz*sj3*x247)))),IKFAST_ATAN2_MAGTHRESH);
if(!x250.valid){
continue;
}
j2array[0]=((-1.5707963267949)+(((1.5707963267949)*(x249.value)))+(x250.value));
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 x251=IKsin(j2);
IkReal x252=IKcos(j2);
IkReal x253=((1.0)*r01);
IkReal x254=(cj1*pz);
IkReal x255=(r00*sj1);
IkReal x256=(cj0*r00);
IkReal x257=(px*sj1);
IkReal x258=(cj0*r02);
IkReal x259=(cj1*py);
IkReal x260=(r02*sj0);
IkReal x261=(r01*sj0);
IkReal x262=(cj1*px);
IkReal x263=(pz*sj1);
IkReal x264=((1.1e-7)*x251);
IkReal x265=((0.106)*x252);
IkReal x266=((0.106)*x251);
IkReal x267=((1.1e-7)*x252);
IkReal x268=((0.112)*x252);
IkReal x269=((0.112)*x251);
IkReal x270=((1.0)*py*sj1);
IkReal x271=(cj3*x268);
IkReal x272=(sj3*x269);
IkReal x273=(cj3*x269);
IkReal x274=(sj3*x268);
IkReal x275=(x265+x264+x271);
IkReal x276=(x266+x274+x273);
evalcond[0]=((-1.1e-7)+(((-1.0)*sj0*x262))+x263+x276+((cj0*x259))+(((-1.0)*x267)));
evalcond[1]=((-0.106)+((sj0*x257))+x254+x272+(((-1.0)*cj0*x270))+(((-1.0)*x275)));
evalcond[2]=((((0.106)*x256))+((x258*x262))+(((-1.0)*sj0*x253*x254))+((x259*x260))+((py*x255))+(((0.106)*x261))+x275+(((-1.0)*x254*x256))+(((-1.0)*x272))+(((-1.0)*x253*x257)));
evalcond[3]=((((-1.1e-7)*x256))+((r00*x259))+(((-1.1e-7)*x261))+(((-1.0)*x253*x262))+(((-1.0)*x257*x258))+x276+((cj0*pz*x255))+((x261*x263))+(((-1.0)*x267))+(((-1.0)*x260*x270)));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
{
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 = j5;
vinfos[4].indices[0] = _ij5[0];
vinfos[4].indices[1] = _ij5[1];
vinfos[4].maxsolutions = _nj5;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

}
} while(0);
if( bgotonextstatement )
{
bool bgotonextstatement = true;
do
{
if( 1 )
{
bgotonextstatement=false;
continue; // branch miss [j2]

}
} while(0);
if( bgotonextstatement )
{
}
}
}
}

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x277=((1.1e-7)*cj1);
IkReal x278=(cj0*r01);
IkReal x279=(cj5*sj3);
IkReal x280=(cj3*cj5);
IkReal x281=(cj1*sj0);
IkReal x282=((0.112)*cj3);
IkReal x283=(r02*sj1);
IkReal x284=((0.112)*sj3);
IkReal x285=((1.1e-7)*x279);
IkReal x286=((1.0)*pz*sj1);
IkReal x287=((1.0)*cj0*cj1*py);
CheckValue<IkReal> x288 = IKatan2WithCheck(IkReal(((((-1.0)*r00*sj0*x277))+(((1.1e-7)*x283))+(((1.1e-7)*x280))+((r00*x281*x284))+(((-1.0)*x280*x286))+(((-1.0)*x280*x287))+((px*x280*x281))+((x277*x278))+(((-1.0)*cj1*x278*x284))+(((-1.0)*x283*x284)))),IkReal((((cj1*x278*x282))+(((0.106)*x283))+(((0.106)*cj1*x278))+((px*x279*x281))+(((-0.106)*r00*x281))+(((-1.0)*r00*x281*x282))+x285+(((-1.0)*x279*x287))+(((-1.0)*x279*x286))+((x282*x283)))),IKFAST_ATAN2_MAGTHRESH);
if(!x288.valid){
continue;
}
CheckValue<IkReal> x289=IKPowWithIntegerCheck(IKsign(((((0.112)*cj5))+(((0.106)*x280))+(((-1.0)*x285)))),-1);
if(!x289.valid){
continue;
}
j2array[0]=((-1.5707963267949)+(x288.value)+(((1.5707963267949)*(x289.value))));
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 x290=IKcos(j2);
IkReal x291=IKsin(j2);
IkReal x292=px*px;
IkReal x293=pz*pz;
IkReal x294=py*py;
IkReal x295=(py*sj1);
IkReal x296=((2.0)*r01);
IkReal x297=(px*sj0);
IkReal x298=(pz*r00);
IkReal x299=(cj0*sj1);
IkReal x300=((0.0237799999999879)*cj3);
IkReal x301=(cj0*r01);
IkReal x302=(px*r00);
IkReal x303=((0.212)*pz);
IkReal x304=((1.0)*px);
IkReal x305=((2.332e-8)*cj5);
IkReal x306=(cj1*r01);
IkReal x307=(cj1*r02);
IkReal x308=((2.2e-7)*r02);
IkReal x309=(r01*sj0);
IkReal x310=(r00*sj0);
IkReal x311=((1.0)*cj1);
IkReal x312=((2.0)*pz);
IkReal x313=(cj0*r00);
IkReal x314=((0.112)*cj3);
IkReal x315=(cj1*py);
IkReal x316=((1.0)*cj3);
IkReal x317=((2.0)*cj0);
IkReal x318=(cj0*py);
IkReal x319=(r02*sj1);
IkReal x320=((2.332e-8)*cj1);
IkReal x321=((1.0)*r01);
IkReal x322=(pz*sj1);
IkReal x323=((1.0)*sj0);
IkReal x324=(py*r01);
IkReal x325=((0.212)*r02);
IkReal x326=((0.0112359999999879)*cj1);
IkReal x327=((2.0)*px);
IkReal x328=(sj3*x290);
IkReal x329=(cj5*x290);
IkReal x330=(cj1*x292);
IkReal x331=((1.0)*x292);
IkReal x332=(cj5*x291);
IkReal x333=(sj1*x294);
IkReal x334=(sj5*x291);
IkReal x335=(sj3*x291);
IkReal x336=((0.106)*x290);
IkReal x337=((1.1e-7)*x290);
IkReal x338=((1.0)*x293);
evalcond[0]=(((cj1*x301))+(((-1.0)*x310*x311))+((sj3*x332))+x319+(((-1.0)*x316*x329)));
evalcond[1]=((((-1.0)*x299*x321))+(((-1.0)*x316*x332))+((sj1*x310))+x307+(((-1.0)*cj5*x328)));
evalcond[2]=((-1.1e-7)+(((-1.0)*x297*x311))+x322+(((0.112)*x328))+(((0.106)*x291))+((x291*x314))+(((-1.0)*x337))+((cj0*x315)));
evalcond[3]=((-0.106)+(((-1.0)*cj0*x295))+(((-1.0)*x290*x314))+((sj1*x297))+((cj1*pz))+(((0.112)*x335))+(((-1.1e-7)*x291))+(((-1.0)*x336)));
evalcond[4]=((((-1.0)*sj5*x290*x314))+(((-1.0)*sj5*x336))+(((-1.1e-7)*x334))+(((0.106)*x309))+((cj0*px*x307))+(((0.112)*sj3*x334))+(((-1.0)*cj0*x298*x311))+(((0.106)*x313))+((r00*x295))+(((-1.0)*pz*x306*x323))+(((-1.0)*r01*sj1*x304))+((py*sj0*x307)));
evalcond[5]=((((-1.0)*r02*x295*x323))+(((-1.1e-7)*x309))+((sj5*x337))+(((-0.106)*x334))+(((-1.1e-7)*x313))+((x298*x299))+((r00*x315))+((x309*x322))+(((-1.0)*r02*x299*x304))+(((-1.0)*x304*x306))+(((-1.0)*x314*x334))+(((-0.112)*sj5*x328)));
evalcond[6]=(((x294*x319))+((x318*x325))+((x301*x326))+(((-1.0)*x301*x303))+(((0.0112359999999879)*x319))+(((-2.464e-8)*x332))+(((-1.0)*x302*x315*x317))+(((-1.0)*x294*x310*x311))+(((-2.332e-8)*sj1*x310))+(((2.2e-7)*x324))+(((-1.0)*x294*x301*x311))+(((-1.0)*x319*x338))+(((-1.0)*cj3*x291*x305))+((x296*x297*x315))+(((-1.0)*pz*x295*x296))+((x305*x328))+(((-0.023744)*x329))+(((-1.0)*x307*x312*x318))+(((-1.0)*x300*x329))+((x297*x307*x312))+(((0.212)*sj0*x298))+(((0.0013080000000121)*sj3*x332))+((cj1*x293*x301))+(((-1.0)*x293*x310*x311))+(((2.2e-7)*x302))+(((2.332e-8)*r01*x299))+(((-1.0)*sj1*x298*x327))+(((-1.0)*x310*x326))+(((-1.0)*x297*x325))+((x292*x319))+(((-2.332e-8)*x307))+((x301*x330))+((x310*x330))+((pz*x308)));
evalcond[7]=((((-1.0)*x300*x332))+(((-1.0)*x295*x296*x297))+(((2.2e-7)*pz*x301))+((x310*x320))+((x292*x307))+(((-1.0)*sj1*x310*x331))+(((0.212)*x302))+(((-0.023744)*x332))+(((-2.332e-8)*x319))+((r02*x303))+((cj0*r02*x295*x312))+(((-1.0)*x293*x299*x321))+(((-1.0)*x308*x318))+((x294*x307))+((x305*x335))+((x295*x302*x317))+(((-1.0)*x301*x320))+((x297*x308))+(((-1.0)*x307*x338))+(((-0.0112359999999879)*sj1*x310))+(((0.0112359999999879)*r01*x299))+((sj1*x293*x310))+(((-0.0112359999999879)*x307))+((cj3*x290*x305))+(((-1.0)*pz*x296*x315))+(((0.212)*x324))+((r01*x294*x299))+(((-2.2e-7)*sj0*x298))+(((-1.0)*x292*x299*x321))+(((-1.0)*x297*x312*x319))+((x310*x333))+(((2.464e-8)*x329))+(((-0.0013080000000121)*cj5*x328))+(((-1.0)*cj1*x298*x327)));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
{
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 = j5;
vinfos[4].indices[0] = _ij5[0];
vinfos[4].indices[1] = _ij5[1];
vinfos[4].maxsolutions = _nj5;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x339=(cj5*sj3);
IkReal x340=(pz*sj1);
IkReal x341=(cj3*cj5);
IkReal x342=((1.1e-7)*sj1);
IkReal x343=(cj0*r01);
IkReal x344=(cj1*r02);
IkReal x345=((0.112)*sj3);
IkReal x346=((0.112)*cj3);
IkReal x347=(r00*sj0);
IkReal x348=((0.106)*sj1);
IkReal x349=((1.1e-7)*x341);
IkReal x350=(cj1*px*sj0);
IkReal x351=(cj0*cj1*py);
IkReal x352=((0.112)*sj1*x347);
CheckValue<IkReal> x353=IKPowWithIntegerCheck(IKsign(((((-0.106)*x339))+(((-1.0)*x349)))),-1);
if(!x353.valid){
continue;
}
CheckValue<IkReal> x354 = IKatan2WithCheck(IkReal(((((-1.1e-7)*x344))+(((-1.1e-7)*x339))+(((-1.0)*sj1*x343*x345))+((sj1*x345*x347))+((x344*x345))+(((-1.0)*x339*x350))+((x339*x340))+(((-1.0)*x342*x347))+((x339*x351))+((x342*x343)))),IkReal(((((-1.0)*x341*x351))+(((-0.106)*x344))+((x341*x350))+x349+(((-1.0)*x344*x346))+((sj1*x343*x346))+(((-1.0)*x340*x341))+(((-1.0)*x347*x348))+(((-1.0)*sj1*x346*x347))+((x343*x348)))),IKFAST_ATAN2_MAGTHRESH);
if(!x354.valid){
continue;
}
j2array[0]=((-1.5707963267949)+(((1.5707963267949)*(x353.value)))+(x354.value));
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 x355=IKcos(j2);
IkReal x356=IKsin(j2);
IkReal x357=px*px;
IkReal x358=pz*pz;
IkReal x359=py*py;
IkReal x360=(py*sj1);
IkReal x361=((2.0)*r01);
IkReal x362=(px*sj0);
IkReal x363=(pz*r00);
IkReal x364=(cj0*sj1);
IkReal x365=((0.0237799999999879)*cj3);
IkReal x366=(cj0*r01);
IkReal x367=(px*r00);
IkReal x368=((0.212)*pz);
IkReal x369=((1.0)*px);
IkReal x370=((2.332e-8)*cj5);
IkReal x371=(cj1*r01);
IkReal x372=(cj1*r02);
IkReal x373=((2.2e-7)*r02);
IkReal x374=(r01*sj0);
IkReal x375=(r00*sj0);
IkReal x376=((1.0)*cj1);
IkReal x377=((2.0)*pz);
IkReal x378=(cj0*r00);
IkReal x379=((0.112)*cj3);
IkReal x380=(cj1*py);
IkReal x381=((1.0)*cj3);
IkReal x382=((2.0)*cj0);
IkReal x383=(cj0*py);
IkReal x384=(r02*sj1);
IkReal x385=((2.332e-8)*cj1);
IkReal x386=((1.0)*r01);
IkReal x387=(pz*sj1);
IkReal x388=((1.0)*sj0);
IkReal x389=(py*r01);
IkReal x390=((0.212)*r02);
IkReal x391=((0.0112359999999879)*cj1);
IkReal x392=((2.0)*px);
IkReal x393=(sj3*x355);
IkReal x394=(cj5*x355);
IkReal x395=(cj1*x357);
IkReal x396=((1.0)*x357);
IkReal x397=(cj5*x356);
IkReal x398=(sj1*x359);
IkReal x399=(sj5*x356);
IkReal x400=(sj3*x356);
IkReal x401=((0.106)*x355);
IkReal x402=((1.1e-7)*x355);
IkReal x403=((1.0)*x358);
evalcond[0]=(x384+((sj3*x397))+((cj1*x366))+(((-1.0)*x381*x394))+(((-1.0)*x375*x376)));
evalcond[1]=(((sj1*x375))+(((-1.0)*x364*x386))+(((-1.0)*cj5*x393))+x372+(((-1.0)*x381*x397)));
evalcond[2]=((-1.1e-7)+(((-1.0)*x362*x376))+((x356*x379))+x387+((cj0*x380))+(((0.112)*x393))+(((-1.0)*x402))+(((0.106)*x356)));
evalcond[3]=((-0.106)+(((-1.0)*x355*x379))+((sj1*x362))+((cj1*pz))+(((-1.0)*cj0*x360))+(((0.112)*x400))+(((-1.1e-7)*x356))+(((-1.0)*x401)));
evalcond[4]=((((-1.1e-7)*x399))+(((-1.0)*r01*sj1*x369))+(((-1.0)*sj5*x401))+((cj0*px*x372))+((r00*x360))+(((-1.0)*sj5*x355*x379))+((py*sj0*x372))+(((-1.0)*pz*x371*x388))+(((0.106)*x378))+(((0.106)*x374))+(((0.112)*sj3*x399))+(((-1.0)*cj0*x363*x376)));
evalcond[5]=((((-0.106)*x399))+(((-1.0)*r02*x360*x388))+((x374*x387))+(((-1.0)*r02*x364*x369))+((sj5*x402))+(((-1.0)*x369*x371))+((r00*x380))+(((-1.0)*x379*x399))+((x363*x364))+(((-0.112)*sj5*x393))+(((-1.1e-7)*x374))+(((-1.1e-7)*x378)));
evalcond[6]=((((-0.023744)*x394))+((cj1*x358*x366))+(((2.2e-7)*x367))+((x359*x384))+(((-1.0)*x362*x390))+(((-2.332e-8)*x372))+(((-1.0)*x359*x366*x376))+((x375*x395))+((x366*x395))+((x366*x391))+((x357*x384))+(((0.0013080000000121)*sj3*x397))+(((-1.0)*x384*x403))+(((-1.0)*sj1*x363*x392))+(((-1.0)*x366*x368))+(((-2.464e-8)*x397))+(((-1.0)*x367*x380*x382))+(((-2.332e-8)*sj1*x375))+(((-1.0)*cj3*x356*x370))+(((2.332e-8)*r01*x364))+((pz*x373))+(((-1.0)*x372*x377*x383))+(((-1.0)*x358*x375*x376))+(((0.212)*sj0*x363))+(((-1.0)*x359*x375*x376))+(((0.0112359999999879)*x384))+(((-1.0)*pz*x360*x361))+(((-1.0)*x365*x394))+((x362*x372*x377))+(((2.2e-7)*x389))+((x370*x393))+(((-1.0)*x375*x391))+((x383*x390))+((x361*x362*x380)));
evalcond[7]=((((-0.023744)*x397))+(((-1.0)*x373*x383))+(((-1.0)*x372*x403))+(((-2.2e-7)*sj0*x363))+((r01*x359*x364))+(((-1.0)*sj1*x375*x396))+((sj1*x358*x375))+((x360*x367*x382))+(((-0.0112359999999879)*x372))+(((-1.0)*x358*x364*x386))+(((-0.0112359999999879)*sj1*x375))+((x370*x400))+((cj3*x355*x370))+((x375*x398))+(((0.212)*x389))+((x359*x372))+(((-2.332e-8)*x384))+(((-1.0)*x360*x361*x362))+(((2.464e-8)*x394))+(((-1.0)*pz*x361*x380))+(((0.212)*x367))+(((-1.0)*cj1*x363*x392))+(((-1.0)*x365*x397))+(((-0.0013080000000121)*cj5*x393))+(((-1.0)*x357*x364*x386))+((x357*x372))+((x375*x385))+(((0.0112359999999879)*r01*x364))+((cj0*r02*x360*x377))+(((-1.0)*x366*x385))+((x362*x373))+((r02*x368))+(((2.2e-7)*pz*x366))+(((-1.0)*x362*x377*x384)));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
{
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 = j5;
vinfos[4].indices[0] = _ij5[0];
vinfos[4].indices[1] = _ij5[1];
vinfos[4].maxsolutions = _nj5;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x404=(cj3*r02);
IkReal x405=((1.0)*sj1);
IkReal x406=(r02*sj3);
IkReal x407=(r00*sj0*sj3);
IkReal x408=(cj3*r00*sj0);
IkReal x409=(cj0*cj3*r01);
IkReal x410=((1.0)*cj0*r01*sj3);
CheckValue<IkReal> x411 = IKatan2WithCheck(IkReal((((sj1*x408))+(((-1.0)*cj1*x410))+(((-1.0)*x405*x406))+(((-1.0)*x405*x409))+((cj1*x404))+((cj1*x407)))),IkReal((((sj1*x407))+((sj1*x404))+(((-1.0)*cj1*x408))+(((-1.0)*cj0*r01*sj3*x405))+((cj1*x406))+((cj1*x409)))),IKFAST_ATAN2_MAGTHRESH);
if(!x411.valid){
continue;
}
CheckValue<IkReal> x412=IKPowWithIntegerCheck(IKsign(cj5),-1);
if(!x412.valid){
continue;
}
j2array[0]=((-1.5707963267949)+(x411.value)+(((1.5707963267949)*(x412.value))));
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 x413=IKcos(j2);
IkReal x414=IKsin(j2);
IkReal x415=px*px;
IkReal x416=pz*pz;
IkReal x417=py*py;
IkReal x418=(py*sj1);
IkReal x419=((2.0)*r01);
IkReal x420=(px*sj0);
IkReal x421=(pz*r00);
IkReal x422=(cj0*sj1);
IkReal x423=((0.0237799999999879)*cj3);
IkReal x424=(cj0*r01);
IkReal x425=(px*r00);
IkReal x426=((0.212)*pz);
IkReal x427=((1.0)*px);
IkReal x428=((2.332e-8)*cj5);
IkReal x429=(cj1*r01);
IkReal x430=(cj1*r02);
IkReal x431=((2.2e-7)*r02);
IkReal x432=(r01*sj0);
IkReal x433=(r00*sj0);
IkReal x434=((1.0)*cj1);
IkReal x435=((2.0)*pz);
IkReal x436=(cj0*r00);
IkReal x437=((0.112)*cj3);
IkReal x438=(cj1*py);
IkReal x439=((1.0)*cj3);
IkReal x440=((2.0)*cj0);
IkReal x441=(cj0*py);
IkReal x442=(r02*sj1);
IkReal x443=((2.332e-8)*cj1);
IkReal x444=((1.0)*r01);
IkReal x445=(pz*sj1);
IkReal x446=((1.0)*sj0);
IkReal x447=(py*r01);
IkReal x448=((0.212)*r02);
IkReal x449=((0.0112359999999879)*cj1);
IkReal x450=((2.0)*px);
IkReal x451=(sj3*x413);
IkReal x452=(cj5*x413);
IkReal x453=(cj1*x415);
IkReal x454=((1.0)*x415);
IkReal x455=(cj5*x414);
IkReal x456=(sj1*x417);
IkReal x457=(sj5*x414);
IkReal x458=(sj3*x414);
IkReal x459=((0.106)*x413);
IkReal x460=((1.1e-7)*x413);
IkReal x461=((1.0)*x416);
evalcond[0]=(((sj3*x455))+(((-1.0)*x433*x434))+(((-1.0)*x439*x452))+x442+((cj1*x424)));
evalcond[1]=((((-1.0)*x439*x455))+(((-1.0)*cj5*x451))+x430+(((-1.0)*x422*x444))+((sj1*x433)));
evalcond[2]=((-1.1e-7)+((x414*x437))+(((-1.0)*x420*x434))+(((0.112)*x451))+(((-1.0)*x460))+((cj0*x438))+x445+(((0.106)*x414)));
evalcond[3]=((-0.106)+(((-1.0)*x413*x437))+(((-1.1e-7)*x414))+(((-1.0)*cj0*x418))+(((0.112)*x458))+((cj1*pz))+(((-1.0)*x459))+((sj1*x420)));
evalcond[4]=(((py*sj0*x430))+(((-1.0)*pz*x429*x446))+(((0.112)*sj3*x457))+(((-1.0)*r01*sj1*x427))+(((-1.0)*sj5*x459))+(((0.106)*x436))+(((0.106)*x432))+(((-1.0)*cj0*x421*x434))+((cj0*px*x430))+((r00*x418))+(((-1.1e-7)*x457))+(((-1.0)*sj5*x413*x437)));
evalcond[5]=((((-1.1e-7)*x436))+(((-1.1e-7)*x432))+(((-1.0)*x437*x457))+(((-1.0)*x427*x429))+(((-0.112)*sj5*x451))+((r00*x438))+(((-1.0)*r02*x418*x446))+((sj5*x460))+(((-0.106)*x457))+((x421*x422))+((x432*x445))+(((-1.0)*r02*x422*x427)));
evalcond[6]=((((-1.0)*x416*x433*x434))+((x419*x420*x438))+(((0.0112359999999879)*x442))+((x428*x451))+(((-1.0)*x430*x435*x441))+((x424*x449))+((x415*x442))+(((-1.0)*x423*x452))+(((-1.0)*sj1*x421*x450))+(((-1.0)*x433*x449))+(((2.332e-8)*r01*x422))+((x433*x453))+(((-1.0)*x420*x448))+(((-1.0)*cj3*x414*x428))+(((-1.0)*pz*x418*x419))+((pz*x431))+(((-2.332e-8)*sj1*x433))+((cj1*x416*x424))+((x424*x453))+(((-1.0)*x417*x424*x434))+((x441*x448))+(((-1.0)*x442*x461))+(((2.2e-7)*x425))+((x420*x430*x435))+(((-1.0)*x424*x426))+(((0.212)*sj0*x421))+(((-0.023744)*x452))+((x417*x442))+(((2.2e-7)*x447))+(((-1.0)*x417*x433*x434))+(((-1.0)*x425*x438*x440))+(((0.0013080000000121)*sj3*x455))+(((-2.464e-8)*x455))+(((-2.332e-8)*x430)));
evalcond[7]=(((x428*x458))+(((-2.332e-8)*x442))+(((-1.0)*x423*x455))+((x415*x430))+(((2.2e-7)*pz*x424))+((x433*x456))+(((2.464e-8)*x452))+((r01*x417*x422))+(((-1.0)*x418*x419*x420))+(((-1.0)*x431*x441))+(((-1.0)*x420*x435*x442))+((r02*x426))+(((-1.0)*pz*x419*x438))+(((0.212)*x425))+(((-1.0)*cj1*x421*x450))+(((0.212)*x447))+(((-1.0)*x424*x443))+((sj1*x416*x433))+((cj0*r02*x418*x435))+(((-2.2e-7)*sj0*x421))+((cj3*x413*x428))+(((-0.0112359999999879)*sj1*x433))+(((-0.023744)*x455))+(((-1.0)*sj1*x433*x454))+(((-1.0)*x415*x422*x444))+((x418*x425*x440))+(((-1.0)*x416*x422*x444))+((x420*x431))+((x433*x443))+((x417*x430))+(((-0.0112359999999879)*x430))+(((-1.0)*x430*x461))+(((-0.0013080000000121)*cj5*x451))+(((0.0112359999999879)*r01*x422)));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
{
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 = j5;
vinfos[4].indices[0] = _ij5[0];
vinfos[4].indices[1] = _ij5[1];
vinfos[4].maxsolutions = _nj5;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}
}
}

}

}
}
}
    }
}
}

}

}
}
return solutions.GetNumSolutions()>0;
}
static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int& numroots)
{
    using std::complex;
    if( rawcoeffs[0] == 0 ) {
        // solve with one reduced degree
        polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
        return;
    }
    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[3];
    const int maxsteps = 110;
    for(int i = 0; i < 3; ++i) {
        coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
    }
    complex<IkReal> roots[3];
    IkReal err[3];
    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 < 3; ++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 < 3; ++i) {
            if ( err[i] >= tol ) {
                changed = true;
                // evaluate
                complex<IkReal> x = roots[i] + coeffs[0];
                for(int j = 1; j < 3; ++j) {
                    x = roots[i] * x + coeffs[j];
                }
                for(int j = 0; j < 3; ++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[3] = {false};
    for(int i = 0; i < 3; ++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 < 3; ++j) {
                // care about error in real much more than imaginary
                if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
                    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);
            }
        }
    }
}
static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int& numroots) {
    IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
    if( det < 0 ) {
        numroots=0;
    }
    else if( det == 0 ) {
        rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
        numroots = 1;
    }
    else {
        det = IKsqrt(det);
        rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
        rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);//rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
        numroots = 2;
    }
}
static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
{
    using std::complex;
    if( rawcoeffs[0] == 0 ) {
        // solve with one reduced degree
        polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
        return;
    }
    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) {
                // care about error in real much more than imaginary
                if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
                    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 bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip) {
IKSolver solver;
return solver.ComputeIk(eetrans,eerot,pfree,solutions);
}

IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - turtlebot_arm (3560475dd424b0fa84bb2c480a84c0dc)>"; }

IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; }

#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