openrave_ik.cpp

Go to the documentation of this file.

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


#include <coverage_3d_arm_navigation/openrave_ik.h>


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

#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 std::isnan
#define std::isnan _std::isnan
#endif
#endif // _MSC_VER

// defined when creating a shared object/dll
#ifdef IKFAST_CLIBRARY
#ifdef _MSC_VER
#define IKFAST_API extern "C" __declspec(dllexport)
#else
#define IKFAST_API extern "C"
#endif
#else
#define IKFAST_API
#endif

// 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

    void IKSolution::GetSolution(IKReal* psolution, const IKReal* pfree) const {
        for(std::size_t i = 0; i < basesol.size(); ++i) {
            if( basesol[i].freeind < 0 )
                psolution[i] = basesol[i].foffset;
            else {
                IKFAST_ASSERT(pfree != NULL);
                psolution[i] = pfree[basesol[i].freeind]*basesol[i].fmul + basesol[i].foffset;
                if( psolution[i] > IKPI ) {
                    psolution[i] -= IK2PI;
                }
                else if( psolution[i] < -IKPI ) {
                    psolution[i] += IK2PI;
                }
            }
        }
    }

    const std::vector<int>& IKSolution::GetFree() const { return vfree; }

    bool IKSolution::Validate() const {
        for(size_t i = 0; i < basesol.size(); ++i) {
            if( basesol[i].maxsolutions == (unsigned char)-1) {
                return false;
            }
            if( basesol[i].maxsolutions > 0 ) {
                if( basesol[i].indices[0] >= basesol[i].maxsolutions ) {
                    return false;
                }
                if( basesol[i].indices[1] != (unsigned char)-1 && basesol[i].indices[1] >= basesol[i].maxsolutions ) {
                    return false;
                }
            }
        }
        return true;
    }

    void IKSolution::GetSolutionIndices(std::vector<unsigned int>& v) const {
        v.resize(0);
        v.push_back(0);
        for(int i = (int)basesol.size()-1; i >= 0; --i) {
            if( basesol[i].maxsolutions != (unsigned char)-1 && basesol[i].maxsolutions > 1 ) {
                for(size_t j = 0; j < v.size(); ++j) {
                    v[j] *= basesol[i].maxsolutions;
                }
                size_t orgsize=v.size();
                if( basesol[i].indices[1] != (unsigned char)-1 ) {
                    for(size_t j = 0; j < orgsize; ++j) {
                        v.push_back(v[j]+basesol[i].indices[1]);
                    }
                }
                if( basesol[i].indices[0] != (unsigned char)-1 ) {
                    for(size_t j = 0; j < orgsize; ++j) {
                        v[j] += basesol[i].indices[0];
                    }
                }
            }
        }
    }

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

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

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

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

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

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

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

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

IKFAST_API void fk(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,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46,x47,x48,x49,x50,x51,x52,x53,x54,x55,x56,x57,x58,x59,x60,x61,x62,x63,x64,x65,x66,x67,x68;
x0=IKcos(j[0]);
x1=IKsin(j[1]);
x2=IKsin(j[2]);
x3=IKcos(j[2]);
x4=IKsin(j[0]);
x5=IKsin(j[4]);
x6=((x0)*(x1)*(x2));
x7=((x3)*(x4));
x8=((x6)+(((-1.00000000000000)*(x7))));
x9=IKcos(j[4]);
x10=IKcos(j[3]);
x11=((x2)*(x4));
x12=((x0)*(x1)*(x3));
x13=((x11)+(x12));
x14=((-1.00000000000000)*(x13));
x15=IKcos(j[1]);
x16=IKsin(j[3]);
x17=((x0)*(x15)*(x16));
x18=IKsin(j[6]);
x19=IKcos(j[5]);
x20=((x5)*(x8));
x21=((x10)*(x14));
x22=IKsin(j[5]);
x23=((-1.00000000000000)*(x14)*(x16));
x24=((-1.00000000000000)*(x0)*(x10)*(x15));
x25=((x24)+(x23));
x26=IKcos(j[6]);
x27=((x8)*(x9));
x28=((-1.00000000000000)*(x17));
x29=((x21)+(x28));
x30=((x29)*(x9));
x31=((x30)+(x20));
x32=((((-1.00000000000000)*(x17)))+(x21));
x33=((x0)*(x2));
x34=((x1)*(x7));
x35=((((-1.00000000000000)*(x34)))+(x33));
x36=((x0)*(x3));
x37=((x1)*(x11));
x38=((x37)+(x36));
x39=((x10)*(x35));
x40=((x38)*(x9));
x41=((x15)*(x16)*(x4));
x42=((((-1.00000000000000)*(x39)))+(x41));
x43=((x42)*(x5));
x44=((x43)+(x40));
x45=((-1.00000000000000)*(x41));
x46=((x39)+(x45));
x47=((x46)*(x9));
x48=((x38)*(x5));
x49=((x48)+(x47));
x50=((-1.00000000000000)*(x10)*(x15)*(x4));
x51=((-1.00000000000000)*(x16)*(x35));
x52=((x51)+(x50));
x53=((x1)*(x16));
x54=((x10)*(x15)*(x3));
x55=((x15)*(x2)*(x9));
x56=((x54)+(((-1.00000000000000)*(x53))));
x57=((x5)*(x56));
x58=((x55)+(x57));
x59=((x15)*(x16)*(x3));
x60=((x1)*(x10));
x61=((x59)+(x60));
x62=((x22)*(x61));
x63=((x53)+(((-1.00000000000000)*(x54))));
x64=((x63)*(x9));
x65=((x15)*(x2)*(x5));
x66=((x64)+(x65));
x67=((x19)*(x66));
x68=((x62)+(x67));
eerot[0]=((((x26)*(((((x19)*(((((x32)*(x9)))+(x20)))))+(((x22)*(x25)))))))+(((x18)*(((x27)+(((x5)*(((((-1.00000000000000)*(x21)))+(x17))))))))));
eerot[1]=((((x18)*(((((-1.00000000000000)*(x22)*(x25)))+(((-1.00000000000000)*(x19)*(x31)))))))+(((x26)*(((x27)+(((x5)*(((((-1.00000000000000)*(x21)))+(x17))))))))));
eerot[2]=((((x22)*(x31)))+(((x19)*(((((x0)*(x10)*(x15)))+(((x14)*(x16))))))));
eetrans[0]=((((x16)*(((((-0.321000000000000)*(x12)))+(((-0.321000000000000)*(x11)))))))+(((0.321000000000000)*(x0)*(x10)*(x15)))+(((0.400000000000000)*(x0)*(x15)))+(((x22)*(((((0.180000000000000)*(x20)))+(((0.180000000000000)*(x32)*(x9)))))))+(((x19)*(((((0.180000000000000)*(x0)*(x10)*(x15)))+(((0.180000000000000)*(x14)*(x16)))))))+(((0.100000000000000)*(x0))));
eerot[3]=((((x18)*(x44)))+(((x26)*(((((x19)*(x49)))+(((x22)*(x52))))))));
eerot[4]=((((x26)*(x44)))+(((x18)*(((((-1.00000000000000)*(x22)*(x52)))+(((-1.00000000000000)*(x19)*(x49))))))));
eerot[5]=((((x22)*(x49)))+(((x19)*(((((x16)*(x35)))+(((x10)*(x15)*(x4))))))));
eetrans[1]=((-0.188000000000000)+(((x16)*(((((-0.321000000000000)*(x34)))+(((0.321000000000000)*(x33)))))))+(((x19)*(((((0.180000000000000)*(x10)*(x15)*(x4)))+(((0.180000000000000)*(x16)*(x35)))))))+(((0.400000000000000)*(x15)*(x4)))+(((0.100000000000000)*(x4)))+(((x22)*(((((0.180000000000000)*(x47)))+(((0.180000000000000)*(x48)))))))+(((0.321000000000000)*(x10)*(x15)*(x4))));
eerot[6]=((((x26)*(x68)))+(((x18)*(x58))));
eerot[7]=((((x26)*(x58)))+(((-1.00000000000000)*(x18)*(x68))));
eerot[8]=((((-1.00000000000000)*(x19)*(x61)))+(((x22)*(x66))));
eetrans[2]=((((x22)*(((((0.180000000000000)*(x65)))+(((0.180000000000000)*(x64)))))))+(((x19)*(((((-0.180000000000000)*(x60)))+(((-0.180000000000000)*(x59)))))))+(((-0.321000000000000)*(x59)))+(((-0.321000000000000)*(x60)))+(((-0.400000000000000)*(x1))));
}

IKFAST_API int getNumFreeParameters() { return 1; }
IKFAST_API int* getFreeParameters() { static int freeparams[] = {2}; return freeparams; }
IKFAST_API int getNumJoints() { return 7; }

IKFAST_API int getIKRealSize() { return sizeof(IKReal); }

IKFAST_API int getIKType() { return 0x67000001; }

class IKSolver {
public:
IKReal j27,cj27,sj27,htj27,j28,cj28,sj28,htj28,j30,cj30,sj30,htj30,j31,cj31,sj31,htj31,j32,cj32,sj32,htj32,j33,cj33,sj33,htj33,j29,cj29,sj29,htj29,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
unsigned char _ij27[2], _nj27,_ij28[2], _nj28,_ij30[2], _nj30,_ij31[2], _nj31,_ij32[2], _nj32,_ij33[2], _nj33,_ij29[2], _nj29;

bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
j27=numeric_limits<IKReal>::quiet_NaN(); _ij27[0] = -1; _ij27[1] = -1; _nj27 = -1; j28=numeric_limits<IKReal>::quiet_NaN(); _ij28[0] = -1; _ij28[1] = -1; _nj28 = -1; j30=numeric_limits<IKReal>::quiet_NaN(); _ij30[0] = -1; _ij30[1] = -1; _nj30 = -1; j31=numeric_limits<IKReal>::quiet_NaN(); _ij31[0] = -1; _ij31[1] = -1; _nj31 = -1; j32=numeric_limits<IKReal>::quiet_NaN(); _ij32[0] = -1; _ij32[1] = -1; _nj32 = -1; j33=numeric_limits<IKReal>::quiet_NaN(); _ij33[0] = -1; _ij33[1] = -1; _nj33 = -1;  _ij29[0] = -1; _ij29[1] = -1; _nj29 = 0; 
for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
    vsolutions.resize(0); vsolutions.reserve(8);
j29=pfree[0]; cj29=cos(pfree[0]); sj29=sin(pfree[0]);
r00 = eerot[0*3+0];
r01 = eerot[0*3+1];
r02 = eerot[0*3+2];
r10 = eerot[1*3+0];
r11 = eerot[1*3+1];
r12 = eerot[1*3+2];
r20 = eerot[2*3+0];
r21 = eerot[2*3+1];
r22 = eerot[2*3+2];
px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];

new_r00=r00;
new_r01=r01;
new_r02=r02;
new_px=((((-0.180000000000000)*(r02)))+(px));
new_r10=r10;
new_r11=r11;
new_r12=r12;
new_py=((0.188000000000000)+(py)+(((-0.180000000000000)*(r12))));
new_r20=r20;
new_r21=r21;
new_r22=r22;
new_pz=((((-0.180000000000000)*(r22)))+(pz));
r00 = new_r00; r01 = new_r01; r02 = new_r02; r10 = new_r10; r11 = new_r11; r12 = new_r12; r20 = new_r20; r21 = new_r21; r22 = new_r22; px = new_px; py = new_py; pz = new_pz;
pp=(((px)*(px))+((py)*(py))+((pz)*(pz)));
npx=((((px)*(r00)))+(((py)*(r10)))+(((pz)*(r20))));
npy=((((px)*(r01)))+(((py)*(r11)))+(((pz)*(r21))));
npz=((((px)*(r02)))+(((py)*(r12)))+(((pz)*(r22))));
rxp0_0=((((-1.00000000000000)*(py)*(r20)))+(((pz)*(r10))));
rxp0_1=((((px)*(r20)))+(((-1.00000000000000)*(pz)*(r00))));
rxp0_2=((((-1.00000000000000)*(px)*(r10)))+(((py)*(r00))));
rxp1_0=((((-1.00000000000000)*(py)*(r21)))+(((pz)*(r11))));
rxp1_1=((((px)*(r21)))+(((-1.00000000000000)*(pz)*(r01))));
rxp1_2=((((-1.00000000000000)*(px)*(r11)))+(((py)*(r01))));
rxp2_0=((((-1.00000000000000)*(py)*(r22)))+(((pz)*(r12))));
rxp2_1=((((px)*(r22)))+(((-1.00000000000000)*(pz)*(r02))));
rxp2_2=((((-1.00000000000000)*(px)*(r12)))+(((py)*(r02))));
{
IKReal dummyeval[1];
IKReal gconst0;
gconst0=((-1.00000000000000)*(py));
IKReal gconst1;
gconst1=((0.642000000000000)*(sj29));
IKReal gconst2;
gconst2=((-1.00000000000000)*(py));
IKReal gconst3;
gconst3=((0.509841000000000)+(((0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
IKReal gconst4;
gconst4=((-0.00375900000000000)+(((0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
IKReal gconst5;
gconst5=((-1.00000000000000)*(py));
IKReal gconst6;
gconst6=((0.642000000000000)*(sj29));
IKReal gconst7;
gconst7=((-1.00000000000000)*(py));
IKReal gconst8;
gconst8=((0.509841000000000)+(((0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
IKReal gconst9;
gconst9=((-0.00375900000000000)+(((0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
IKReal gconst10;
gconst10=((2.00000000000000)*(px));
IKReal gconst11;
gconst11=((2.00000000000000)*(px));
IKReal gconst12;
gconst12=((0.400000000000000)*(py));
IKReal gconst13;
gconst13=((0.400000000000000)*(py));
IKReal gconst14;
gconst14=((2.00000000000000)*(px));
IKReal gconst15;
gconst15=((2.00000000000000)*(px));
IKReal gconst16;
gconst16=((0.400000000000000)*(py));
IKReal gconst17;
gconst17=((0.400000000000000)*(py));
IKReal gconst18;
gconst18=py;
IKReal gconst19;
gconst19=((0.642000000000000)*(sj29));
IKReal gconst20;
gconst20=py;
IKReal gconst21;
gconst21=((0.509841000000000)+(((-0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
IKReal gconst22;
gconst22=((-0.00375900000000000)+(((-0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
IKReal gconst23;
gconst23=py;
IKReal gconst24;
gconst24=((0.642000000000000)*(sj29));
IKReal gconst25;
gconst25=py;
IKReal gconst26;
gconst26=((0.509841000000000)+(((-0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
IKReal gconst27;
gconst27=((-0.00375900000000000)+(((-0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
dummyeval[0]=((((-1.00000000000000)*(gconst19)*(gconst22)*(gconst24)*(gconst26)))+(((gconst20)*(gconst21)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst23)*(gconst27)))+(((gconst18)*(gconst22)*(gconst25)*(gconst26)))+(((-1.00000000000000)*(gconst20)*(gconst21)*(gconst25)*(gconst26))));
if( IKabs(dummyeval[0]) < 0.0000001000000000  )
{
continue;

} else
{
IKReal op[8+1], zeror[8];
int numroots;
op[0]=((((-1.00000000000000)*(gconst19)*(gconst22)*(gconst24)*(gconst26)))+(((gconst20)*(gconst21)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst23)*(gconst27)))+(((gconst18)*(gconst22)*(gconst25)*(gconst26)))+(((-1.00000000000000)*(gconst20)*(gconst21)*(gconst25)*(gconst26))));
op[1]=((((-1.00000000000000)*(gconst13)*(gconst18)*(gconst23)*(gconst27)))+(((gconst10)*(gconst22)*(gconst25)*(gconst26)))+(((gconst12)*(gconst20)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst17)*(gconst18)*(gconst22)*(gconst23)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst21)*(gconst26)))+(((gconst16)*(gconst18)*(gconst22)*(gconst25)))+(((gconst15)*(gconst18)*(gconst22)*(gconst26)))+(((-1.00000000000000)*(gconst16)*(gconst20)*(gconst21)*(gconst25)))+(((-1.00000000000000)*(gconst12)*(gconst20)*(gconst25)*(gconst26)))+(((gconst11)*(gconst21)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst10)*(gconst22)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst16)*(gconst19)*(gconst22)*(gconst24)))+(((gconst13)*(gconst18)*(gconst25)*(gconst26)))+(((gconst14)*(gconst20)*(gconst21)*(gconst27)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst24)*(gconst26)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst22)*(gconst27)))+(((-1.00000000000000)*(gconst11)*(gconst21)*(gconst25)*(gconst26)))+(((gconst17)*(gconst20)*(gconst21)*(gconst23))));
op[2]=((((gconst2)*(gconst21)*(gconst23)*(gconst27)))+(((gconst20)*(gconst21)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst16)*(gconst20)*(gconst21)))+(((-1.00000000000000)*(gconst19)*(gconst22)*(gconst24)*(gconst8)))+(((gconst20)*(gconst21)*(gconst23)*(gconst9)))+(((gconst20)*(gconst23)*(gconst27)*(gconst3)))+(((gconst11)*(gconst17)*(gconst21)*(gconst23)))+(((-1.00000000000000)*(gconst14)*(gconst17)*(gconst18)*(gconst22)))+(((gconst13)*(gconst16)*(gconst18)*(gconst25)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst23)*(gconst27)))+(((gconst11)*(gconst12)*(gconst23)*(gconst27)))+(((gconst10)*(gconst15)*(gconst22)*(gconst26)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst25)*(gconst26)))+(((-1.00000000000000)*(gconst18)*(gconst23)*(gconst27)*(gconst4)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst23)*(gconst9)))+(((gconst14)*(gconst17)*(gconst20)*(gconst21)))+(((-1.00000000000000)*(gconst19)*(gconst24)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst13)*(gconst14)*(gconst18)*(gconst27)))+(((gconst18)*(gconst22)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst20)*(gconst25)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst18)*(gconst23)))+(((-1.00000000000000)*(gconst11)*(gconst16)*(gconst21)*(gconst25)))+(((-1.00000000000000)*(gconst0)*(gconst22)*(gconst23)*(gconst27)))+(((gconst12)*(gconst17)*(gconst20)*(gconst23)))+(((gconst18)*(gconst22)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst12)*(gconst16)*(gconst20)*(gconst25)))+(((-1.00000000000000)*(gconst13)*(gconst16)*(gconst19)*(gconst24)))+(((gconst0)*(gconst22)*(gconst25)*(gconst26)))+(((-1.00000000000000)*(gconst20)*(gconst21)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst27)*(gconst5)))+(((gconst10)*(gconst13)*(gconst25)*(gconst26)))+(((gconst11)*(gconst14)*(gconst21)*(gconst27)))+(((gconst12)*(gconst14)*(gconst20)*(gconst27)))+(((-1.00000000000000)*(gconst12)*(gconst15)*(gconst20)*(gconst26)))+(((-1.00000000000000)*(gconst11)*(gconst15)*(gconst21)*(gconst26)))+(((gconst10)*(gconst16)*(gconst22)*(gconst25)))+(((-1.00000000000000)*(gconst20)*(gconst21)*(gconst25)*(gconst8)))+(((gconst15)*(gconst16)*(gconst18)*(gconst22)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst22)*(gconst27)))+(((gconst13)*(gconst15)*(gconst18)*(gconst26)))+(((gconst18)*(gconst25)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst10)*(gconst17)*(gconst22)*(gconst23)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst25)*(gconst26)))+(((-1.00000000000000)*(gconst19)*(gconst22)*(gconst26)*(gconst6)))+(((-1.00000000000000)*(gconst1)*(gconst22)*(gconst24)*(gconst26))));
op[3]=((((-1.00000000000000)*(gconst11)*(gconst12)*(gconst16)*(gconst25)))+(((gconst16)*(gconst18)*(gconst25)*(gconst4)))+(((gconst15)*(gconst18)*(gconst22)*(gconst8)))+(((gconst11)*(gconst12)*(gconst14)*(gconst27)))+(((gconst0)*(gconst16)*(gconst22)*(gconst25)))+(((-1.00000000000000)*(gconst0)*(gconst14)*(gconst22)*(gconst27)))+(((gconst0)*(gconst15)*(gconst22)*(gconst26)))+(((-1.00000000000000)*(gconst16)*(gconst20)*(gconst21)*(gconst7)))+(((gconst14)*(gconst2)*(gconst21)*(gconst27)))+(((gconst10)*(gconst13)*(gconst15)*(gconst26)))+(((-1.00000000000000)*(gconst11)*(gconst25)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst12)*(gconst15)*(gconst16)*(gconst20)))+(((gconst11)*(gconst23)*(gconst27)*(gconst3)))+(((-1.00000000000000)*(gconst17)*(gconst18)*(gconst22)*(gconst5)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst22)*(gconst23)))+(((-1.00000000000000)*(gconst16)*(gconst19)*(gconst22)*(gconst6)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst11)*(gconst15)*(gconst16)*(gconst21)))+(((gconst11)*(gconst14)*(gconst17)*(gconst21)))+(((-1.00000000000000)*(gconst12)*(gconst2)*(gconst25)*(gconst26)))+(((-1.00000000000000)*(gconst16)*(gconst19)*(gconst24)*(gconst4)))+(((-1.00000000000000)*(gconst12)*(gconst20)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst21)*(gconst25)))+(((gconst12)*(gconst14)*(gconst17)*(gconst20)))+(((-1.00000000000000)*(gconst13)*(gconst18)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst12)*(gconst20)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst10)*(gconst22)*(gconst23)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst18)*(gconst23)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst14)*(gconst17)*(gconst18)))+(((gconst11)*(gconst21)*(gconst23)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst17)*(gconst22)))+(((gconst14)*(gconst20)*(gconst21)*(gconst9)))+(((gconst16)*(gconst18)*(gconst22)*(gconst7)))+(((gconst12)*(gconst20)*(gconst23)*(gconst9)))+(((gconst13)*(gconst18)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst26)*(gconst6)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst14)*(gconst27)))+(((-1.00000000000000)*(gconst11)*(gconst21)*(gconst25)*(gconst8)))+(((gconst10)*(gconst25)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst10)*(gconst23)*(gconst27)*(gconst4)))+(((gconst14)*(gconst20)*(gconst27)*(gconst3)))+(((gconst15)*(gconst18)*(gconst26)*(gconst4)))+(((gconst13)*(gconst15)*(gconst16)*(gconst18)))+(((gconst10)*(gconst22)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst1)*(gconst16)*(gconst22)*(gconst24)))+(((-1.00000000000000)*(gconst10)*(gconst22)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst27)*(gconst4)))+(((-1.00000000000000)*(gconst1)*(gconst13)*(gconst24)*(gconst26)))+(((gconst11)*(gconst12)*(gconst17)*(gconst23)))+(((gconst12)*(gconst2)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst15)*(gconst26)))+(((gconst17)*(gconst20)*(gconst23)*(gconst3)))+(((gconst17)*(gconst20)*(gconst21)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst21)*(gconst8)))+(((gconst10)*(gconst15)*(gconst16)*(gconst22)))+(((gconst11)*(gconst21)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst22)*(gconst9)))+(((-1.00000000000000)*(gconst17)*(gconst18)*(gconst23)*(gconst4)))+(((gconst13)*(gconst18)*(gconst26)*(gconst7)))+(((gconst17)*(gconst2)*(gconst21)*(gconst23)))+(((gconst0)*(gconst13)*(gconst25)*(gconst26)))+(((gconst10)*(gconst22)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst21)*(gconst26)*(gconst7)))+(((gconst12)*(gconst20)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst21)*(gconst26)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst17)*(gconst23)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst24)*(gconst8)))+(((gconst10)*(gconst13)*(gconst16)*(gconst25)))+(((-1.00000000000000)*(gconst16)*(gconst20)*(gconst25)*(gconst3))));
op[4]=((((gconst12)*(gconst14)*(gconst2)*(gconst27)))+(((gconst10)*(gconst15)*(gconst22)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst14)*(gconst27)))+(((-1.00000000000000)*(gconst20)*(gconst21)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst27)*(gconst5)))+(((gconst2)*(gconst23)*(gconst27)*(gconst3)))+(((-1.00000000000000)*(gconst15)*(gconst16)*(gconst2)*(gconst21)))+(((gconst18)*(gconst22)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst12)*(gconst15)*(gconst20)*(gconst8)))+(((-1.00000000000000)*(gconst12)*(gconst15)*(gconst2)*(gconst26)))+(((gconst14)*(gconst17)*(gconst2)*(gconst21)))+(((gconst20)*(gconst27)*(gconst3)*(gconst5)))+(((gconst11)*(gconst12)*(gconst14)*(gconst17)))+(((-1.00000000000000)*(gconst10)*(gconst17)*(gconst22)*(gconst5)))+(((-1.00000000000000)*(gconst19)*(gconst24)*(gconst4)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst13)*(gconst16)*(gconst19)*(gconst6)))+(((gconst10)*(gconst16)*(gconst22)*(gconst7)))+(((gconst13)*(gconst15)*(gconst18)*(gconst8)))+(((gconst12)*(gconst17)*(gconst2)*(gconst23)))+(((gconst10)*(gconst13)*(gconst15)*(gconst16)))+(((gconst11)*(gconst14)*(gconst27)*(gconst3)))+(((gconst18)*(gconst25)*(gconst4)*(gconst8)))+(((-1.00000000000000)*(gconst1)*(gconst24)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst20)*(gconst25)*(gconst3)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst16)*(gconst25)*(gconst3)))+(((gconst0)*(gconst13)*(gconst16)*(gconst25)))+(((gconst13)*(gconst16)*(gconst18)*(gconst7)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst17)*(gconst23)))+(((-1.00000000000000)*(gconst1)*(gconst13)*(gconst16)*(gconst24)))+(((gconst11)*(gconst12)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst0)*(gconst23)*(gconst27)*(gconst4)))+(((-1.00000000000000)*(gconst1)*(gconst22)*(gconst24)*(gconst8)))+(((-1.00000000000000)*(gconst13)*(gconst14)*(gconst18)*(gconst9)))+(((-1.00000000000000)*(gconst18)*(gconst27)*(gconst4)*(gconst5)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst5)*(gconst9)))+(((gconst10)*(gconst13)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst15)*(gconst16)*(gconst20)*(gconst3)))+(((-1.00000000000000)*(gconst19)*(gconst22)*(gconst6)*(gconst8)))+(((gconst10)*(gconst13)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst18)*(gconst23)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst15)*(gconst26)*(gconst3)))+(((gconst0)*(gconst13)*(gconst15)*(gconst26)))+(((gconst2)*(gconst21)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst0)*(gconst22)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst2)*(gconst25)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst14)*(gconst17)))+(((gconst2)*(gconst21)*(gconst23)*(gconst9)))+(((gconst12)*(gconst14)*(gconst20)*(gconst9)))+(((-1.00000000000000)*(gconst14)*(gconst17)*(gconst18)*(gconst4)))+(((gconst18)*(gconst26)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst18)*(gconst5)))+(((-1.00000000000000)*(gconst19)*(gconst26)*(gconst4)*(gconst6)))+(((gconst20)*(gconst21)*(gconst5)*(gconst9)))+(((gconst11)*(gconst12)*(gconst23)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst16)*(gconst21)*(gconst7)))+(((-1.00000000000000)*(gconst20)*(gconst26)*(gconst3)*(gconst7)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst22)*(gconst9)))+(((gconst10)*(gconst15)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst12)*(gconst16)*(gconst2)*(gconst25)))+(((gconst11)*(gconst17)*(gconst23)*(gconst3)))+(((-1.00000000000000)*(gconst0)*(gconst14)*(gconst17)*(gconst22)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst15)*(gconst16)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst23)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst15)*(gconst21)*(gconst8)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst25)*(gconst8)))+(((gconst0)*(gconst15)*(gconst16)*(gconst22)))+(((gconst0)*(gconst25)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst10)*(gconst17)*(gconst23)*(gconst4)))+(((gconst0)*(gconst22)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst26)*(gconst7)))+(((gconst11)*(gconst14)*(gconst21)*(gconst9)))+(((gconst14)*(gconst17)*(gconst20)*(gconst3)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst27)*(gconst4)))+(((gconst12)*(gconst17)*(gconst20)*(gconst5)))+(((gconst11)*(gconst17)*(gconst21)*(gconst5)))+(((gconst15)*(gconst16)*(gconst18)*(gconst4)))+(((gconst0)*(gconst22)*(gconst26)*(gconst7)))+(((gconst20)*(gconst23)*(gconst3)*(gconst9)))+(((-1.00000000000000)*(gconst12)*(gconst16)*(gconst20)*(gconst7)))+(((-1.00000000000000)*(gconst1)*(gconst22)*(gconst26)*(gconst6)))+(((-1.00000000000000)*(gconst0)*(gconst22)*(gconst23)*(gconst9)))+(((gconst10)*(gconst16)*(gconst25)*(gconst4))));
op[5]=((((-1.00000000000000)*(gconst0)*(gconst13)*(gconst14)*(gconst17)))+(((-1.00000000000000)*(gconst10)*(gconst22)*(gconst5)*(gconst9)))+(((gconst0)*(gconst13)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst26)*(gconst3)*(gconst7)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst6)*(gconst8)))+(((-1.00000000000000)*(gconst12)*(gconst15)*(gconst16)*(gconst2)))+(((gconst17)*(gconst20)*(gconst3)*(gconst5)))+(((-1.00000000000000)*(gconst12)*(gconst2)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst16)*(gconst7)))+(((gconst10)*(gconst15)*(gconst16)*(gconst4)))+(((-1.00000000000000)*(gconst16)*(gconst20)*(gconst3)*(gconst7)))+(((gconst12)*(gconst20)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst23)*(gconst4)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst4)*(gconst9)))+(((gconst11)*(gconst14)*(gconst17)*(gconst3)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst25)*(gconst3)))+(((gconst0)*(gconst15)*(gconst22)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst22)*(gconst5)))+(((-1.00000000000000)*(gconst16)*(gconst19)*(gconst4)*(gconst6)))+(((gconst10)*(gconst25)*(gconst4)*(gconst8)))+(((-1.00000000000000)*(gconst12)*(gconst20)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst14)*(gconst27)*(gconst4)))+(((gconst17)*(gconst2)*(gconst23)*(gconst3)))+(((gconst0)*(gconst16)*(gconst25)*(gconst4)))+(((-1.00000000000000)*(gconst1)*(gconst13)*(gconst24)*(gconst8)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst17)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst27)*(gconst4)*(gconst5)))+(((gconst10)*(gconst26)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst25)*(gconst3)*(gconst8)))+(((gconst14)*(gconst2)*(gconst21)*(gconst9)))+(((gconst17)*(gconst2)*(gconst21)*(gconst5)))+(((gconst12)*(gconst14)*(gconst17)*(gconst2)))+(((gconst11)*(gconst23)*(gconst3)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst14)*(gconst22)*(gconst9)))+(((gconst10)*(gconst13)*(gconst16)*(gconst7)))+(((gconst11)*(gconst12)*(gconst17)*(gconst5)))+(((-1.00000000000000)*(gconst1)*(gconst13)*(gconst26)*(gconst6)))+(((gconst12)*(gconst2)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst3)*(gconst8)))+(((gconst11)*(gconst21)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst14)*(gconst9)))+(((-1.00000000000000)*(gconst12)*(gconst2)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst26)*(gconst3)))+(((gconst0)*(gconst13)*(gconst15)*(gconst16)))+(((gconst14)*(gconst20)*(gconst3)*(gconst9)))+(((gconst16)*(gconst18)*(gconst4)*(gconst7)))+(((gconst12)*(gconst2)*(gconst23)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst21)*(gconst7)*(gconst8)))+(((gconst10)*(gconst22)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst17)*(gconst4)))+(((-1.00000000000000)*(gconst13)*(gconst18)*(gconst5)*(gconst9)))+(((gconst11)*(gconst12)*(gconst14)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst23)*(gconst4)*(gconst9)))+(((gconst13)*(gconst18)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst23)*(gconst9)))+(((gconst10)*(gconst13)*(gconst15)*(gconst8)))+(((gconst14)*(gconst2)*(gconst27)*(gconst3)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst21)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst15)*(gconst16)*(gconst3)))+(((-1.00000000000000)*(gconst1)*(gconst16)*(gconst24)*(gconst4)))+(((gconst0)*(gconst15)*(gconst26)*(gconst4)))+(((gconst15)*(gconst18)*(gconst4)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst27)*(gconst5)))+(((gconst0)*(gconst13)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst15)*(gconst8)))+(((gconst11)*(gconst27)*(gconst3)*(gconst5)))+(((-1.00000000000000)*(gconst17)*(gconst18)*(gconst4)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst21)*(gconst8)))+(((gconst0)*(gconst16)*(gconst22)*(gconst7)))+(((-1.00000000000000)*(gconst1)*(gconst16)*(gconst22)*(gconst6))));
op[6]=((((-1.00000000000000)*(gconst1)*(gconst13)*(gconst16)*(gconst6)))+(((gconst0)*(gconst13)*(gconst15)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst27)*(gconst4)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst17)*(gconst4)*(gconst5)))+(((gconst14)*(gconst17)*(gconst2)*(gconst3)))+(((-1.00000000000000)*(gconst2)*(gconst26)*(gconst3)*(gconst7)))+(((gconst2)*(gconst23)*(gconst3)*(gconst9)))+(((gconst10)*(gconst13)*(gconst7)*(gconst8)))+(((gconst11)*(gconst12)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst24)*(gconst4)*(gconst8)))+(((-1.00000000000000)*(gconst20)*(gconst3)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst1)*(gconst26)*(gconst4)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst15)*(gconst3)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst22)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst14)*(gconst17)*(gconst4)))+(((gconst20)*(gconst3)*(gconst5)*(gconst9)))+(((gconst2)*(gconst27)*(gconst3)*(gconst5)))+(((gconst10)*(gconst16)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst19)*(gconst4)*(gconst6)*(gconst8)))+(((gconst0)*(gconst22)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst17)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst25)*(gconst3)*(gconst8)))+(((gconst11)*(gconst17)*(gconst3)*(gconst5)))+(((gconst0)*(gconst13)*(gconst16)*(gconst7)))+(((-1.00000000000000)*(gconst15)*(gconst16)*(gconst2)*(gconst3)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst4)*(gconst9)))+(((gconst0)*(gconst15)*(gconst16)*(gconst4)))+(((-1.00000000000000)*(gconst0)*(gconst23)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst18)*(gconst4)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst12)*(gconst16)*(gconst2)*(gconst7)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst7)*(gconst8)))+(((gconst0)*(gconst25)*(gconst4)*(gconst8)))+(((gconst12)*(gconst14)*(gconst2)*(gconst9)))+(((gconst18)*(gconst4)*(gconst7)*(gconst8)))+(((gconst11)*(gconst14)*(gconst3)*(gconst9)))+(((gconst10)*(gconst15)*(gconst4)*(gconst8)))+(((gconst2)*(gconst21)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst22)*(gconst6)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst16)*(gconst3)*(gconst7)))+(((gconst0)*(gconst26)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst14)*(gconst9)))+(((-1.00000000000000)*(gconst12)*(gconst15)*(gconst2)*(gconst8)))+(((gconst12)*(gconst17)*(gconst2)*(gconst5))));
op[7]=((((gconst0)*(gconst13)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst12)*(gconst2)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst3)*(gconst7)))+(((-1.00000000000000)*(gconst1)*(gconst13)*(gconst6)*(gconst8)))+(((gconst0)*(gconst15)*(gconst4)*(gconst8)))+(((gconst12)*(gconst2)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst4)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst3)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst3)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst5)*(gconst9)))+(((gconst17)*(gconst2)*(gconst3)*(gconst5)))+(((-1.00000000000000)*(gconst1)*(gconst16)*(gconst4)*(gconst6)))+(((gconst14)*(gconst2)*(gconst3)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst14)*(gconst4)*(gconst9)))+(((gconst10)*(gconst4)*(gconst7)*(gconst8)))+(((gconst11)*(gconst3)*(gconst5)*(gconst9)))+(((gconst0)*(gconst16)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst4)*(gconst5))));
op[8]=((((gconst0)*(gconst4)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst4)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst4)*(gconst6)*(gconst8)))+(((gconst2)*(gconst3)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst3)*(gconst7)*(gconst8))));
polyroots8(op,zeror,numroots);
IKReal j27array[8], cj27array[8], sj27array[8], tempj27array[1];
int numsolutions = 0;
for(int ij27 = 0; ij27 < numroots; ++ij27)
{
IKReal htj27 = zeror[ij27];
tempj27array[0]=((2.00000000000000)*(atan(htj27)));
for(int kj27 = 0; kj27 < 1; ++kj27)
{
j27array[numsolutions] = tempj27array[kj27];
if( j27array[numsolutions] > IKPI )
{
    j27array[numsolutions]-=IK2PI;
}
else if( j27array[numsolutions] < -IKPI )
{
    j27array[numsolutions]+=IK2PI;
}
sj27array[numsolutions] = IKsin(j27array[numsolutions]);
cj27array[numsolutions] = IKcos(j27array[numsolutions]);
numsolutions++;
}
}
bool j27valid[8]={true,true,true,true,true,true,true,true};
_nj27 = 8;
for(int ij27 = 0; ij27 < numsolutions; ++ij27)
    {
if( !j27valid[ij27] )
{
    continue;
}
    j27 = j27array[ij27]; cj27 = cj27array[ij27]; sj27 = sj27array[ij27];
htj27 = IKtan(j27/2);

_ij27[0] = ij27; _ij27[1] = -1;
for(int iij27 = ij27+1; iij27 < numsolutions; ++iij27)
{
if( j27valid[iij27] && IKabs(cj27array[ij27]-cj27array[iij27]) < IKFAST_SOLUTION_THRESH && IKabs(sj27array[ij27]-sj27array[iij27]) < IKFAST_SOLUTION_THRESH )
{
    j27valid[iij27]=false; _ij27[1] = iij27; break; 
}
}
{
IKReal dummyeval[1];
IKReal gconst44;
gconst44=IKsign(((((-4.00000000000000)*(sj29)*((pz)*(pz))))+(((0.800000000000000)*(py)*(sj27)*(sj29)))+(((-4.00000000000000)*(sj29)*((cj27)*(cj27))*((px)*(px))))+(((0.800000000000000)*(cj27)*(px)*(sj29)))+(((-8.00000000000000)*(cj27)*(px)*(py)*(sj27)*(sj29)))+(((-0.0400000000000000)*(sj29)))+(((-4.00000000000000)*(sj29)*((py)*(py))*((sj27)*(sj27))))));
dummyeval[0]=((((20.0000000000000)*(cj27)*(px)*(sj29)))+(((-100.000000000000)*(sj29)*((py)*(py))*((sj27)*(sj27))))+(((20.0000000000000)*(py)*(sj27)*(sj29)))+(((-200.000000000000)*(cj27)*(px)*(py)*(sj27)*(sj29)))+(((-1.00000000000000)*(sj29)))+(((-100.000000000000)*(sj29)*((cj27)*(cj27))*((px)*(px))))+(((-100.000000000000)*(sj29)*((pz)*(pz)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=sj29;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[3];
IKReal x69=((px)*(sj27));
IKReal x70=((cj27)*(py));
IKReal x71=((((-1.00000000000000)*(x70)))+(x69));
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j29)), 6.28318530717959)));
evalcond[1]=x71;
evalcond[2]=x71;
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
{
{
IKReal j30array[2], cj30array[2], sj30array[2];
bool j30valid[2]={false};
_nj30 = 2;
cj30array[0]=((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))));
if( cj30array[0] >= -1-IKFAST_SINCOS_THRESH && cj30array[0] <= 1+IKFAST_SINCOS_THRESH )
{
    j30valid[0] = j30valid[1] = true;
    j30array[0] = IKacos(cj30array[0]);
    sj30array[0] = IKsin(j30array[0]);
    cj30array[1] = cj30array[0];
    j30array[1] = -j30array[0];
    sj30array[1] = -sj30array[0];
}
else if( std::isnan(cj30array[0]) )
{
    // probably any value will work
    j30valid[0] = true;
    cj30array[0] = 1; sj30array[0] = 0; j30array[0] = 0;
}
for(int ij30 = 0; ij30 < 2; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 2; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

{
IKReal dummyeval[1];
IKReal gconst47;
gconst47=IKsign(((40.0000000000000)+(((-321.000000000000)*(cj27)*(cj30)*(px)))+(((-400.000000000000)*(cj27)*(px)))+(((-400.000000000000)*(py)*(sj27)))+(((-321.000000000000)*(cj30)*(py)*(sj27)))+(((-321.000000000000)*(pz)*(sj30)))+(((32.1000000000000)*(cj30)))));
dummyeval[0]=((1.24610591900312)+(((-10.0000000000000)*(pz)*(sj30)))+(((-10.0000000000000)*(cj30)*(py)*(sj27)))+(cj30)+(((-12.4610591900312)*(py)*(sj27)))+(((-12.4610591900312)*(cj27)*(px)))+(((-10.0000000000000)*(cj27)*(cj30)*(px))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=((((-10.0000000000000)*(cj27)*(px)*(sj30)))+(((12.4610591900312)*(pz)))+(((-10.0000000000000)*(py)*(sj27)*(sj30)))+(((10.0000000000000)*(cj30)*(pz)))+(sj30));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j28array[1], cj28array[1], sj28array[1];
bool j28valid[1]={false};
_nj28 = 1;
if( IKabs(((((IKabs(((((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((321000.000000000)*(cj30)*(pz)))+(((400000.000000000)*(pz)))+(((32100.0000000000)*(sj30))))) != 0)?((IKReal)1/(((((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((321000.000000000)*(cj30)*(pz)))+(((400000.000000000)*(pz)))+(((32100.0000000000)*(sj30)))))):(IKReal)1.0e30))*(((((103041.000000000)*((sj30)*(sj30))))+(((-1000000.00000000)*((pz)*(pz)))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((((321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((400.000000000000)*(pz))))) != 0)?((IKReal)1/(((((321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((400.000000000000)*(pz)))))):(IKReal)1.0e30))*(((((1000.00000000000)*(cj27)*(px)*(pz)))+(((-128.400000000000)*(sj30)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((-100.000000000000)*(pz)))+(((-103.041000000000)*(cj30)*(sj30))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j28array[0]=IKatan2(((((IKabs(((((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((321000.000000000)*(cj30)*(pz)))+(((400000.000000000)*(pz)))+(((32100.0000000000)*(sj30))))) != 0)?((IKReal)1/(((((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((321000.000000000)*(cj30)*(pz)))+(((400000.000000000)*(pz)))+(((32100.0000000000)*(sj30)))))):(IKReal)1.0e30))*(((((103041.000000000)*((sj30)*(sj30))))+(((-1000000.00000000)*((pz)*(pz))))))), ((((IKabs(((((321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((400.000000000000)*(pz))))) != 0)?((IKReal)1/(((((321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((400.000000000000)*(pz)))))):(IKReal)1.0e30))*(((((1000.00000000000)*(cj27)*(px)*(pz)))+(((-128.400000000000)*(sj30)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((-100.000000000000)*(pz)))+(((-103.041000000000)*(cj30)*(sj30)))))));
sj28array[0]=IKsin(j28array[0]);
cj28array[0]=IKcos(j28array[0]);
if( j28array[0] > IKPI )
{
    j28array[0]-=IK2PI;
}
else if( j28array[0] < -IKPI )
{    j28array[0]+=IK2PI;
}
j28valid[0] = true;
for(int ij28 = 0; ij28 < 1; ++ij28)
{
if( !j28valid[ij28] )
{
    continue;
}
_ij28[0] = ij28; _ij28[1] = -1;
for(int iij28 = ij28+1; iij28 < 1; ++iij28)
{
if( j28valid[iij28] && IKabs(cj28array[ij28]-cj28array[iij28]) < IKFAST_SOLUTION_THRESH && IKabs(sj28array[ij28]-sj28array[iij28]) < IKFAST_SOLUTION_THRESH )
{
    j28valid[iij28]=false; _ij28[1] = iij28; break; 
}
}
j28 = j28array[ij28]; cj28 = cj28array[ij28]; sj28 = sj28array[ij28];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
{
IKReal j28array[1], cj28array[1], sj28array[1];
bool j28valid[1]={false};
_nj28 = 1;
if( IKabs(((gconst47)*(((((128.400000000000)*(sj30)))+(((1000.00000000000)*(cj27)*(px)*(pz)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((103.041000000000)*(cj30)*(sj30)))+(((-100.000000000000)*(pz))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.00000000000000)*(gconst47)*(((160.000000000000)+(((-1000.00000000000)*((pz)*(pz))))+(((103.041000000000)*((cj30)*(cj30))))+(((256.800000000000)*(cj30))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j28array[0]=IKatan2(((gconst47)*(((((128.400000000000)*(sj30)))+(((1000.00000000000)*(cj27)*(px)*(pz)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((103.041000000000)*(cj30)*(sj30)))+(((-100.000000000000)*(pz)))))), ((-1.00000000000000)*(gconst47)*(((160.000000000000)+(((-1000.00000000000)*((pz)*(pz))))+(((103.041000000000)*((cj30)*(cj30))))+(((256.800000000000)*(cj30)))))));
sj28array[0]=IKsin(j28array[0]);
cj28array[0]=IKcos(j28array[0]);
if( j28array[0] > IKPI )
{
    j28array[0]-=IK2PI;
}
else if( j28array[0] < -IKPI )
{    j28array[0]+=IK2PI;
}
j28valid[0] = true;
for(int ij28 = 0; ij28 < 1; ++ij28)
{
if( !j28valid[ij28] )
{
    continue;
}
_ij28[0] = ij28; _ij28[1] = -1;
for(int iij28 = ij28+1; iij28 < 1; ++iij28)
{
if( j28valid[iij28] && IKabs(cj28array[ij28]-cj28array[iij28]) < IKFAST_SOLUTION_THRESH && IKabs(sj28array[ij28]-sj28array[iij28]) < IKFAST_SOLUTION_THRESH )
{
    j28valid[iij28]=false; _ij28[1] = iij28; break; 
}
}
j28 = j28array[ij28]; cj28 = cj28array[ij28]; sj28 = sj28array[ij28];

rotationfunction0(vsolutions);
}
}

}

}
}
}

} else
{
IKReal x120=((cj27)*(py));
IKReal x121=((px)*(sj27));
evalcond[0]=((-3.14159265358979)+(IKfmod(j29, 6.28318530717959)));
evalcond[1]=((((-1.00000000000000)*(x120)))+(x121));
evalcond[2]=((((-1.00000000000000)*(x121)))+(x120));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
{
{
IKReal j30array[2], cj30array[2], sj30array[2];
bool j30valid[2]={false};
_nj30 = 2;
cj30array[0]=((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))));
if( cj30array[0] >= -1-IKFAST_SINCOS_THRESH && cj30array[0] <= 1+IKFAST_SINCOS_THRESH )
{
    j30valid[0] = j30valid[1] = true;
    j30array[0] = IKacos(cj30array[0]);
    sj30array[0] = IKsin(j30array[0]);
    cj30array[1] = cj30array[0];
    j30array[1] = -j30array[0];
    sj30array[1] = -sj30array[0];
}
else if( std::isnan(cj30array[0]) )
{
    // probably any value will work
    j30valid[0] = true;
    cj30array[0] = 1; sj30array[0] = 0; j30array[0] = 0;
}
for(int ij30 = 0; ij30 < 2; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 2; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

{
IKReal dummyeval[1];
dummyeval[0]=((((-10.0000000000000)*(cj27)*(px)*(sj30)))+(((-10.0000000000000)*(cj30)*(pz)))+(((-10.0000000000000)*(py)*(sj27)*(sj30)))+(sj30)+(((-12.4610591900312)*(pz))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=((1.55277996137460)+(((2.49221183800623)*(cj30)))+((sj30)*(sj30))+((cj30)*(cj30)));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=((1.24610591900312)+(((-10.0000000000000)*(cj30)*(py)*(sj27)))+(((10.0000000000000)*(pz)*(sj30)))+(cj30)+(((-12.4610591900312)*(py)*(sj27)))+(((-12.4610591900312)*(cj27)*(px)))+(((-10.0000000000000)*(cj27)*(cj30)*(px))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j28array[1], cj28array[1], sj28array[1];
bool j28valid[1]={false};
_nj28 = 1;
IKReal x122=((321.000000000000)*(pz)*(sj30));
IKReal x123=((32.1000000000000)*(cj30));
IKReal x124=((40.0000000000000)+(x122)+(x123));
IKReal x125=((321.000000000000)*(cj30)*(py)*(sj27));
IKReal x126=((400.000000000000)*(py)*(sj27));
IKReal x127=((321.000000000000)*(cj27)*(cj30)*(px));
IKReal x128=((400.000000000000)*(cj27)*(px));
IKReal x129=((x126)+(x127)+(x125)+(x128));
IKReal x130=((((-1.00000000000000)*(x129)))+(x124));
IKReal x131=((IKabs(x130) != 0)?((IKReal)1/(x130)):(IKReal)1.0e30);
if( IKabs(((x131)*(((((1000.00000000000)*(cj27)*(px)*(pz)))+(((-128.400000000000)*(sj30)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((-100.000000000000)*(pz)))+(((-103.041000000000)*(cj30)*(sj30))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.00000000000000)*(x131)*(((160.000000000000)+(((-1000.00000000000)*((pz)*(pz))))+(((103.041000000000)*((cj30)*(cj30))))+(((256.800000000000)*(cj30))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j28array[0]=IKatan2(((x131)*(((((1000.00000000000)*(cj27)*(px)*(pz)))+(((-128.400000000000)*(sj30)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((-100.000000000000)*(pz)))+(((-103.041000000000)*(cj30)*(sj30)))))), ((-1.00000000000000)*(x131)*(((160.000000000000)+(((-1000.00000000000)*((pz)*(pz))))+(((103.041000000000)*((cj30)*(cj30))))+(((256.800000000000)*(cj30)))))));
sj28array[0]=IKsin(j28array[0]);
cj28array[0]=IKcos(j28array[0]);
if( j28array[0] > IKPI )
{
    j28array[0]-=IK2PI;
}
else if( j28array[0] < -IKPI )
{    j28array[0]+=IK2PI;
}
j28valid[0] = true;
for(int ij28 = 0; ij28 < 1; ++ij28)
{
if( !j28valid[ij28] )
{
    continue;
}
_ij28[0] = ij28; _ij28[1] = -1;
for(int iij28 = ij28+1; iij28 < 1; ++iij28)
{
if( j28valid[iij28] && IKabs(cj28array[ij28]-cj28array[iij28]) < IKFAST_SOLUTION_THRESH && IKabs(sj28array[ij28]-sj28array[iij28]) < IKFAST_SOLUTION_THRESH )
{
    j28valid[iij28]=false; _ij28[1] = iij28; break; 
}
}
j28 = j28array[ij28]; cj28 = cj28array[ij28]; sj28 = sj28array[ij28];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
{
IKReal j28array[1], cj28array[1], sj28array[1];
bool j28valid[1]={false};
_nj28 = 1;
IKReal x132=(cj30)*(cj30);
IKReal x133=((103041.000000000)*(x132));
IKReal x134=(sj30)*(sj30);
IKReal x135=((103041.000000000)*(x134));
IKReal x136=((256800.000000000)*(cj30));
IKReal x137=((160000.000000000)+(x135)+(x136)+(x133));
IKReal x138=((IKabs(x137) != 0)?((IKReal)1/(x137)):(IKReal)1.0e30);
if( IKabs(((-1.00000000000000)*(x138)*(((((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((321000.000000000)*(cj30)*(pz)))+(((400000.000000000)*(pz)))+(((32100.0000000000)*(sj30))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.00000000000000)*(x138)*(((40000.0000000000)+(((-321000.000000000)*(cj27)*(cj30)*(px)))+(((-321000.000000000)*(cj30)*(py)*(sj27)))+(((-400000.000000000)*(py)*(sj27)))+(((-400000.000000000)*(cj27)*(px)))+(((32100.0000000000)*(cj30)))+(((-321000.000000000)*(pz)*(sj30))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j28array[0]=IKatan2(((-1.00000000000000)*(x138)*(((((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((321000.000000000)*(cj30)*(pz)))+(((400000.000000000)*(pz)))+(((32100.0000000000)*(sj30)))))), ((-1.00000000000000)*(x138)*(((40000.0000000000)+(((-321000.000000000)*(cj27)*(cj30)*(px)))+(((-321000.000000000)*(cj30)*(py)*(sj27)))+(((-400000.000000000)*(py)*(sj27)))+(((-400000.000000000)*(cj27)*(px)))+(((32100.0000000000)*(cj30)))+(((-321000.000000000)*(pz)*(sj30)))))));
sj28array[0]=IKsin(j28array[0]);
cj28array[0]=IKcos(j28array[0]);
if( j28array[0] > IKPI )
{
    j28array[0]-=IK2PI;
}
else if( j28array[0] < -IKPI )
{    j28array[0]+=IK2PI;
}
j28valid[0] = true;
for(int ij28 = 0; ij28 < 1; ++ij28)
{
if( !j28valid[ij28] )
{
    continue;
}
_ij28[0] = ij28; _ij28[1] = -1;
for(int iij28 = ij28+1; iij28 < 1; ++iij28)
{
if( j28valid[iij28] && IKabs(cj28array[ij28]-cj28array[iij28]) < IKFAST_SOLUTION_THRESH && IKabs(sj28array[ij28]-sj28array[iij28]) < IKFAST_SOLUTION_THRESH )
{
    j28valid[iij28]=false; _ij28[1] = iij28; break; 
}
}
j28 = j28array[ij28]; cj28 = cj28array[ij28]; sj28 = sj28array[ij28];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
{
IKReal j28array[1], cj28array[1], sj28array[1];
bool j28valid[1]={false};
_nj28 = 1;
if( IKabs(((((IKabs(((((-400000.000000000)*(pz)))+(((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(cj30)*(pz)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((32100.0000000000)*(sj30))))) != 0)?((IKReal)1/(((((-400000.000000000)*(pz)))+(((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(cj30)*(pz)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((32100.0000000000)*(sj30)))))):(IKReal)1.0e30))*(((((-103041.000000000)*((sj30)*(sj30))))+(((1000000.00000000)*((pz)*(pz)))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((((-321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((-400.000000000000)*(pz))))) != 0)?((IKReal)1/(((((-321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((-400.000000000000)*(pz)))))):(IKReal)1.0e30))*(((((-128.400000000000)*(sj30)))+(((-1000.00000000000)*(cj27)*(px)*(pz)))+(((100.000000000000)*(pz)))+(((-1000.00000000000)*(py)*(pz)*(sj27)))+(((-103.041000000000)*(cj30)*(sj30))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j28array[0]=IKatan2(((((IKabs(((((-400000.000000000)*(pz)))+(((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(cj30)*(pz)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((32100.0000000000)*(sj30))))) != 0)?((IKReal)1/(((((-400000.000000000)*(pz)))+(((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(cj30)*(pz)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((32100.0000000000)*(sj30)))))):(IKReal)1.0e30))*(((((-103041.000000000)*((sj30)*(sj30))))+(((1000000.00000000)*((pz)*(pz))))))), ((((IKabs(((((-321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((-400.000000000000)*(pz))))) != 0)?((IKReal)1/(((((-321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((-400.000000000000)*(pz)))))):(IKReal)1.0e30))*(((((-128.400000000000)*(sj30)))+(((-1000.00000000000)*(cj27)*(px)*(pz)))+(((100.000000000000)*(pz)))+(((-1000.00000000000)*(py)*(pz)*(sj27)))+(((-103.041000000000)*(cj30)*(sj30)))))));
sj28array[0]=IKsin(j28array[0]);
cj28array[0]=IKcos(j28array[0]);
if( j28array[0] > IKPI )
{
    j28array[0]-=IK2PI;
}
else if( j28array[0] < -IKPI )
{    j28array[0]+=IK2PI;
}
j28valid[0] = true;
for(int ij28 = 0; ij28 < 1; ++ij28)
{
if( !j28valid[ij28] )
{
    continue;
}
_ij28[0] = ij28; _ij28[1] = -1;
for(int iij28 = ij28+1; iij28 < 1; ++iij28)
{
if( j28valid[iij28] && IKabs(cj28array[ij28]-cj28array[iij28]) < IKFAST_SOLUTION_THRESH && IKabs(sj28array[ij28]-sj28array[iij28]) < IKFAST_SOLUTION_THRESH )
{
    j28valid[iij28]=false; _ij28[1] = iij28; break; 
}
}
j28 = j28array[ij28]; cj28 = cj28array[ij28]; sj28 = sj28array[ij28];

rotationfunction0(vsolutions);
}
}

}

}
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

{
IKReal dummyeval[1];
IKReal gconst45;
gconst45=IKsign(((40.0000000000000)+(((-321.000000000000)*(cj27)*(cj30)*(px)))+(((-400.000000000000)*(cj27)*(px)))+(((-400.000000000000)*(py)*(sj27)))+(((-321.000000000000)*(cj30)*(py)*(sj27)))+(((-321.000000000000)*(cj29)*(pz)*(sj30)))+(((32.1000000000000)*(cj30)))));
dummyeval[0]=((1.24610591900312)+(((-10.0000000000000)*(cj30)*(py)*(sj27)))+(cj30)+(((-12.4610591900312)*(py)*(sj27)))+(((-12.4610591900312)*(cj27)*(px)))+(((-10.0000000000000)*(cj29)*(pz)*(sj30)))+(((-10.0000000000000)*(cj27)*(cj30)*(px))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j28array[1], cj28array[1], sj28array[1];
bool j28valid[1]={false};
_nj28 = 1;
if( IKabs(((gconst45)*(((((128.400000000000)*(cj29)*(sj30)))+(((1000.00000000000)*(cj27)*(px)*(pz)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((-100.000000000000)*(pz)))+(((103.041000000000)*(cj29)*(cj30)*(sj30))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.00000000000000)*(gconst45)*(((160.000000000000)+(((-1000.00000000000)*((pz)*(pz))))+(((103.041000000000)*((cj30)*(cj30))))+(((256.800000000000)*(cj30))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j28array[0]=IKatan2(((gconst45)*(((((128.400000000000)*(cj29)*(sj30)))+(((1000.00000000000)*(cj27)*(px)*(pz)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((-100.000000000000)*(pz)))+(((103.041000000000)*(cj29)*(cj30)*(sj30)))))), ((-1.00000000000000)*(gconst45)*(((160.000000000000)+(((-1000.00000000000)*((pz)*(pz))))+(((103.041000000000)*((cj30)*(cj30))))+(((256.800000000000)*(cj30)))))));
sj28array[0]=IKsin(j28array[0]);
cj28array[0]=IKcos(j28array[0]);
if( j28array[0] > IKPI )
{
    j28array[0]-=IK2PI;
}
else if( j28array[0] < -IKPI )
{    j28array[0]+=IK2PI;
}
j28valid[0] = true;
for(int ij28 = 0; ij28 < 1; ++ij28)
{
if( !j28valid[ij28] )
{
    continue;
}
_ij28[0] = ij28; _ij28[1] = -1;
for(int iij28 = ij28+1; iij28 < 1; ++iij28)
{
if( j28valid[iij28] && IKabs(cj28array[ij28]-cj28array[iij28]) < IKFAST_SOLUTION_THRESH && IKabs(sj28array[ij28]-sj28array[iij28]) < IKFAST_SOLUTION_THRESH )
{
    j28valid[iij28]=false; _ij28[1] = iij28; break; 
}
}
j28 = j28array[ij28]; cj28 = cj28array[ij28]; sj28 = sj28array[ij28];

rotationfunction0(vsolutions);
}
}

}

}
}
}

}

}

} else
{
{
IKReal j28array[1], cj28array[1], sj28array[1];
bool j28valid[1]={false};
_nj28 = 1;
IKReal x139=(py)*(py);
IKReal x140=(sj27)*(sj27);
IKReal x141=(cj27)*(cj27);
IKReal x142=(px)*(px);
if( IKabs(((gconst44)*(((((-4.00000000000000)*(cj27)*(cj29)*(sj27)*(x142)))+(((-4.00000000000000)*(cj29)*(px)*(py)*(x140)))+(((-1.00000000000000)*(cj27)*(px)*(pz)*(sj29)))+(((0.400000000000000)*(cj29)*(px)*(sj27)))+(((4.00000000000000)*(cj27)*(cj29)*(sj27)*(x139)))+(((5.00000000000000)*(pp)*(pz)*(sj29)))+(((-1.00000000000000)*(py)*(pz)*(sj27)*(sj29)))+(((0.334795000000000)*(pz)*(sj29)))+(((4.00000000000000)*(cj29)*(px)*(py)*(x141)))+(((-0.400000000000000)*(cj27)*(cj29)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst44)*(((((0.0334795000000000)*(sj29)))+(((4.00000000000000)*(cj27)*(cj29)*(py)*(pz)))+(((-5.00000000000000)*(pp)*(py)*(sj27)*(sj29)))+(((0.500000000000000)*(pp)*(sj29)))+(((-5.00000000000000)*(cj27)*(pp)*(px)*(sj29)))+(((-4.00000000000000)*(cj29)*(px)*(pz)*(sj27)))+(((sj29)*(x141)*(x142)))+(((2.00000000000000)*(cj27)*(px)*(py)*(sj27)*(sj29)))+(((-0.434795000000000)*(cj27)*(px)*(sj29)))+(((-0.434795000000000)*(py)*(sj27)*(sj29)))+(((sj29)*(x139)*(x140))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j28array[0]=IKatan2(((gconst44)*(((((-4.00000000000000)*(cj27)*(cj29)*(sj27)*(x142)))+(((-4.00000000000000)*(cj29)*(px)*(py)*(x140)))+(((-1.00000000000000)*(cj27)*(px)*(pz)*(sj29)))+(((0.400000000000000)*(cj29)*(px)*(sj27)))+(((4.00000000000000)*(cj27)*(cj29)*(sj27)*(x139)))+(((5.00000000000000)*(pp)*(pz)*(sj29)))+(((-1.00000000000000)*(py)*(pz)*(sj27)*(sj29)))+(((0.334795000000000)*(pz)*(sj29)))+(((4.00000000000000)*(cj29)*(px)*(py)*(x141)))+(((-0.400000000000000)*(cj27)*(cj29)*(py)))))), ((gconst44)*(((((0.0334795000000000)*(sj29)))+(((4.00000000000000)*(cj27)*(cj29)*(py)*(pz)))+(((-5.00000000000000)*(pp)*(py)*(sj27)*(sj29)))+(((0.500000000000000)*(pp)*(sj29)))+(((-5.00000000000000)*(cj27)*(pp)*(px)*(sj29)))+(((-4.00000000000000)*(cj29)*(px)*(pz)*(sj27)))+(((sj29)*(x141)*(x142)))+(((2.00000000000000)*(cj27)*(px)*(py)*(sj27)*(sj29)))+(((-0.434795000000000)*(cj27)*(px)*(sj29)))+(((-0.434795000000000)*(py)*(sj27)*(sj29)))+(((sj29)*(x139)*(x140)))))));
sj28array[0]=IKsin(j28array[0]);
cj28array[0]=IKcos(j28array[0]);
if( j28array[0] > IKPI )
{
    j28array[0]-=IK2PI;
}
else if( j28array[0] < -IKPI )
{    j28array[0]+=IK2PI;
}
j28valid[0] = true;
for(int ij28 = 0; ij28 < 1; ++ij28)
{
if( !j28valid[ij28] )
{
    continue;
}
_ij28[0] = ij28; _ij28[1] = -1;
for(int iij28 = ij28+1; iij28 < 1; ++iij28)
{
if( j28valid[iij28] && IKabs(cj28array[ij28]-cj28array[iij28]) < IKFAST_SOLUTION_THRESH && IKabs(sj28array[ij28]-sj28array[iij28]) < IKFAST_SOLUTION_THRESH )
{
    j28valid[iij28]=false; _ij28[1] = iij28; break; 
}
}
j28 = j28array[ij28]; cj28 = cj28array[ij28]; sj28 = sj28array[ij28];

{
IKReal dummyeval[1];
dummyeval[0]=sj29;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=sj29;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[2];
dummyeval[0]=cj28;
dummyeval[1]=cj29;
if( IKabs(dummyeval[0]) < 0.0000010000000000  || IKabs(dummyeval[1]) < 0.0000010000000000  )
{
{
IKReal evalcond[4];
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.57079632679490)+(j28)), 6.28318530717959)));
evalcond[1]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
evalcond[2]=((((cj29)*(px)*(sj27)))+(((0.100000000000000)*(sj29)))+(((-1.00000000000000)*(py)*(sj27)*(sj29)))+(((-1.00000000000000)*(cj27)*(px)*(sj29)))+(((-1.00000000000000)*(cj27)*(cj29)*(py))));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=sj29;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=cj29;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=sj29;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[4];
IKReal x143=((px)*(sj27));
IKReal x144=((cj27)*(py));
IKReal x145=((((-1.00000000000000)*(x144)))+(x143));
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j29)), 6.28318530717959)));
evalcond[1]=x145;
evalcond[2]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
evalcond[3]=x145;
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((0.311526479750779)+(((-3.11526479750779)*(py)*(sj27)))+(((-3.11526479750779)*(cj27)*(px))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((-3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((0.311526479750779)+(((-3.11526479750779)*(py)*(sj27)))+(((-3.11526479750779)*(cj27)*(px)))), ((-1.24610591900312)+(((-3.11526479750779)*(pz)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

} else
{
IKReal x146=((cj27)*(py));
IKReal x147=((px)*(sj27));
evalcond[0]=((-3.14159265358979)+(IKfmod(j29, 6.28318530717959)));
evalcond[1]=((((-1.00000000000000)*(x146)))+(x147));
evalcond[2]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
evalcond[3]=((((-1.00000000000000)*(x147)))+(x146));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((-0.311526479750779)+(((3.11526479750779)*(py)*(sj27)))+(((3.11526479750779)*(cj27)*(px))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((-3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((-0.311526479750779)+(((3.11526479750779)*(py)*(sj27)))+(((3.11526479750779)*(cj27)*(px)))), ((-1.24610591900312)+(((-3.11526479750779)*(pz)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

} else
{
IKReal x148=((cj27)*(px));
IKReal x149=((py)*(sj27));
IKReal x150=((x148)+(x149));
IKReal x151=((0.100000000000000)+(((-1.00000000000000)*(x150))));
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.57079632679490)+(j29)), 6.28318530717959)));
evalcond[1]=x151;
evalcond[2]=((-0.0669590000000000)+(((-0.800000000000000)*(pz)))+(((0.200000000000000)*(x148)))+(((0.200000000000000)*(x149)))+(((-1.00000000000000)*(pp))));
evalcond[3]=x151;
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((-3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py)))), ((-1.24610591900312)+(((-3.11526479750779)*(pz)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

} else
{
IKReal x152=((cj27)*(px));
IKReal x153=((py)*(sj27));
evalcond[0]=((-3.14159265358979)+(IKfmod(((4.71238898038469)+(j29)), 6.28318530717959)));
evalcond[1]=((0.100000000000000)+(((-1.00000000000000)*(x152)))+(((-1.00000000000000)*(x153))));
evalcond[2]=((-0.0669590000000000)+(((-0.800000000000000)*(pz)))+(((0.200000000000000)*(x153)))+(((0.200000000000000)*(x152)))+(((-1.00000000000000)*(pp))));
evalcond[3]=((-0.100000000000000)+(x153)+(x152));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((-3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27)))), ((-1.24610591900312)+(((-3.11526479750779)*(pz)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}
}
}

} else
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((0.00311526479750779)*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((100.000000000000)+(((-1000.00000000000)*(cj27)*(px)))+(((-1000.00000000000)*(py)*(sj27))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((-3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((100.000000000000)+(((-1000.00000000000)*(cj27)*(px)))+(((-1000.00000000000)*(py)*(sj27)))))), ((-1.24610591900312)+(((-3.11526479750779)*(pz)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((-3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-1.24610591900312)+(((-3.11526479750779)*(pz)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
evalcond[0]=((-3.14159265358979)+(IKfmod(((4.71238898038469)+(j28)), 6.28318530717959)));
evalcond[1]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
evalcond[2]=((((cj29)*(px)*(sj27)))+(((cj27)*(px)*(sj29)))+(((py)*(sj27)*(sj29)))+(((-0.100000000000000)*(sj29)))+(((-1.00000000000000)*(cj27)*(cj29)*(py))));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=sj29;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=cj29;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=sj29;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[4];
IKReal x154=((px)*(sj27));
IKReal x155=((cj27)*(py));
IKReal x156=((((-1.00000000000000)*(x155)))+(x154));
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j29)), 6.28318530717959)));
evalcond[1]=x156;
evalcond[2]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
evalcond[3]=x156;
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((-0.311526479750779)+(((3.11526479750779)*(py)*(sj27)))+(((3.11526479750779)*(cj27)*(px))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((-0.311526479750779)+(((3.11526479750779)*(py)*(sj27)))+(((3.11526479750779)*(cj27)*(px)))), ((-1.24610591900312)+(((3.11526479750779)*(pz)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

} else
{
IKReal x157=((cj27)*(py));
IKReal x158=((px)*(sj27));
evalcond[0]=((-3.14159265358979)+(IKfmod(j29, 6.28318530717959)));
evalcond[1]=((((-1.00000000000000)*(x157)))+(x158));
evalcond[2]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
evalcond[3]=((((-1.00000000000000)*(x158)))+(x157));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((0.311526479750779)+(((-3.11526479750779)*(py)*(sj27)))+(((-3.11526479750779)*(cj27)*(px))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((0.311526479750779)+(((-3.11526479750779)*(py)*(sj27)))+(((-3.11526479750779)*(cj27)*(px)))), ((-1.24610591900312)+(((3.11526479750779)*(pz)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

} else
{
IKReal x159=((cj27)*(px));
IKReal x160=((py)*(sj27));
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.57079632679490)+(j29)), 6.28318530717959)));
evalcond[1]=((0.100000000000000)+(((-1.00000000000000)*(x159)))+(((-1.00000000000000)*(x160))));
evalcond[2]=((-0.0669590000000000)+(((0.800000000000000)*(pz)))+(((0.200000000000000)*(x160)))+(((0.200000000000000)*(x159)))+(((-1.00000000000000)*(pp))));
evalcond[3]=((-0.100000000000000)+(x160)+(x159));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py)))), ((-1.24610591900312)+(((3.11526479750779)*(pz)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

} else
{
IKReal x161=((cj27)*(px));
IKReal x162=((py)*(sj27));
IKReal x163=((x162)+(x161));
IKReal x164=((0.100000000000000)+(((-1.00000000000000)*(x163))));
evalcond[0]=((-3.14159265358979)+(IKfmod(((4.71238898038469)+(j29)), 6.28318530717959)));
evalcond[1]=x164;
evalcond[2]=((-0.0669590000000000)+(((0.800000000000000)*(pz)))+(((0.200000000000000)*(x162)))+(((0.200000000000000)*(x161)))+(((-1.00000000000000)*(pp))));
evalcond[3]=x164;
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27)))), ((-1.24610591900312)+(((3.11526479750779)*(pz)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}
}
}

} else
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((-0.00311526479750779)*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((100.000000000000)+(((-1000.00000000000)*(cj27)*(px)))+(((-1000.00000000000)*(py)*(sj27))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((-0.00311526479750779)*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((100.000000000000)+(((-1000.00000000000)*(cj27)*(px)))+(((-1000.00000000000)*(py)*(sj27)))))), ((-1.24610591900312)+(((3.11526479750779)*(pz)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-1.24610591900312)+(((3.11526479750779)*(pz)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.57079632679490)+(j29)), 6.28318530717959)));
evalcond[1]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)*(sj28)))+(((0.800000000000000)*(cj27)*(cj28)*(px)))+(((-0.0800000000000000)*(cj28)))+(((0.800000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
evalcond[2]=((((-1.00000000000000)*(cj28)*(pz)))+(((0.100000000000000)*(sj28)))+(((-1.00000000000000)*(py)*(sj27)*(sj28)))+(((-1.00000000000000)*(cj27)*(px)*(sj28))));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py)))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

} else
{
evalcond[0]=((-3.14159265358979)+(IKfmod(((4.71238898038469)+(j29)), 6.28318530717959)));
evalcond[1]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)*(sj28)))+(((0.800000000000000)*(cj27)*(cj28)*(px)))+(((-0.0800000000000000)*(cj28)))+(((0.800000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
evalcond[2]=((((cj28)*(pz)))+(((cj27)*(px)*(sj28)))+(((py)*(sj27)*(sj28)))+(((-0.100000000000000)*(sj28))));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27)))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

} else
{
IKReal x165=((px)*(sj27));
IKReal x166=((cj27)*(py));
IKReal x167=((((-1.00000000000000)*(x166)))+(x165));
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j29)), 6.28318530717959)));
evalcond[1]=x167;
evalcond[2]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)*(sj28)))+(((0.800000000000000)*(cj27)*(cj28)*(px)))+(((-0.0800000000000000)*(cj28)))+(((0.800000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
evalcond[3]=x167;
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((((-3.11526479750779)*(cj28)*(pz)))+(((-3.11526479750779)*(py)*(sj27)*(sj28)))+(((0.311526479750779)*(sj28)))+(((-3.11526479750779)*(cj27)*(px)*(sj28))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((((-3.11526479750779)*(cj28)*(pz)))+(((-3.11526479750779)*(py)*(sj27)*(sj28)))+(((0.311526479750779)*(sj28)))+(((-3.11526479750779)*(cj27)*(px)*(sj28)))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

} else
{
IKReal x168=((cj27)*(py));
IKReal x169=((px)*(sj27));
evalcond[0]=((-3.14159265358979)+(IKfmod(j29, 6.28318530717959)));
evalcond[1]=((((-1.00000000000000)*(x168)))+(x169));
evalcond[2]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)*(sj28)))+(((0.800000000000000)*(cj27)*(cj28)*(px)))+(((-0.0800000000000000)*(cj28)))+(((0.800000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
evalcond[3]=((((-1.00000000000000)*(x169)))+(x168));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
IKReal gconst46;
gconst46=IKsign(((((-321.000000000000)*((sj28)*(sj28))))+(((-321.000000000000)*((cj28)*(cj28))))));
dummyeval[0]=((((-1.00000000000000)*((cj28)*(cj28))))+(((-1.00000000000000)*((sj28)*(sj28)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=cj28;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=cj28;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((((1000.00000000000)*(cj27)*(cj28)*(px)*(sj28)))+(((-1000.00000000000)*(pz)*((sj28)*(sj28))))+(((1000.00000000000)*(cj28)*(py)*(sj27)*(sj28)))+(((1000.00000000000)*(pz)))+(((-100.000000000000)*(cj28)*(sj28))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(cj27)*(cj28)*(px)))+(((-0.311526479750779)*(cj28)))+(((3.11526479750779)*(cj28)*(py)*(sj27)))+(((-3.11526479750779)*(pz)*(sj28))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((((1000.00000000000)*(cj27)*(cj28)*(px)*(sj28)))+(((-1000.00000000000)*(pz)*((sj28)*(sj28))))+(((1000.00000000000)*(cj28)*(py)*(sj27)*(sj28)))+(((1000.00000000000)*(pz)))+(((-100.000000000000)*(cj28)*(sj28)))))), ((-1.24610591900312)+(((3.11526479750779)*(cj27)*(cj28)*(px)))+(((-0.311526479750779)*(cj28)))+(((3.11526479750779)*(cj28)*(py)*(sj27)))+(((-3.11526479750779)*(pz)*(sj28)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((((83.6987500000000)*(sj28)))+(((1250.00000000000)*(pp)*(sj28)))+(((1000.00000000000)*(pz)))+(((-250.000000000000)*(cj27)*(px)*(sj28)))+(((-250.000000000000)*(py)*(sj27)*(sj28))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((((83.6987500000000)*(sj28)))+(((1250.00000000000)*(pp)*(sj28)))+(((1000.00000000000)*(pz)))+(((-250.000000000000)*(cj27)*(px)*(sj28)))+(((-250.000000000000)*(py)*(sj27)*(sj28)))))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((gconst46)*(((((100.000000000000)*(sj28)))+(((-1000.00000000000)*(cj28)*(pz)))+(((-1000.00000000000)*(cj27)*(px)*(sj28)))+(((-1000.00000000000)*(py)*(sj27)*(sj28))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst46)*(((((1000.00000000000)*(pz)*(sj28)))+(((-1000.00000000000)*(cj27)*(cj28)*(px)))+(((100.000000000000)*(cj28)))+(((400.000000000000)*((sj28)*(sj28))))+(((-1000.00000000000)*(cj28)*(py)*(sj27)))+(((400.000000000000)*((cj28)*(cj28)))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((gconst46)*(((((100.000000000000)*(sj28)))+(((-1000.00000000000)*(cj28)*(pz)))+(((-1000.00000000000)*(cj27)*(px)*(sj28)))+(((-1000.00000000000)*(py)*(sj27)*(sj28)))))), ((gconst46)*(((((1000.00000000000)*(pz)*(sj28)))+(((-1000.00000000000)*(cj27)*(cj28)*(px)))+(((100.000000000000)*(cj28)))+(((400.000000000000)*((sj28)*(sj28))))+(((-1000.00000000000)*(cj28)*(py)*(sj27)))+(((400.000000000000)*((cj28)*(cj28))))))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}
}
}
}
}

} else
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((((250.000000000000)*(py)*(sj27)*(sj28)))+(((-1250.00000000000)*(pp)*(sj28)))+(((-83.6987500000000)*(sj28)))+(((-1000.00000000000)*(pz)))+(((250.000000000000)*(cj27)*(px)*(sj28))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((((250.000000000000)*(py)*(sj27)*(sj28)))+(((-1250.00000000000)*(pp)*(sj28)))+(((-83.6987500000000)*(sj28)))+(((-1000.00000000000)*(pz)))+(((250.000000000000)*(cj27)*(px)*(sj28)))))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(cj27)*(cj28)*(px)))+(((-0.311526479750779)*(cj28)))+(((3.11526479750779)*(cj28)*(py)*(sj27)))+(((-3.11526479750779)*(pz)*(sj28))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-1.24610591900312)+(((3.11526479750779)*(cj27)*(cj28)*(px)))+(((-0.311526479750779)*(cj28)))+(((3.11526479750779)*(cj28)*(py)*(sj27)))+(((-3.11526479750779)*(pz)*(sj28)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
{
IKReal j30array[1], cj30array[1], sj30array[1];
bool j30valid[1]={false};
_nj30 = 1;
if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
sj30array[0]=IKsin(j30array[0]);
cj30array[0]=IKcos(j30array[0]);
if( j30array[0] > IKPI )
{
    j30array[0]-=IK2PI;
}
else if( j30array[0] < -IKPI )
{    j30array[0]+=IK2PI;
}
j30valid[0] = true;
for(int ij30 = 0; ij30 < 1; ++ij30)
{
if( !j30valid[ij30] )
{
    continue;
}
_ij30[0] = ij30; _ij30[1] = -1;
for(int iij30 = ij30+1; iij30 < 1; ++iij30)
{
if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
{
    j30valid[iij30]=false; _ij30[1] = iij30; break; 
}
}
j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];

rotationfunction0(vsolutions);
}
}

}

}
}
}

}

}
    }

}

}
}
return vsolutions.size()>0;
}
inline void rotationfunction0(std::vector<IKSolution>& vsolutions) {
for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
IKReal x72=((cj29)*(cj30)*(sj28));
IKReal x73=((cj28)*(sj30));
IKReal x74=((x73)+(x72));
IKReal x75=((-1.00000000000000)*(x74));
IKReal x76=((cj27)*(x75));
IKReal x77=((-1.00000000000000)*(cj30)*(sj27)*(sj29));
IKReal x78=((x77)+(x76));
IKReal x79=((sj28)*(sj30));
IKReal x80=((cj28)*(cj29)*(cj30));
IKReal x81=((x79)+(((-1.00000000000000)*(x80))));
IKReal x82=((cj27)*(cj30)*(sj29));
IKReal x83=((sj27)*(x75));
IKReal x84=((x82)+(x83));
IKReal x85=((cj27)*(sj28)*(sj29));
IKReal x86=((cj29)*(sj27));
IKReal x87=((x85)+(((-1.00000000000000)*(x86))));
IKReal x88=((sj27)*(sj28)*(sj29));
IKReal x89=((cj27)*(cj29));
IKReal x90=((x88)+(x89));
IKReal x91=((cj28)*(cj30));
IKReal x92=((cj29)*(x79));
IKReal x93=((x91)+(((-1.00000000000000)*(x92))));
IKReal x94=((cj30)*(sj28));
IKReal x95=((cj29)*(x73));
IKReal x96=((x95)+(x94));
IKReal x97=((-1.00000000000000)*(x96));
IKReal x98=((cj27)*(sj29)*(sj30));
IKReal x99=((sj27)*(x93));
IKReal x100=((x99)+(x98));
IKReal x101=((cj27)*(x93));
IKReal x102=((sj27)*(sj29)*(sj30));
IKReal x103=((((-1.00000000000000)*(x102)))+(x101));
new_r00=((((r10)*(x84)))+(((r20)*(x81)))+(((r00)*(x78))));
new_r01=((((r21)*(x81)))+(((r11)*(x84)))+(((r01)*(x78))));
new_r02=((((r12)*(x84)))+(((r02)*(x78)))+(((r22)*(x81))));
new_r10=((((r10)*(x90)))+(((r00)*(x87)))+(((cj28)*(r20)*(sj29))));
new_r11=((((r11)*(x90)))+(((cj28)*(r21)*(sj29)))+(((r01)*(x87))));
new_r12=((((r12)*(x90)))+(((cj28)*(r22)*(sj29)))+(((r02)*(x87))));
new_r20=((((r10)*(x100)))+(((r20)*(x97)))+(((r00)*(x103))));
new_r21=((((r21)*(x97)))+(((r01)*(x103)))+(((r11)*(x100))));
new_r22=((((r12)*(x100)))+(((r02)*(((x101)+(((-1.00000000000000)*(x102)))))))+(((r22)*(x97))));
{
IKReal j32array[2], cj32array[2], sj32array[2];
bool j32valid[2]={false};
_nj32 = 2;
cj32array[0]=new_r22;
if( cj32array[0] >= -1-IKFAST_SINCOS_THRESH && cj32array[0] <= 1+IKFAST_SINCOS_THRESH )
{
    j32valid[0] = j32valid[1] = true;
    j32array[0] = IKacos(cj32array[0]);
    sj32array[0] = IKsin(j32array[0]);
    cj32array[1] = cj32array[0];
    j32array[1] = -j32array[0];
    sj32array[1] = -sj32array[0];
}
else if( std::isnan(cj32array[0]) )
{
    // probably any value will work
    j32valid[0] = true;
    cj32array[0] = 1; sj32array[0] = 0; j32array[0] = 0;
}
for(int ij32 = 0; ij32 < 2; ++ij32)
{
if( !j32valid[ij32] )
{
    continue;
}
_ij32[0] = ij32; _ij32[1] = -1;
for(int iij32 = ij32+1; iij32 < 2; ++iij32)
{
if( j32valid[iij32] && IKabs(cj32array[ij32]-cj32array[iij32]) < IKFAST_SOLUTION_THRESH && IKabs(sj32array[ij32]-sj32array[iij32]) < IKFAST_SOLUTION_THRESH )
{
    j32valid[iij32]=false; _ij32[1] = iij32; break; 
}
}
j32 = j32array[ij32]; cj32 = cj32array[ij32]; sj32 = sj32array[ij32];

{
IKReal dummyeval[1];
IKReal gconst49;
gconst49=IKsign(sj32);
dummyeval[0]=sj32;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
IKReal gconst48;
gconst48=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[7];
IKReal x104=((-1.00000000000000)+(new_r22));
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j32)), 6.28318530717959)));
evalcond[1]=x104;
evalcond[2]=new_r20;
evalcond[3]=new_r21;
evalcond[4]=new_r20;
evalcond[5]=new_r21;
evalcond[6]=x104;
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
{
{
IKReal j31array[2], cj31array[2], sj31array[2];
bool j31valid[2]={false};
_nj31 = 2;
if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
    continue;
IKReal x105=IKatan2(new_r02, new_r12);
j31array[0]=((-1.00000000000000)*(x105));
sj31array[0]=IKsin(j31array[0]);
cj31array[0]=IKcos(j31array[0]);
j31array[1]=((3.14159265358979)+(((-1.00000000000000)*(x105))));
sj31array[1]=IKsin(j31array[1]);
cj31array[1]=IKcos(j31array[1]);
if( j31array[0] > IKPI )
{
    j31array[0]-=IK2PI;
}
else if( j31array[0] < -IKPI )
{    j31array[0]+=IK2PI;
}
j31valid[0] = true;
if( j31array[1] > IKPI )
{
    j31array[1]-=IK2PI;
}
else if( j31array[1] < -IKPI )
{    j31array[1]+=IK2PI;
}
j31valid[1] = true;
for(int ij31 = 0; ij31 < 2; ++ij31)
{
if( !j31valid[ij31] )
{
    continue;
}
_ij31[0] = ij31; _ij31[1] = -1;
for(int iij31 = ij31+1; iij31 < 2; ++iij31)
{
if( j31valid[iij31] && IKabs(cj31array[ij31]-cj31array[iij31]) < IKFAST_SOLUTION_THRESH && IKabs(sj31array[ij31]-sj31array[iij31]) < IKFAST_SOLUTION_THRESH )
{
    j31valid[iij31]=false; _ij31[1] = iij31; break; 
}
}
j31 = j31array[ij31]; cj31 = cj31array[ij31]; sj31 = sj31array[ij31];

{
IKReal j33array[1], cj33array[1], sj33array[1];
bool j33valid[1]={false};
_nj33 = 1;
if( IKabs(((((-1.00000000000000)*(cj31)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj31)*(new_r00)))+(((new_r10)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j33array[0]=IKatan2(((((-1.00000000000000)*(cj31)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj31)))), ((((cj31)*(new_r00)))+(((new_r10)*(sj31)))));
sj33array[0]=IKsin(j33array[0]);
cj33array[0]=IKcos(j33array[0]);
if( j33array[0] > IKPI )
{
    j33array[0]-=IK2PI;
}
else if( j33array[0] < -IKPI )
{    j33array[0]+=IK2PI;
}
j33valid[0] = true;
for(int ij33 = 0; ij33 < 1; ++ij33)
{
if( !j33valid[ij33] )
{
    continue;
}
_ij33[0] = ij33; _ij33[1] = -1;
for(int iij33 = ij33+1; iij33 < 1; ++iij33)
{
if( j33valid[iij33] && IKabs(cj33array[ij33]-cj33array[iij33]) < IKFAST_SOLUTION_THRESH && IKabs(sj33array[ij33]-sj33array[iij33]) < IKFAST_SOLUTION_THRESH )
{
    j33valid[iij33]=false; _ij33[1] = iij33; break; 
}
}
j33 = j33array[ij33]; cj33 = cj33array[ij33]; sj33 = sj33array[ij33];

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j27;
solution.basesol[0].indices[0] = _ij27[0];
solution.basesol[0].indices[1] = _ij27[1];
solution.basesol[0].maxsolutions = _nj27;
solution.basesol[1].foffset = j28;
solution.basesol[1].indices[0] = _ij28[0];
solution.basesol[1].indices[1] = _ij28[1];
solution.basesol[1].maxsolutions = _nj28;
solution.basesol[2].foffset = j29;
solution.basesol[2].indices[0] = _ij29[0];
solution.basesol[2].indices[1] = _ij29[1];
solution.basesol[2].maxsolutions = _nj29;
solution.basesol[3].foffset = j30;
solution.basesol[3].indices[0] = _ij30[0];
solution.basesol[3].indices[1] = _ij30[1];
solution.basesol[3].maxsolutions = _nj30;
solution.basesol[4].foffset = j31;
solution.basesol[4].indices[0] = _ij31[0];
solution.basesol[4].indices[1] = _ij31[1];
solution.basesol[4].maxsolutions = _nj31;
solution.basesol[5].foffset = j32;
solution.basesol[5].indices[0] = _ij32[0];
solution.basesol[5].indices[1] = _ij32[1];
solution.basesol[5].maxsolutions = _nj32;
solution.basesol[6].foffset = j33;
solution.basesol[6].indices[0] = _ij33[0];
solution.basesol[6].indices[1] = _ij33[1];
solution.basesol[6].maxsolutions = _nj33;
solution.vfree.resize(0);
}
}
}
}
}

} else
{
evalcond[0]=((-3.14159265358979)+(IKfmod(j32, 6.28318530717959)));
evalcond[1]=((1.00000000000000)+(new_r22));
evalcond[2]=new_r20;
evalcond[3]=new_r21;
evalcond[4]=((-1.00000000000000)*(new_r20));
evalcond[5]=((-1.00000000000000)*(new_r21));
evalcond[6]=((-1.00000000000000)+(((-1.00000000000000)*(new_r22))));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
{
{
IKReal j31array[2], cj31array[2], sj31array[2];
bool j31valid[2]={false};
_nj31 = 2;
if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
    continue;
IKReal x106=IKatan2(new_r02, new_r12);
j31array[0]=((-1.00000000000000)*(x106));
sj31array[0]=IKsin(j31array[0]);
cj31array[0]=IKcos(j31array[0]);
j31array[1]=((3.14159265358979)+(((-1.00000000000000)*(x106))));
sj31array[1]=IKsin(j31array[1]);
cj31array[1]=IKcos(j31array[1]);
if( j31array[0] > IKPI )
{
    j31array[0]-=IK2PI;
}
else if( j31array[0] < -IKPI )
{    j31array[0]+=IK2PI;
}
j31valid[0] = true;
if( j31array[1] > IKPI )
{
    j31array[1]-=IK2PI;
}
else if( j31array[1] < -IKPI )
{    j31array[1]+=IK2PI;
}
j31valid[1] = true;
for(int ij31 = 0; ij31 < 2; ++ij31)
{
if( !j31valid[ij31] )
{
    continue;
}
_ij31[0] = ij31; _ij31[1] = -1;
for(int iij31 = ij31+1; iij31 < 2; ++iij31)
{
if( j31valid[iij31] && IKabs(cj31array[ij31]-cj31array[iij31]) < IKFAST_SOLUTION_THRESH && IKabs(sj31array[ij31]-sj31array[iij31]) < IKFAST_SOLUTION_THRESH )
{
    j31valid[iij31]=false; _ij31[1] = iij31; break; 
}
}
j31 = j31array[ij31]; cj31 = cj31array[ij31]; sj31 = sj31array[ij31];

{
IKReal j33array[1], cj33array[1], sj33array[1];
bool j33valid[1]={false};
_nj33 = 1;
if( IKabs(((((cj31)*(new_r01)))+(((new_r11)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.00000000000000)*(cj31)*(new_r00)))+(((-1.00000000000000)*(new_r10)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j33array[0]=IKatan2(((((cj31)*(new_r01)))+(((new_r11)*(sj31)))), ((((-1.00000000000000)*(cj31)*(new_r00)))+(((-1.00000000000000)*(new_r10)*(sj31)))));
sj33array[0]=IKsin(j33array[0]);
cj33array[0]=IKcos(j33array[0]);
if( j33array[0] > IKPI )
{
    j33array[0]-=IK2PI;
}
else if( j33array[0] < -IKPI )
{    j33array[0]+=IK2PI;
}
j33valid[0] = true;
for(int ij33 = 0; ij33 < 1; ++ij33)
{
if( !j33valid[ij33] )
{
    continue;
}
_ij33[0] = ij33; _ij33[1] = -1;
for(int iij33 = ij33+1; iij33 < 1; ++iij33)
{
if( j33valid[iij33] && IKabs(cj33array[ij33]-cj33array[iij33]) < IKFAST_SOLUTION_THRESH && IKabs(sj33array[ij33]-sj33array[iij33]) < IKFAST_SOLUTION_THRESH )
{
    j33valid[iij33]=false; _ij33[1] = iij33; break; 
}
}
j33 = j33array[ij33]; cj33 = cj33array[ij33]; sj33 = sj33array[ij33];

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j27;
solution.basesol[0].indices[0] = _ij27[0];
solution.basesol[0].indices[1] = _ij27[1];
solution.basesol[0].maxsolutions = _nj27;
solution.basesol[1].foffset = j28;
solution.basesol[1].indices[0] = _ij28[0];
solution.basesol[1].indices[1] = _ij28[1];
solution.basesol[1].maxsolutions = _nj28;
solution.basesol[2].foffset = j29;
solution.basesol[2].indices[0] = _ij29[0];
solution.basesol[2].indices[1] = _ij29[1];
solution.basesol[2].maxsolutions = _nj29;
solution.basesol[3].foffset = j30;
solution.basesol[3].indices[0] = _ij30[0];
solution.basesol[3].indices[1] = _ij30[1];
solution.basesol[3].maxsolutions = _nj30;
solution.basesol[4].foffset = j31;
solution.basesol[4].indices[0] = _ij31[0];
solution.basesol[4].indices[1] = _ij31[1];
solution.basesol[4].maxsolutions = _nj31;
solution.basesol[5].foffset = j32;
solution.basesol[5].indices[0] = _ij32[0];
solution.basesol[5].indices[1] = _ij32[1];
solution.basesol[5].maxsolutions = _nj32;
solution.basesol[6].foffset = j33;
solution.basesol[6].indices[0] = _ij33[0];
solution.basesol[6].indices[1] = _ij33[1];
solution.basesol[6].maxsolutions = _nj33;
solution.vfree.resize(0);
}
}
}
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j31array[1], cj31array[1], sj31array[1];
bool j31valid[1]={false};
_nj31 = 1;
if( IKabs(((gconst48)*(new_r12)*(sj32))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst48)*(new_r02)*(sj32))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j31array[0]=IKatan2(((gconst48)*(new_r12)*(sj32)), ((gconst48)*(new_r02)*(sj32)));
sj31array[0]=IKsin(j31array[0]);
cj31array[0]=IKcos(j31array[0]);
if( j31array[0] > IKPI )
{
    j31array[0]-=IK2PI;
}
else if( j31array[0] < -IKPI )
{    j31array[0]+=IK2PI;
}
j31valid[0] = true;
for(int ij31 = 0; ij31 < 1; ++ij31)
{
if( !j31valid[ij31] )
{
    continue;
}
_ij31[0] = ij31; _ij31[1] = -1;
for(int iij31 = ij31+1; iij31 < 1; ++iij31)
{
if( j31valid[iij31] && IKabs(cj31array[ij31]-cj31array[iij31]) < IKFAST_SOLUTION_THRESH && IKabs(sj31array[ij31]-sj31array[iij31]) < IKFAST_SOLUTION_THRESH )
{
    j31valid[iij31]=false; _ij31[1] = iij31; break; 
}
}
j31 = j31array[ij31]; cj31 = cj31array[ij31]; sj31 = sj31array[ij31];

{
IKReal dummyeval[1];
IKReal gconst50;
gconst50=IKsign(sj32);
dummyeval[0]=sj32;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[11];
IKReal x107=((cj31)*(new_r12));
IKReal x108=((new_r02)*(sj31));
IKReal x109=((((-1.00000000000000)*(x108)))+(x107));
IKReal x110=((-1.00000000000000)+(new_r22));
IKReal x111=((cj31)*(new_r02));
IKReal x112=((new_r12)*(sj31));
IKReal x113=((x112)+(x111));
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j32)), 6.28318530717959)));
evalcond[1]=x110;
evalcond[2]=new_r20;
evalcond[3]=new_r21;
evalcond[4]=x109;
evalcond[5]=x109;
evalcond[6]=x113;
evalcond[7]=new_r20;
evalcond[8]=new_r21;
evalcond[9]=x110;
evalcond[10]=x113;
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  && IKabs(evalcond[9]) < 0.0000010000000000  && IKabs(evalcond[10]) < 0.0000010000000000  )
{
{
IKReal j33array[1], cj33array[1], sj33array[1];
bool j33valid[1]={false};
_nj33 = 1;
if( IKabs(((((-1.00000000000000)*(cj31)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj31)*(new_r00)))+(((new_r10)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j33array[0]=IKatan2(((((-1.00000000000000)*(cj31)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj31)))), ((((cj31)*(new_r00)))+(((new_r10)*(sj31)))));
sj33array[0]=IKsin(j33array[0]);
cj33array[0]=IKcos(j33array[0]);
if( j33array[0] > IKPI )
{
    j33array[0]-=IK2PI;
}
else if( j33array[0] < -IKPI )
{    j33array[0]+=IK2PI;
}
j33valid[0] = true;
for(int ij33 = 0; ij33 < 1; ++ij33)
{
if( !j33valid[ij33] )
{
    continue;
}
_ij33[0] = ij33; _ij33[1] = -1;
for(int iij33 = ij33+1; iij33 < 1; ++iij33)
{
if( j33valid[iij33] && IKabs(cj33array[ij33]-cj33array[iij33]) < IKFAST_SOLUTION_THRESH && IKabs(sj33array[ij33]-sj33array[iij33]) < IKFAST_SOLUTION_THRESH )
{
    j33valid[iij33]=false; _ij33[1] = iij33; break; 
}
}
j33 = j33array[ij33]; cj33 = cj33array[ij33]; sj33 = sj33array[ij33];

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j27;
solution.basesol[0].indices[0] = _ij27[0];
solution.basesol[0].indices[1] = _ij27[1];
solution.basesol[0].maxsolutions = _nj27;
solution.basesol[1].foffset = j28;
solution.basesol[1].indices[0] = _ij28[0];
solution.basesol[1].indices[1] = _ij28[1];
solution.basesol[1].maxsolutions = _nj28;
solution.basesol[2].foffset = j29;
solution.basesol[2].indices[0] = _ij29[0];
solution.basesol[2].indices[1] = _ij29[1];
solution.basesol[2].maxsolutions = _nj29;
solution.basesol[3].foffset = j30;
solution.basesol[3].indices[0] = _ij30[0];
solution.basesol[3].indices[1] = _ij30[1];
solution.basesol[3].maxsolutions = _nj30;
solution.basesol[4].foffset = j31;
solution.basesol[4].indices[0] = _ij31[0];
solution.basesol[4].indices[1] = _ij31[1];
solution.basesol[4].maxsolutions = _nj31;
solution.basesol[5].foffset = j32;
solution.basesol[5].indices[0] = _ij32[0];
solution.basesol[5].indices[1] = _ij32[1];
solution.basesol[5].maxsolutions = _nj32;
solution.basesol[6].foffset = j33;
solution.basesol[6].indices[0] = _ij33[0];
solution.basesol[6].indices[1] = _ij33[1];
solution.basesol[6].maxsolutions = _nj33;
solution.vfree.resize(0);
}
}
}

} else
{
IKReal x114=((cj31)*(new_r12));
IKReal x115=((new_r02)*(sj31));
IKReal x116=((((-1.00000000000000)*(x115)))+(x114));
IKReal x117=((cj31)*(new_r02));
IKReal x118=((new_r12)*(sj31));
IKReal x119=((x117)+(x118));
evalcond[0]=((-3.14159265358979)+(IKfmod(j32, 6.28318530717959)));
evalcond[1]=((1.00000000000000)+(new_r22));
evalcond[2]=new_r20;
evalcond[3]=new_r21;
evalcond[4]=x116;
evalcond[5]=x116;
evalcond[6]=x119;
evalcond[7]=((-1.00000000000000)*(new_r20));
evalcond[8]=((-1.00000000000000)*(new_r21));
evalcond[9]=((-1.00000000000000)+(((-1.00000000000000)*(new_r22))));
evalcond[10]=((-1.00000000000000)*(x119));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  && IKabs(evalcond[9]) < 0.0000010000000000  && IKabs(evalcond[10]) < 0.0000010000000000  )
{
{
IKReal j33array[1], cj33array[1], sj33array[1];
bool j33valid[1]={false};
_nj33 = 1;
if( IKabs(((((cj31)*(new_r01)))+(((new_r11)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.00000000000000)*(cj31)*(new_r00)))+(((-1.00000000000000)*(new_r10)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j33array[0]=IKatan2(((((cj31)*(new_r01)))+(((new_r11)*(sj31)))), ((((-1.00000000000000)*(cj31)*(new_r00)))+(((-1.00000000000000)*(new_r10)*(sj31)))));
sj33array[0]=IKsin(j33array[0]);
cj33array[0]=IKcos(j33array[0]);
if( j33array[0] > IKPI )
{
    j33array[0]-=IK2PI;
}
else if( j33array[0] < -IKPI )
{    j33array[0]+=IK2PI;
}
j33valid[0] = true;
for(int ij33 = 0; ij33 < 1; ++ij33)
{
if( !j33valid[ij33] )
{
    continue;
}
_ij33[0] = ij33; _ij33[1] = -1;
for(int iij33 = ij33+1; iij33 < 1; ++iij33)
{
if( j33valid[iij33] && IKabs(cj33array[ij33]-cj33array[iij33]) < IKFAST_SOLUTION_THRESH && IKabs(sj33array[ij33]-sj33array[iij33]) < IKFAST_SOLUTION_THRESH )
{
    j33valid[iij33]=false; _ij33[1] = iij33; break; 
}
}
j33 = j33array[ij33]; cj33 = cj33array[ij33]; sj33 = sj33array[ij33];

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j27;
solution.basesol[0].indices[0] = _ij27[0];
solution.basesol[0].indices[1] = _ij27[1];
solution.basesol[0].maxsolutions = _nj27;
solution.basesol[1].foffset = j28;
solution.basesol[1].indices[0] = _ij28[0];
solution.basesol[1].indices[1] = _ij28[1];
solution.basesol[1].maxsolutions = _nj28;
solution.basesol[2].foffset = j29;
solution.basesol[2].indices[0] = _ij29[0];
solution.basesol[2].indices[1] = _ij29[1];
solution.basesol[2].maxsolutions = _nj29;
solution.basesol[3].foffset = j30;
solution.basesol[3].indices[0] = _ij30[0];
solution.basesol[3].indices[1] = _ij30[1];
solution.basesol[3].maxsolutions = _nj30;
solution.basesol[4].foffset = j31;
solution.basesol[4].indices[0] = _ij31[0];
solution.basesol[4].indices[1] = _ij31[1];
solution.basesol[4].maxsolutions = _nj31;
solution.basesol[5].foffset = j32;
solution.basesol[5].indices[0] = _ij32[0];
solution.basesol[5].indices[1] = _ij32[1];
solution.basesol[5].maxsolutions = _nj32;
solution.basesol[6].foffset = j33;
solution.basesol[6].indices[0] = _ij33[0];
solution.basesol[6].indices[1] = _ij33[1];
solution.basesol[6].maxsolutions = _nj33;
solution.vfree.resize(0);
}
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j33array[1], cj33array[1], sj33array[1];
bool j33valid[1]={false};
_nj33 = 1;
if( IKabs(((gconst50)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.00000000000000)*(gconst50)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j33array[0]=IKatan2(((gconst50)*(new_r21)), ((-1.00000000000000)*(gconst50)*(new_r20)));
sj33array[0]=IKsin(j33array[0]);
cj33array[0]=IKcos(j33array[0]);
if( j33array[0] > IKPI )
{
    j33array[0]-=IK2PI;
}
else if( j33array[0] < -IKPI )
{    j33array[0]+=IK2PI;
}
j33valid[0] = true;
for(int ij33 = 0; ij33 < 1; ++ij33)
{
if( !j33valid[ij33] )
{
    continue;
}
_ij33[0] = ij33; _ij33[1] = -1;
for(int iij33 = ij33+1; iij33 < 1; ++iij33)
{
if( j33valid[iij33] && IKabs(cj33array[ij33]-cj33array[iij33]) < IKFAST_SOLUTION_THRESH && IKabs(sj33array[ij33]-sj33array[iij33]) < IKFAST_SOLUTION_THRESH )
{
    j33valid[iij33]=false; _ij33[1] = iij33; break; 
}
}
j33 = j33array[ij33]; cj33 = cj33array[ij33]; sj33 = sj33array[ij33];

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j27;
solution.basesol[0].indices[0] = _ij27[0];
solution.basesol[0].indices[1] = _ij27[1];
solution.basesol[0].maxsolutions = _nj27;
solution.basesol[1].foffset = j28;
solution.basesol[1].indices[0] = _ij28[0];
solution.basesol[1].indices[1] = _ij28[1];
solution.basesol[1].maxsolutions = _nj28;
solution.basesol[2].foffset = j29;
solution.basesol[2].indices[0] = _ij29[0];
solution.basesol[2].indices[1] = _ij29[1];
solution.basesol[2].maxsolutions = _nj29;
solution.basesol[3].foffset = j30;
solution.basesol[3].indices[0] = _ij30[0];
solution.basesol[3].indices[1] = _ij30[1];
solution.basesol[3].maxsolutions = _nj30;
solution.basesol[4].foffset = j31;
solution.basesol[4].indices[0] = _ij31[0];
solution.basesol[4].indices[1] = _ij31[1];
solution.basesol[4].maxsolutions = _nj31;
solution.basesol[5].foffset = j32;
solution.basesol[5].indices[0] = _ij32[0];
solution.basesol[5].indices[1] = _ij32[1];
solution.basesol[5].maxsolutions = _nj32;
solution.basesol[6].foffset = j33;
solution.basesol[6].indices[0] = _ij33[0];
solution.basesol[6].indices[1] = _ij33[1];
solution.basesol[6].maxsolutions = _nj33;
solution.vfree.resize(0);
}
}
}

}

}
}
}

}

}

} else
{
{
IKReal j33array[1], cj33array[1], sj33array[1];
bool j33valid[1]={false};
_nj33 = 1;
if( IKabs(((gconst49)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.00000000000000)*(gconst49)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j33array[0]=IKatan2(((gconst49)*(new_r21)), ((-1.00000000000000)*(gconst49)*(new_r20)));
sj33array[0]=IKsin(j33array[0]);
cj33array[0]=IKcos(j33array[0]);
if( j33array[0] > IKPI )
{
    j33array[0]-=IK2PI;
}
else if( j33array[0] < -IKPI )
{    j33array[0]+=IK2PI;
}
j33valid[0] = true;
for(int ij33 = 0; ij33 < 1; ++ij33)
{
if( !j33valid[ij33] )
{
    continue;
}
_ij33[0] = ij33; _ij33[1] = -1;
for(int iij33 = ij33+1; iij33 < 1; ++iij33)
{
if( j33valid[iij33] && IKabs(cj33array[ij33]-cj33array[iij33]) < IKFAST_SOLUTION_THRESH && IKabs(sj33array[ij33]-sj33array[iij33]) < IKFAST_SOLUTION_THRESH )
{
    j33valid[iij33]=false; _ij33[1] = iij33; break; 
}
}
j33 = j33array[ij33]; cj33 = cj33array[ij33]; sj33 = sj33array[ij33];

{
IKReal dummyeval[1];
IKReal gconst51;
gconst51=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j31array[1], cj31array[1], sj31array[1];
bool j31valid[1]={false};
_nj31 = 1;
if( IKabs(((gconst51)*(new_r12)*(sj32))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst51)*(new_r02)*(sj32))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j31array[0]=IKatan2(((gconst51)*(new_r12)*(sj32)), ((gconst51)*(new_r02)*(sj32)));
sj31array[0]=IKsin(j31array[0]);
cj31array[0]=IKcos(j31array[0]);
if( j31array[0] > IKPI )
{
    j31array[0]-=IK2PI;
}
else if( j31array[0] < -IKPI )
{    j31array[0]+=IK2PI;
}
j31valid[0] = true;
for(int ij31 = 0; ij31 < 1; ++ij31)
{
if( !j31valid[ij31] )
{
    continue;
}
_ij31[0] = ij31; _ij31[1] = -1;
for(int iij31 = ij31+1; iij31 < 1; ++iij31)
{
if( j31valid[iij31] && IKabs(cj31array[ij31]-cj31array[iij31]) < IKFAST_SOLUTION_THRESH && IKabs(sj31array[ij31]-sj31array[iij31]) < IKFAST_SOLUTION_THRESH )
{
    j31valid[iij31]=false; _ij31[1] = iij31; break; 
}
}
j31 = j31array[ij31]; cj31 = cj31array[ij31]; sj31 = sj31array[ij31];

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j27;
solution.basesol[0].indices[0] = _ij27[0];
solution.basesol[0].indices[1] = _ij27[1];
solution.basesol[0].maxsolutions = _nj27;
solution.basesol[1].foffset = j28;
solution.basesol[1].indices[0] = _ij28[0];
solution.basesol[1].indices[1] = _ij28[1];
solution.basesol[1].maxsolutions = _nj28;
solution.basesol[2].foffset = j29;
solution.basesol[2].indices[0] = _ij29[0];
solution.basesol[2].indices[1] = _ij29[1];
solution.basesol[2].maxsolutions = _nj29;
solution.basesol[3].foffset = j30;
solution.basesol[3].indices[0] = _ij30[0];
solution.basesol[3].indices[1] = _ij30[1];
solution.basesol[3].maxsolutions = _nj30;
solution.basesol[4].foffset = j31;
solution.basesol[4].indices[0] = _ij31[0];
solution.basesol[4].indices[1] = _ij31[1];
solution.basesol[4].maxsolutions = _nj31;
solution.basesol[5].foffset = j32;
solution.basesol[5].indices[0] = _ij32[0];
solution.basesol[5].indices[1] = _ij32[1];
solution.basesol[5].maxsolutions = _nj32;
solution.basesol[6].foffset = j33;
solution.basesol[6].indices[0] = _ij33[0];
solution.basesol[6].indices[1] = _ij33[1];
solution.basesol[6].maxsolutions = _nj33;
solution.vfree.resize(0);
}
}
}

}

}
}
}

}

}
}
}
}
}static inline void polyroots8(IKReal rawcoeffs[8+1], IKReal rawroots[8], int& numroots)
{
    using std::complex;
    IKFAST_ASSERT(rawcoeffs[0] != 0);
    const IKReal tol = 128.0*std::numeric_limits<IKReal>::epsilon();
    const IKReal tolsqrt = 8*sqrt(std::numeric_limits<IKReal>::epsilon());
    complex<IKReal> coeffs[8];
    const int maxsteps = 50;
    for(int i = 0; i < 8; ++i) {
        coeffs[i] = complex<IKReal>(rawcoeffs[i+1]/rawcoeffs[0]);
    }
    complex<IKReal> roots[8];
    IKReal err[8];
    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 < 8; ++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 < 8; ++i) {
            if ( err[i] >= tol ) {
                changed = true;
                // evaluate
                complex<IKReal> x = roots[i] + coeffs[0];
                for(int j = 1; j < 8; ++j) {
                    x = roots[i] * x + coeffs[j];
                }
                for(int j = 0; j < 8; ++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[8] = {false};
    for(int i = 0; i < 8; ++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 < 8; ++j) {
                if( abs(roots[i]-roots[j]) < tolsqrt ) {
                    newroot += roots[j];
                    n += 1;
                    visited[j] = true;
                }
            }
            if( n > 1 ) {
                newroot /= n;
            }
            if( IKabs(imag(newroot)) < tol ) {
                rawroots[numroots++] = real(newroot);
            }
        }
    }
}
};


IKFAST_API bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
IKSolver solver;
return solver.ik(eetrans,eerot,pfree,vsolutions);
}

IKFAST_API const char* getKinematicsHash() { return "ad4f1590d2a9b2079648c64e9db33191"; }

IKFAST_API const char* getIKFastVersion() { return "52"; }

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

    std::vector<IKSolution> vsolutions;
    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 = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);

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

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

#endif