sia10d_mesh_manipulator_ikfast_output.cpp

Go to the documentation of this file.

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

#ifdef IKFAST_HEADER
#include IKFAST_HEADER
#endif

#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 isnan
#define isnan _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

#ifdef IKFAST_REAL
typedef IKFAST_REAL IKReal;
#else
typedef double IKReal;
#endif

class IKSolution
{
public:
    void 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>& GetFree() const { return vfree; }

    struct VARIABLE
    {
        VARIABLE() : freeind(-1), fmul(0), foffset(0) {}
        VARIABLE(int freeind, IKReal fmul, IKReal foffset) : freeind(freeind), fmul(fmul), foffset(foffset) {}
        int freeind;
        IKReal fmul, foffset; 
    };

    std::vector<VARIABLE> basesol;       
    std::vector<int> vfree;
};

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

#ifndef IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH ((IKReal)0.000001)
#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( isnan(fy) ) {
        IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
        return IKPI_2;
    }
    else if( isnan(fx) ) {
        return 0;
    }
    return atan2f(fy,fx);
}
inline double IKatan2(double fy, double fx) {
    if( isnan(fy) ) {
        IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
        return IKPI_2;
    }
    else if( isnan(fx) ) {
        return 0;
    }
    return atan2(fy,fx);
}

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

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 j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,j5,cj5,sj5,htj5,j6,cj6,sj6,htj6,j2,cj2,sj2,htj2,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;

bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
vsolutions.resize(0); vsolutions.reserve(8);
j2=pfree[0]; cj2=cos(pfree[0]); sj2=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=((-1.00000000000000)*(r01));
new_r02=((-1.00000000000000)*(r02));
new_px=((((-0.155000000000000)*(r02)))+(px));
new_r10=r10;
new_r11=((-1.00000000000000)*(r11));
new_r12=((-1.00000000000000)*(r12));
new_py=((((-0.155000000000000)*(r12)))+(py));
new_r20=r20;
new_r21=((-1.00000000000000)*(r21));
new_r22=((-1.00000000000000)*(r22));
new_pz=((-0.360000000000000)+(((-0.155000000000000)*(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 j3array[2], cj3array[2], sj3array[2];
bool j3valid[2]={false};
cj3array[0]=((-1.00000000000000)+(((3.85802469135802)*(pp))));
if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
{
    j3valid[0] = j3valid[1] = true;
    j3array[0] = IKacos(cj3array[0]);
    sj3array[0] = IKsin(j3array[0]);
    cj3array[1] = cj3array[0];
    j3array[1] = -j3array[0];
    sj3array[1] = -sj3array[0];
}
else if( isnan(cj3array[0]) )
{
    // probably any value will work
    j3valid[0] = true;
    cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
}
if( j3valid[0] && j3valid[1] && IKabs(cj3array[0]-cj3array[1]) < 0.0001 && IKabs(sj3array[0]-sj3array[1]) < 0.0001 )
{
    j3valid[1]=false;
}
for(int ij3 = 0; ij3 < 2; ++ij3)
{
if( !j3valid[ij3] )
{
    continue;
}
j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];

{
IKReal dummyeval[1];
IKReal gconst0;
IKReal x75=(py)*(py);
IKReal x76=(sj2)*(sj2);
IKReal x77=(cj2)*(cj2);
gconst0=IKsign(((((25.0000000000000)*(px)*(((((-1.00000000000000)*(x75)*(x77)))+(((-1.00000000000000)*(x75)*(x76)))))))+(((25.0000000000000)*(py)*(((((px)*(py)*(x76)))+(((px)*(py)*(x77)))))))));
dummyeval[0]=0;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=(((px)*(px))+((py)*(py)));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
dummyeval[0]=((1.00000000000000)+((cj3)*(cj3))+((((cj2)*(cj2))*((sj3)*(sj3))))+(((2.00000000000000)*(cj3))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[3];
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j3)), 6.28318530717959)));
evalcond[1]=((-1.00000000000000)*(pp));
evalcond[2]=((-1.00000000000000)*(pz));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
{
{
IKReal j0array[2], cj0array[2], sj0array[2];
bool j0valid[2]={false};
IKReal x78=IKatan2(px, py);
j0array[0]=((-1.00000000000000)*(x78));
sj0array[0]=IKsin(j0array[0]);
cj0array[0]=IKcos(j0array[0]);
j0array[1]=((3.14159265358979)+(((-1.00000000000000)*(x78))));
sj0array[1]=IKsin(j0array[1]);
cj0array[1]=IKcos(j0array[1]);
if( j0array[0] > IKPI )
{
    j0array[0]-=IK2PI;
}
else if( j0array[0] < -IKPI )
{    j0array[0]+=IK2PI;
}
j0valid[0] = true;
if( j0array[1] > IKPI )
{
    j0array[1]-=IK2PI;
}
else if( j0array[1] < -IKPI )
{    j0array[1]+=IK2PI;
}
j0valid[1] = true;
if( j0valid[0] && j0valid[1] && IKabs(cj0array[0]-cj0array[1]) < 0.0001 && IKabs(sj0array[0]-sj0array[1]) < 0.0001 )
{
    j0valid[1]=false;
}
for(int ij0 = 0; ij0 < 2; ++ij0)
{
if( !j0valid[ij0] )
{
    continue;
}
j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];

{
IKReal dummyeval[1];
IKReal gconst7;
gconst7=IKsign(((((-2.00000000000000)*(cj0)*(px)*(py)*(sj0)*(sj2)))+(((-1.00000000000000)*(sj2)*((cj0)*(cj0))*((px)*(px))))+(((-1.00000000000000)*(sj2)*((pz)*(pz))))+(((-1.00000000000000)*(sj2)*((py)*(py))*((sj0)*(sj0))))));
dummyeval[0]=((((-2.00000000000000)*(cj0)*(px)*(py)*(sj0)*(sj2)))+(((-1.00000000000000)*(sj2)*((cj0)*(cj0))*((px)*(px))))+(((-1.00000000000000)*(sj2)*((pz)*(pz))))+(((-1.00000000000000)*(sj2)*((py)*(py))*((sj0)*(sj0)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[6];
IKReal x79=((cj0)*(py));
IKReal x80=((px)*(sj0));
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j2)), 6.28318530717959)));
evalcond[1]=((-1.00000000000000)*(pp));
evalcond[2]=((((-1.00000000000000)*(x79)))+(x80));
evalcond[3]=((-1.00000000000000)*(pz));
evalcond[4]=((((cj0)*(px)))+(((py)*(sj0))));
evalcond[5]=((x79)+(((-1.00000000000000)*(x80))));
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  )
{
{
IKReal dummyeval[1];
dummyeval[0]=((((-1.00000000000000)*((cj0)*(cj0))*((px)*(px))))+(((-1.00000000000000)*((pz)*(pz))))+(((-2.00000000000000)*(cj0)*(px)*(py)*(sj0)))+(((-1.00000000000000)*((py)*(py))*((sj0)*(sj0)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
IKReal x81=(pz)*(pz);
IKReal x82=((18.0000000000000)*(x81));
IKReal x83=(cj0)*(cj0);
IKReal x84=(px)*(px);
IKReal x85=((18.0000000000000)*(x83)*(x84));
IKReal x86=(py)*(py);
IKReal x87=(sj0)*(sj0);
IKReal x88=((18.0000000000000)*(x86)*(x87));
IKReal x89=((36.0000000000000)*(cj0)*(px)*(py)*(sj0));
IKReal x90=((x88)+(x89)+(x82)+(x85));
j1array[0]=IKatan2(((-1.00000000000000)*(((IKabs(x90) != 0)?((IKReal)1/(x90)):(IKReal)1.0e30))*(((((-25.0000000000000)*(pp)*(py)*(sj0)))+(((-25.0000000000000)*(cj0)*(pp)*(px)))))), ((25.0000000000000)*(pp)*(pz)*(((IKabs(x90) != 0)?((IKReal)1/(x90)):(IKReal)1.0e30))));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
for(int ij1 = 0; ij1 < 1; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
IKReal x138=((px)*(sj0));
IKReal x139=((cj0)*(py));
IKReal x140=((x138)+(((-1.00000000000000)*(x139))));
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j2)), 6.28318530717959)));
evalcond[1]=((-1.00000000000000)*(pp));
evalcond[2]=x140;
evalcond[3]=((-1.00000000000000)*(pz));
evalcond[4]=((((cj0)*(px)))+(((py)*(sj0))));
evalcond[5]=x140;
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  )
{
{
IKReal dummyeval[1];
IKReal gconst8;
gconst8=IKsign(((((18.0000000000000)*((pz)*(pz))))+(((18.0000000000000)*((cj0)*(cj0))*((px)*(px))))+(((18.0000000000000)*((py)*(py))*((sj0)*(sj0))))+(((36.0000000000000)*(cj0)*(px)*(py)*(sj0)))));
dummyeval[0]=(((((py)*(py))*((sj0)*(sj0))))+((pz)*(pz))+((((cj0)*(cj0))*((px)*(px))))+(((2.00000000000000)*(cj0)*(px)*(py)*(sj0))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
j1array[0]=IKatan2(((gconst8)*(((((25.0000000000000)*(pp)*(py)*(sj0)))+(((25.0000000000000)*(cj0)*(pp)*(px)))))), ((25.0000000000000)*(gconst8)*(pp)*(pz)));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
for(int ij1 = 0; ij1 < 1; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
j1array[0]=IKatan2(((gconst7)*(((((-1.00000000000000)*(cj2)*(px)*(pz)*(sj0)))+(((cj0)*(cj2)*(py)*(pz)))))), ((gconst7)*(((((-1.00000000000000)*(cj0)*(cj2)*(sj0)*((py)*(py))))+(((cj2)*(px)*(py)*((sj0)*(sj0))))+(((cj0)*(cj2)*(sj0)*((px)*(px))))+(((-1.00000000000000)*(cj2)*(px)*(py)*((cj0)*(cj0))))))));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
for(int ij1 = 0; ij1 < 1; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];

rotationfunction0(vsolutions);
}
}

}

}
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}

} else
{
{
IKReal j1array[2], cj1array[2], sj1array[2];
bool j1valid[2]={false};
IKReal x141=((0.360000000000000)*(cj3));
IKReal x142=((0.360000000000000)+(x141));
IKReal x143=(x142)*(x142);
IKReal x144=(cj2)*(cj2);
IKReal x145=(sj3)*(sj3);
IKReal x146=((0.129600000000000)*(x144)*(x145));
IKReal x147=((x143)+(x146));
if( (x147) < (IKReal)-0.00001 )
    continue;
IKReal x148=IKsqrt(x147);
IKReal x149=IKabs(x148);
IKReal x150=((IKabs(x149) != 0)?((IKReal)1/(x149)):(IKReal)1.0e30);
IKReal x151=((pz)*(x150));
if( (x151) < -1-IKFAST_SINCOS_THRESH || (x151) > 1+IKFAST_SINCOS_THRESH )
    continue;
IKReal x152=IKasin(x151);
IKReal x153=((0.360000000000000)*(cj2)*(sj3));
IKReal x154=IKatan2(x142, x153);
j1array[0]=((((-1.00000000000000)*(x154)))+(x152));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
j1array[1]=((3.14159265358979)+(((-1.00000000000000)*(x152)))+(((-1.00000000000000)*(x154))));
sj1array[1]=IKsin(j1array[1]);
cj1array[1]=IKcos(j1array[1]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
if( j1array[1] > IKPI )
{
    j1array[1]-=IK2PI;
}
else if( j1array[1] < -IKPI )
{    j1array[1]+=IK2PI;
}
j1valid[1] = true;
if( j1valid[0] && j1valid[1] && IKabs(cj1array[0]-cj1array[1]) < 0.0001 && IKabs(sj1array[0]-sj1array[1]) < 0.0001 )
{
    j1valid[1]=false;
}
for(int ij1 = 0; ij1 < 2; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];

{
IKReal dummyeval[1];
IKReal gconst1;
gconst1=IKsign(((((-450.000000000000)*(sj1)*((py)*(py))))+(((-450.000000000000)*(sj1)*((px)*(px))))));
dummyeval[0]=((((-1.00000000000000)*(sj1)*((py)*(py))))+(((-1.00000000000000)*(sj1)*((px)*(px)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[5];
IKReal x155=((0.360000000000000)*(cj3));
IKReal x156=((0.360000000000000)+(x155));
IKReal x157=((x156)+(((-1.00000000000000)*(pz))));
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j1)), 6.28318530717959)));
evalcond[1]=((0.259200000000000)+(((0.259200000000000)*(cj3)))+(((-1.00000000000000)*(pp))));
evalcond[2]=((((0.720000000000000)*(pz)))+(((-1.00000000000000)*(pp))));
evalcond[3]=x157;
evalcond[4]=x157;
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  )
{
{
IKReal dummyeval[1];
IKReal gconst2;
gconst2=IKsign(((((-25.0000000000000)*((py)*(py))))+(((-25.0000000000000)*((px)*(px))))));
dummyeval[0]=((((-1.00000000000000)*((px)*(px))))+(((-1.00000000000000)*((py)*(py)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j0array[1], cj0array[1], sj0array[1];
bool j0valid[1]={false};
j0array[0]=IKatan2(((gconst2)*(((((-9.00000000000000)*(px)*(sj2)*(sj3)))+(((9.00000000000000)*(cj2)*(py)*(sj3)))))), ((gconst2)*(((((9.00000000000000)*(py)*(sj2)*(sj3)))+(((9.00000000000000)*(cj2)*(px)*(sj3)))))));
sj0array[0]=IKsin(j0array[0]);
cj0array[0]=IKcos(j0array[0]);
if( j0array[0] > IKPI )
{
    j0array[0]-=IK2PI;
}
else if( j0array[0] < -IKPI )
{    j0array[0]+=IK2PI;
}
j0valid[0] = true;
for(int ij0 = 0; ij0 < 1; ++ij0)
{
if( !j0valid[ij0] )
{
    continue;
}
j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
IKReal x158=((0.360000000000000)*(cj3));
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j1)), 6.28318530717959)));
evalcond[1]=((0.259200000000000)+(((0.259200000000000)*(cj3)))+(((-1.00000000000000)*(pp))));
evalcond[2]=((((-0.720000000000000)*(pz)))+(((-1.00000000000000)*(pp))));
evalcond[3]=((-0.360000000000000)+(((-1.00000000000000)*(x158)))+(((-1.00000000000000)*(pz))));
evalcond[4]=((0.360000000000000)+(x158)+(pz));
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  )
{
{
IKReal dummyeval[1];
IKReal gconst3;
gconst3=IKsign(((((25.0000000000000)*((px)*(px))))+(((25.0000000000000)*((py)*(py))))));
dummyeval[0]=(((px)*(px))+((py)*(py)));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j0array[1], cj0array[1], sj0array[1];
bool j0valid[1]={false};
j0array[0]=IKatan2(((gconst3)*(((((9.00000000000000)*(px)*(sj2)*(sj3)))+(((9.00000000000000)*(cj2)*(py)*(sj3)))))), ((gconst3)*(((((9.00000000000000)*(cj2)*(px)*(sj3)))+(((-9.00000000000000)*(py)*(sj2)*(sj3)))))));
sj0array[0]=IKsin(j0array[0]);
cj0array[0]=IKcos(j0array[0]);
if( j0array[0] > IKPI )
{
    j0array[0]-=IK2PI;
}
else if( j0array[0] < -IKPI )
{    j0array[0]+=IK2PI;
}
j0valid[0] = true;
for(int ij0 = 0; ij0 < 1; ++ij0)
{
if( !j0valid[ij0] )
{
    continue;
}
j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j0array[1], cj0array[1], sj0array[1];
bool j0valid[1]={false};
j0array[0]=IKatan2(((gconst1)*(((((-625.000000000000)*(pp)*(py)))+(((450.000000000000)*(cj1)*(py)*(pz)))+(((-162.000000000000)*(px)*(sj1)*(sj2)*(sj3)))))), ((gconst1)*(((((450.000000000000)*(cj1)*(px)*(pz)))+(((-625.000000000000)*(pp)*(px)))+(((162.000000000000)*(py)*(sj1)*(sj2)*(sj3)))))));
sj0array[0]=IKsin(j0array[0]);
cj0array[0]=IKcos(j0array[0]);
if( j0array[0] > IKPI )
{
    j0array[0]-=IK2PI;
}
else if( j0array[0] < -IKPI )
{    j0array[0]+=IK2PI;
}
j0valid[0] = true;
for(int ij0 = 0; ij0 < 1; ++ij0)
{
if( !j0valid[ij0] )
{
    continue;
}
j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];

rotationfunction0(vsolutions);
}
}

}

}
}
}

}

}

} else
{
{
IKReal j0array[2], cj0array[2], sj0array[2];
bool j0valid[2]={false};
IKReal x159=((-1.00000000000000)*(py));
IKReal x160=IKatan2(x159, px);
IKReal x161=(px)*(px);
IKReal x162=(py)*(py);
IKReal x163=((x162)+(x161));
if( (x163) < (IKReal)-0.00001 )
    continue;
IKReal x164=IKsqrt(x163);
IKReal x165=IKabs(x164);
IKReal x166=((IKabs(x165) != 0)?((IKReal)1/(x165)):(IKReal)1.0e30);
IKReal x167=((0.360000000000000)*(sj2)*(sj3)*(x166));
if( (x167) < -1-IKFAST_SINCOS_THRESH || (x167) > 1+IKFAST_SINCOS_THRESH )
    continue;
IKReal x168=IKasin(x167);
j0array[0]=((((-1.00000000000000)*(x160)))+(x168));
sj0array[0]=IKsin(j0array[0]);
cj0array[0]=IKcos(j0array[0]);
j0array[1]=((3.14159265358979)+(((-1.00000000000000)*(x168)))+(((-1.00000000000000)*(x160))));
sj0array[1]=IKsin(j0array[1]);
cj0array[1]=IKcos(j0array[1]);
if( j0array[0] > IKPI )
{
    j0array[0]-=IK2PI;
}
else if( j0array[0] < -IKPI )
{    j0array[0]+=IK2PI;
}
j0valid[0] = true;
if( j0array[1] > IKPI )
{
    j0array[1]-=IK2PI;
}
else if( j0array[1] < -IKPI )
{    j0array[1]+=IK2PI;
}
j0valid[1] = true;
if( j0valid[0] && j0valid[1] && IKabs(cj0array[0]-cj0array[1]) < 0.0001 && IKabs(sj0array[0]-sj0array[1]) < 0.0001 )
{
    j0valid[1]=false;
}
for(int ij0 = 0; ij0 < 2; ++ij0)
{
if( !j0valid[ij0] )
{
    continue;
}
j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];

{
IKReal dummyeval[1];
IKReal gconst4;
gconst4=IKsign(((((162.000000000000)*(cj0)*(cj3)*(px)))+(((162.000000000000)*(cj3)*(py)*(sj0)))+(((162.000000000000)*(py)*(sj0)))+(((-162.000000000000)*(cj2)*(pz)*(sj3)))+(((162.000000000000)*(cj0)*(px)))));
dummyeval[0]=((((-1.00000000000000)*(cj2)*(pz)*(sj3)))+(((cj3)*(py)*(sj0)))+(((cj0)*(cj3)*(px)))+(((cj0)*(px)))+(((py)*(sj0))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[5];
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j3)), 6.28318530717959)));
evalcond[1]=((-1.00000000000000)*(pp));
evalcond[2]=((((-1.00000000000000)*(cj0)*(py)))+(((px)*(sj0))));
evalcond[3]=((-1.00000000000000)*(pz));
evalcond[4]=((((cj0)*(px)))+(((py)*(sj0))));
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  )
{
{
IKReal dummyeval[1];
IKReal gconst5;
gconst5=IKsign(((((-2.00000000000000)*(cj0)*(px)*(py)*(sj0)*(sj2)))+(((-1.00000000000000)*(sj2)*((cj0)*(cj0))*((px)*(px))))+(((-1.00000000000000)*(sj2)*((pz)*(pz))))+(((-1.00000000000000)*(sj2)*((py)*(py))*((sj0)*(sj0))))));
dummyeval[0]=((((-2.00000000000000)*(cj0)*(px)*(py)*(sj0)*(sj2)))+(((-1.00000000000000)*(sj2)*((cj0)*(cj0))*((px)*(px))))+(((-1.00000000000000)*(sj2)*((pz)*(pz))))+(((-1.00000000000000)*(sj2)*((py)*(py))*((sj0)*(sj0)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[6];
IKReal x169=((cj0)*(py));
IKReal x170=((px)*(sj0));
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j2)), 6.28318530717959)));
evalcond[1]=((-1.00000000000000)*(pp));
evalcond[2]=((((-1.00000000000000)*(x169)))+(x170));
evalcond[3]=((-1.00000000000000)*(pz));
evalcond[4]=((((cj0)*(px)))+(((py)*(sj0))));
evalcond[5]=((((-1.00000000000000)*(x170)))+(x169));
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  )
{
{
IKReal dummyeval[1];
dummyeval[0]=((((-1.00000000000000)*((cj0)*(cj0))*((px)*(px))))+(((-1.00000000000000)*((pz)*(pz))))+(((-2.00000000000000)*(cj0)*(px)*(py)*(sj0)))+(((-1.00000000000000)*((py)*(py))*((sj0)*(sj0)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
IKReal x171=(pz)*(pz);
IKReal x172=((18.0000000000000)*(x171));
IKReal x173=(cj0)*(cj0);
IKReal x174=(px)*(px);
IKReal x175=((18.0000000000000)*(x173)*(x174));
IKReal x176=(py)*(py);
IKReal x177=(sj0)*(sj0);
IKReal x178=((18.0000000000000)*(x176)*(x177));
IKReal x179=((36.0000000000000)*(cj0)*(px)*(py)*(sj0));
IKReal x180=((x179)+(x178)+(x172)+(x175));
j1array[0]=IKatan2(((-1.00000000000000)*(((IKabs(x180) != 0)?((IKReal)1/(x180)):(IKReal)1.0e30))*(((((-25.0000000000000)*(pp)*(py)*(sj0)))+(((-25.0000000000000)*(cj0)*(pp)*(px)))))), ((25.0000000000000)*(pp)*(pz)*(((IKabs(x180) != 0)?((IKReal)1/(x180)):(IKReal)1.0e30))));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
for(int ij1 = 0; ij1 < 1; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
IKReal x181=((px)*(sj0));
IKReal x182=((cj0)*(py));
IKReal x183=((((-1.00000000000000)*(x182)))+(x181));
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j2)), 6.28318530717959)));
evalcond[1]=((-1.00000000000000)*(pp));
evalcond[2]=x183;
evalcond[3]=((-1.00000000000000)*(pz));
evalcond[4]=((((cj0)*(px)))+(((py)*(sj0))));
evalcond[5]=x183;
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  )
{
{
IKReal dummyeval[1];
IKReal gconst6;
gconst6=IKsign(((((18.0000000000000)*((pz)*(pz))))+(((18.0000000000000)*((cj0)*(cj0))*((px)*(px))))+(((18.0000000000000)*((py)*(py))*((sj0)*(sj0))))+(((36.0000000000000)*(cj0)*(px)*(py)*(sj0)))));
dummyeval[0]=(((((py)*(py))*((sj0)*(sj0))))+((pz)*(pz))+((((cj0)*(cj0))*((px)*(px))))+(((2.00000000000000)*(cj0)*(px)*(py)*(sj0))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
j1array[0]=IKatan2(((gconst6)*(((((25.0000000000000)*(pp)*(py)*(sj0)))+(((25.0000000000000)*(cj0)*(pp)*(px)))))), ((25.0000000000000)*(gconst6)*(pp)*(pz)));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
for(int ij1 = 0; ij1 < 1; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
j1array[0]=IKatan2(((gconst5)*(((((-1.00000000000000)*(cj2)*(px)*(pz)*(sj0)))+(((cj0)*(cj2)*(py)*(pz)))))), ((gconst5)*(((((-1.00000000000000)*(cj0)*(cj2)*(sj0)*((py)*(py))))+(((cj2)*(px)*(py)*((sj0)*(sj0))))+(((cj0)*(cj2)*(sj0)*((px)*(px))))+(((-1.00000000000000)*(cj2)*(px)*(py)*((cj0)*(cj0))))))));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
for(int ij1 = 0; ij1 < 1; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}

} else
{
{
IKReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
j1array[0]=IKatan2(((gconst4)*(((((225.000000000000)*(cj3)*(pp)))+(((-450.000000000000)*((pz)*(pz))))+(((225.000000000000)*(pp)))))), ((gconst4)*(((((450.000000000000)*(cj0)*(px)*(pz)))+(((-225.000000000000)*(cj2)*(pp)*(sj3)))+(((450.000000000000)*(py)*(pz)*(sj0)))))));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
for(int ij1 = 0; ij1 < 1; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];

rotationfunction0(vsolutions);
}
}

}

}
}
}

}

}

} else
{
{
IKReal j0array[1], cj0array[1], sj0array[1];
bool j0valid[1]={false};
IKReal x184=(py)*(py);
IKReal x185=(sj2)*(sj2);
IKReal x186=(cj2)*(cj2);
j0array[0]=IKatan2(((gconst0)*(((((9.00000000000000)*(sj2)*(sj3)*(((((-1.00000000000000)*(x184)*(x185)))+(((-1.00000000000000)*(x184)*(x186)))))))+(((9.00000000000000)*(sj2)*(sj3)*(x184)))))), ((gconst0)*(((((-9.00000000000000)*(sj2)*(sj3)*(((((px)*(py)*(x185)))+(((px)*(py)*(x186)))))))+(((9.00000000000000)*(px)*(py)*(sj2)*(sj3)))))));
sj0array[0]=IKsin(j0array[0]);
cj0array[0]=IKcos(j0array[0]);
if( j0array[0] > IKPI )
{
    j0array[0]-=IK2PI;
}
else if( j0array[0] < -IKPI )
{    j0array[0]+=IK2PI;
}
j0valid[0] = true;
for(int ij0 = 0; ij0 < 1; ++ij0)
{
if( !j0valid[ij0] )
{
    continue;
}
j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];

{
IKReal dummyeval[1];
IKReal gconst4;
gconst4=IKsign(((((162.000000000000)*(cj0)*(cj3)*(px)))+(((162.000000000000)*(cj3)*(py)*(sj0)))+(((162.000000000000)*(py)*(sj0)))+(((-162.000000000000)*(cj2)*(pz)*(sj3)))+(((162.000000000000)*(cj0)*(px)))));
dummyeval[0]=((((-1.00000000000000)*(cj2)*(pz)*(sj3)))+(((cj3)*(py)*(sj0)))+(((cj0)*(cj3)*(px)))+(((cj0)*(px)))+(((py)*(sj0))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[5];
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j3)), 6.28318530717959)));
evalcond[1]=((-1.00000000000000)*(pp));
evalcond[2]=((((-1.00000000000000)*(cj0)*(py)))+(((px)*(sj0))));
evalcond[3]=((-1.00000000000000)*(pz));
evalcond[4]=((((cj0)*(px)))+(((py)*(sj0))));
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  )
{
{
IKReal dummyeval[1];
IKReal gconst5;
gconst5=IKsign(((((-2.00000000000000)*(cj0)*(px)*(py)*(sj0)*(sj2)))+(((-1.00000000000000)*(sj2)*((cj0)*(cj0))*((px)*(px))))+(((-1.00000000000000)*(sj2)*((pz)*(pz))))+(((-1.00000000000000)*(sj2)*((py)*(py))*((sj0)*(sj0))))));
dummyeval[0]=((((-2.00000000000000)*(cj0)*(px)*(py)*(sj0)*(sj2)))+(((-1.00000000000000)*(sj2)*((cj0)*(cj0))*((px)*(px))))+(((-1.00000000000000)*(sj2)*((pz)*(pz))))+(((-1.00000000000000)*(sj2)*((py)*(py))*((sj0)*(sj0)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[6];
IKReal x187=((cj0)*(py));
IKReal x188=((px)*(sj0));
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j2)), 6.28318530717959)));
evalcond[1]=((-1.00000000000000)*(pp));
evalcond[2]=((((-1.00000000000000)*(x187)))+(x188));
evalcond[3]=((-1.00000000000000)*(pz));
evalcond[4]=((((cj0)*(px)))+(((py)*(sj0))));
evalcond[5]=((((-1.00000000000000)*(x188)))+(x187));
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  )
{
{
IKReal dummyeval[1];
dummyeval[0]=((((-1.00000000000000)*((cj0)*(cj0))*((px)*(px))))+(((-1.00000000000000)*((pz)*(pz))))+(((-2.00000000000000)*(cj0)*(px)*(py)*(sj0)))+(((-1.00000000000000)*((py)*(py))*((sj0)*(sj0)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
IKReal x189=(pz)*(pz);
IKReal x190=((18.0000000000000)*(x189));
IKReal x191=(cj0)*(cj0);
IKReal x192=(px)*(px);
IKReal x193=((18.0000000000000)*(x191)*(x192));
IKReal x194=(py)*(py);
IKReal x195=(sj0)*(sj0);
IKReal x196=((18.0000000000000)*(x194)*(x195));
IKReal x197=((36.0000000000000)*(cj0)*(px)*(py)*(sj0));
IKReal x198=((x193)+(x190)+(x197)+(x196));
j1array[0]=IKatan2(((-1.00000000000000)*(((IKabs(x198) != 0)?((IKReal)1/(x198)):(IKReal)1.0e30))*(((((-25.0000000000000)*(pp)*(py)*(sj0)))+(((-25.0000000000000)*(cj0)*(pp)*(px)))))), ((25.0000000000000)*(pp)*(pz)*(((IKabs(x198) != 0)?((IKReal)1/(x198)):(IKReal)1.0e30))));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
for(int ij1 = 0; ij1 < 1; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
IKReal x199=((px)*(sj0));
IKReal x200=((cj0)*(py));
IKReal x201=((x199)+(((-1.00000000000000)*(x200))));
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j2)), 6.28318530717959)));
evalcond[1]=((-1.00000000000000)*(pp));
evalcond[2]=x201;
evalcond[3]=((-1.00000000000000)*(pz));
evalcond[4]=((((cj0)*(px)))+(((py)*(sj0))));
evalcond[5]=x201;
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  )
{
{
IKReal dummyeval[1];
IKReal gconst6;
gconst6=IKsign(((((18.0000000000000)*((pz)*(pz))))+(((18.0000000000000)*((cj0)*(cj0))*((px)*(px))))+(((18.0000000000000)*((py)*(py))*((sj0)*(sj0))))+(((36.0000000000000)*(cj0)*(px)*(py)*(sj0)))));
dummyeval[0]=(((((py)*(py))*((sj0)*(sj0))))+((pz)*(pz))+((((cj0)*(cj0))*((px)*(px))))+(((2.00000000000000)*(cj0)*(px)*(py)*(sj0))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
j1array[0]=IKatan2(((gconst6)*(((((25.0000000000000)*(pp)*(py)*(sj0)))+(((25.0000000000000)*(cj0)*(pp)*(px)))))), ((25.0000000000000)*(gconst6)*(pp)*(pz)));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
for(int ij1 = 0; ij1 < 1; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
j1array[0]=IKatan2(((gconst5)*(((((-1.00000000000000)*(cj2)*(px)*(pz)*(sj0)))+(((cj0)*(cj2)*(py)*(pz)))))), ((gconst5)*(((((-1.00000000000000)*(cj0)*(cj2)*(sj0)*((py)*(py))))+(((cj2)*(px)*(py)*((sj0)*(sj0))))+(((cj0)*(cj2)*(sj0)*((px)*(px))))+(((-1.00000000000000)*(cj2)*(px)*(py)*((cj0)*(cj0))))))));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
for(int ij1 = 0; ij1 < 1; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];

rotationfunction0(vsolutions);
}
}

}

}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}

} else
{
{
IKReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
j1array[0]=IKatan2(((gconst4)*(((((225.000000000000)*(cj3)*(pp)))+(((-450.000000000000)*((pz)*(pz))))+(((225.000000000000)*(pp)))))), ((gconst4)*(((((450.000000000000)*(cj0)*(px)*(pz)))+(((-225.000000000000)*(cj2)*(pp)*(sj3)))+(((450.000000000000)*(py)*(pz)*(sj0)))))));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
for(int ij1 = 0; ij1 < 1; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];

rotationfunction0(vsolutions);
}
}

}

}
}
}

}

}
}
}
}
return vsolutions.size()>0;
}
inline void rotationfunction0(std::vector<IKSolution>& vsolutions) {
for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
IKReal x91=((sj1)*(sj3));
IKReal x92=((cj1)*(cj2)*(cj3));
IKReal x93=((x91)+(x92));
IKReal x94=((cj0)*(cj3)*(sj2));
IKReal x95=((sj0)*(x93));
IKReal x96=((x95)+(x94));
IKReal x97=((cj0)*(x93));
IKReal x98=((cj3)*(sj0)*(sj2));
IKReal x99=((x97)+(((-1.00000000000000)*(x98))));
IKReal x100=((cj1)*(sj3));
IKReal x101=((cj2)*(cj3)*(sj1));
IKReal x102=((((-1.00000000000000)*(x101)))+(x100));
IKReal x103=((cj2)*(sj0));
IKReal x104=((cj0)*(cj1)*(sj2));
IKReal x105=((x104)+(x103));
IKReal x106=((cj1)*(sj0)*(sj2));
IKReal x107=((cj0)*(cj2));
IKReal x108=((((-1.00000000000000)*(x107)))+(x106));
IKReal x109=((cj2)*(x100));
IKReal x110=((cj3)*(sj1));
IKReal x111=((((-1.00000000000000)*(x110)))+(x109));
IKReal x112=((cj1)*(cj3));
IKReal x113=((cj2)*(x91));
IKReal x114=((x113)+(x112));
IKReal x115=((-1.00000000000000)*(x114));
IKReal x116=((sj0)*(x111));
IKReal x117=((cj0)*(sj2)*(sj3));
IKReal x118=((x117)+(x116));
IKReal x119=((cj0)*(x111));
IKReal x120=((-1.00000000000000)*(sj0)*(sj2)*(sj3));
IKReal x121=((x120)+(x119));
new_r00=((((r10)*(x96)))+(((r00)*(x99)))+(((r20)*(x102))));
new_r01=((((r11)*(x96)))+(((r21)*(x102)))+(((r01)*(x99))));
new_r02=((((r12)*(x96)))+(((r22)*(x102)))+(((r02)*(x99))));
new_r10=((((r10)*(x108)))+(((-1.00000000000000)*(r20)*(sj1)*(sj2)))+(((r00)*(x105))));
new_r11=((((-1.00000000000000)*(r21)*(sj1)*(sj2)))+(((r01)*(x105)))+(((r11)*(x108))));
new_r12=((((r02)*(x105)))+(((r12)*(x108)))+(((-1.00000000000000)*(r22)*(sj1)*(sj2))));
new_r20=((((r10)*(x118)))+(((r00)*(x121)))+(((r20)*(x115))));
new_r21=((((r01)*(x121)))+(((r21)*(x115)))+(((r11)*(x118))));
new_r22=((((r22)*(x115)))+(((r02)*(x121)))+(((r12)*(x118))));
{
IKReal j5array[2], cj5array[2], sj5array[2];
bool j5valid[2]={false};
cj5array[0]=new_r22;
if( cj5array[0] >= -1-IKFAST_SINCOS_THRESH && cj5array[0] <= 1+IKFAST_SINCOS_THRESH )
{
    j5valid[0] = j5valid[1] = true;
    j5array[0] = IKacos(cj5array[0]);
    sj5array[0] = IKsin(j5array[0]);
    cj5array[1] = cj5array[0];
    j5array[1] = -j5array[0];
    sj5array[1] = -sj5array[0];
}
else if( isnan(cj5array[0]) )
{
    // probably any value will work
    j5valid[0] = true;
    cj5array[0] = 1; sj5array[0] = 0; j5array[0] = 0;
}
if( j5valid[0] && j5valid[1] && IKabs(cj5array[0]-cj5array[1]) < 0.0001 && IKabs(sj5array[0]-sj5array[1]) < 0.0001 )
{
    j5valid[1]=false;
}
for(int ij5 = 0; ij5 < 2; ++ij5)
{
if( !j5valid[ij5] )
{
    continue;
}
j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];

{
IKReal dummyeval[1];
IKReal gconst10;
gconst10=IKsign(sj5);
dummyeval[0]=sj5;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
IKReal gconst9;
gconst9=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];
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j5)), 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 j4array[2], cj4array[2], sj4array[2];
bool j4valid[2]={false};
IKReal x122=IKatan2(new_r02, new_r12);
j4array[0]=((-1.00000000000000)*(x122));
sj4array[0]=IKsin(j4array[0]);
cj4array[0]=IKcos(j4array[0]);
j4array[1]=((3.14159265358979)+(((-1.00000000000000)*(x122))));
sj4array[1]=IKsin(j4array[1]);
cj4array[1]=IKcos(j4array[1]);
if( j4array[0] > IKPI )
{
    j4array[0]-=IK2PI;
}
else if( j4array[0] < -IKPI )
{    j4array[0]+=IK2PI;
}
j4valid[0] = true;
if( j4array[1] > IKPI )
{
    j4array[1]-=IK2PI;
}
else if( j4array[1] < -IKPI )
{    j4array[1]+=IK2PI;
}
j4valid[1] = true;
if( j4valid[0] && j4valid[1] && IKabs(cj4array[0]-cj4array[1]) < 0.0001 && IKabs(sj4array[0]-sj4array[1]) < 0.0001 )
{
    j4valid[1]=false;
}
for(int ij4 = 0; ij4 < 2; ++ij4)
{
if( !j4valid[ij4] )
{
    continue;
}
j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];

{
IKReal j6array[1], cj6array[1], sj6array[1];
bool j6valid[1]={false};
j6array[0]=IKatan2(((((-1.00000000000000)*(cj4)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj4)))), ((((cj4)*(new_r00)))+(((new_r10)*(sj4)))));
sj6array[0]=IKsin(j6array[0]);
cj6array[0]=IKcos(j6array[0]);
if( j6array[0] > IKPI )
{
    j6array[0]-=IK2PI;
}
else if( j6array[0] < -IKPI )
{    j6array[0]+=IK2PI;
}
j6valid[0] = true;
for(int ij6 = 0; ij6 < 1; ++ij6)
{
if( !j6valid[ij6] )
{
    continue;
}
j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j0;
solution.basesol[1].foffset = j1;
solution.basesol[2].foffset = j2;
solution.basesol[3].foffset = j3;
solution.basesol[4].foffset = j4;
solution.basesol[5].foffset = j5;
solution.basesol[6].foffset = j6;
solution.vfree.resize(0);
}
}
}
}
}

} else
{
IKReal x123=((1.00000000000000)+(new_r22));
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j5)), 6.28318530717959)));
evalcond[1]=x123;
evalcond[2]=new_r20;
evalcond[3]=new_r21;
evalcond[4]=new_r20;
evalcond[5]=new_r21;
evalcond[6]=x123;
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 j4array[2], cj4array[2], sj4array[2];
bool j4valid[2]={false};
IKReal x124=IKatan2(new_r02, new_r12);
j4array[0]=((-1.00000000000000)*(x124));
sj4array[0]=IKsin(j4array[0]);
cj4array[0]=IKcos(j4array[0]);
j4array[1]=((3.14159265358979)+(((-1.00000000000000)*(x124))));
sj4array[1]=IKsin(j4array[1]);
cj4array[1]=IKcos(j4array[1]);
if( j4array[0] > IKPI )
{
    j4array[0]-=IK2PI;
}
else if( j4array[0] < -IKPI )
{    j4array[0]+=IK2PI;
}
j4valid[0] = true;
if( j4array[1] > IKPI )
{
    j4array[1]-=IK2PI;
}
else if( j4array[1] < -IKPI )
{    j4array[1]+=IK2PI;
}
j4valid[1] = true;
if( j4valid[0] && j4valid[1] && IKabs(cj4array[0]-cj4array[1]) < 0.0001 && IKabs(sj4array[0]-sj4array[1]) < 0.0001 )
{
    j4valid[1]=false;
}
for(int ij4 = 0; ij4 < 2; ++ij4)
{
if( !j4valid[ij4] )
{
    continue;
}
j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];

{
IKReal j6array[1], cj6array[1], sj6array[1];
bool j6valid[1]={false};
j6array[0]=IKatan2(((((cj4)*(new_r01)))+(((new_r11)*(sj4)))), ((((-1.00000000000000)*(new_r10)*(sj4)))+(((-1.00000000000000)*(cj4)*(new_r00)))));
sj6array[0]=IKsin(j6array[0]);
cj6array[0]=IKcos(j6array[0]);
if( j6array[0] > IKPI )
{
    j6array[0]-=IK2PI;
}
else if( j6array[0] < -IKPI )
{    j6array[0]+=IK2PI;
}
j6valid[0] = true;
for(int ij6 = 0; ij6 < 1; ++ij6)
{
if( !j6valid[ij6] )
{
    continue;
}
j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j0;
solution.basesol[1].foffset = j1;
solution.basesol[2].foffset = j2;
solution.basesol[3].foffset = j3;
solution.basesol[4].foffset = j4;
solution.basesol[5].foffset = j5;
solution.basesol[6].foffset = j6;
solution.vfree.resize(0);
}
}
}
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j4array[1], cj4array[1], sj4array[1];
bool j4valid[1]={false};
j4array[0]=IKatan2(((gconst9)*(new_r12)*(sj5)), ((gconst9)*(new_r02)*(sj5)));
sj4array[0]=IKsin(j4array[0]);
cj4array[0]=IKcos(j4array[0]);
if( j4array[0] > IKPI )
{
    j4array[0]-=IK2PI;
}
else if( j4array[0] < -IKPI )
{    j4array[0]+=IK2PI;
}
j4valid[0] = true;
for(int ij4 = 0; ij4 < 1; ++ij4)
{
if( !j4valid[ij4] )
{
    continue;
}
j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];

{
IKReal dummyeval[1];
IKReal gconst11;
gconst11=IKsign(sj5);
dummyeval[0]=sj5;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[11];
IKReal x125=((cj4)*(new_r12));
IKReal x126=((new_r02)*(sj4));
IKReal x127=((((-1.00000000000000)*(x126)))+(x125));
IKReal x128=((new_r12)*(sj4));
IKReal x129=((cj4)*(new_r02));
IKReal x130=((x128)+(x129));
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j5)), 6.28318530717959)));
evalcond[1]=((-1.00000000000000)+(new_r22));
evalcond[2]=new_r20;
evalcond[3]=new_r21;
evalcond[4]=x127;
evalcond[5]=x127;
evalcond[6]=x130;
evalcond[7]=x130;
evalcond[8]=((-1.00000000000000)*(new_r20));
evalcond[9]=((-1.00000000000000)*(new_r21));
evalcond[10]=((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  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  && IKabs(evalcond[9]) < 0.0000010000000000  && IKabs(evalcond[10]) < 0.0000010000000000  )
{
{
IKReal j6array[1], cj6array[1], sj6array[1];
bool j6valid[1]={false};
j6array[0]=IKatan2(((((-1.00000000000000)*(cj4)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj4)))), ((((cj4)*(new_r00)))+(((new_r10)*(sj4)))));
sj6array[0]=IKsin(j6array[0]);
cj6array[0]=IKcos(j6array[0]);
if( j6array[0] > IKPI )
{
    j6array[0]-=IK2PI;
}
else if( j6array[0] < -IKPI )
{    j6array[0]+=IK2PI;
}
j6valid[0] = true;
for(int ij6 = 0; ij6 < 1; ++ij6)
{
if( !j6valid[ij6] )
{
    continue;
}
j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j0;
solution.basesol[1].foffset = j1;
solution.basesol[2].foffset = j2;
solution.basesol[3].foffset = j3;
solution.basesol[4].foffset = j4;
solution.basesol[5].foffset = j5;
solution.basesol[6].foffset = j6;
solution.vfree.resize(0);
}
}
}

} else
{
IKReal x131=((cj4)*(new_r12));
IKReal x132=((new_r02)*(sj4));
IKReal x133=((x131)+(((-1.00000000000000)*(x132))));
IKReal x134=((new_r12)*(sj4));
IKReal x135=((cj4)*(new_r02));
IKReal x136=((x135)+(x134));
IKReal x137=((1.00000000000000)+(new_r22));
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j5)), 6.28318530717959)));
evalcond[1]=x137;
evalcond[2]=new_r20;
evalcond[3]=new_r21;
evalcond[4]=x133;
evalcond[5]=x133;
evalcond[6]=x136;
evalcond[7]=((-1.00000000000000)*(x136));
evalcond[8]=new_r20;
evalcond[9]=new_r21;
evalcond[10]=x137;
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 j6array[1], cj6array[1], sj6array[1];
bool j6valid[1]={false};
j6array[0]=IKatan2(((((cj4)*(new_r01)))+(((new_r11)*(sj4)))), ((((-1.00000000000000)*(new_r10)*(sj4)))+(((-1.00000000000000)*(cj4)*(new_r00)))));
sj6array[0]=IKsin(j6array[0]);
cj6array[0]=IKcos(j6array[0]);
if( j6array[0] > IKPI )
{
    j6array[0]-=IK2PI;
}
else if( j6array[0] < -IKPI )
{    j6array[0]+=IK2PI;
}
j6valid[0] = true;
for(int ij6 = 0; ij6 < 1; ++ij6)
{
if( !j6valid[ij6] )
{
    continue;
}
j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j0;
solution.basesol[1].foffset = j1;
solution.basesol[2].foffset = j2;
solution.basesol[3].foffset = j3;
solution.basesol[4].foffset = j4;
solution.basesol[5].foffset = j5;
solution.basesol[6].foffset = j6;
solution.vfree.resize(0);
}
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j6array[1], cj6array[1], sj6array[1];
bool j6valid[1]={false};
j6array[0]=IKatan2(((gconst11)*(new_r21)), ((-1.00000000000000)*(gconst11)*(new_r20)));
sj6array[0]=IKsin(j6array[0]);
cj6array[0]=IKcos(j6array[0]);
if( j6array[0] > IKPI )
{
    j6array[0]-=IK2PI;
}
else if( j6array[0] < -IKPI )
{    j6array[0]+=IK2PI;
}
j6valid[0] = true;
for(int ij6 = 0; ij6 < 1; ++ij6)
{
if( !j6valid[ij6] )
{
    continue;
}
j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j0;
solution.basesol[1].foffset = j1;
solution.basesol[2].foffset = j2;
solution.basesol[3].foffset = j3;
solution.basesol[4].foffset = j4;
solution.basesol[5].foffset = j5;
solution.basesol[6].foffset = j6;
solution.vfree.resize(0);
}
}
}

}

}
}
}

}

}

} else
{
{
IKReal j6array[1], cj6array[1], sj6array[1];
bool j6valid[1]={false};
j6array[0]=IKatan2(((gconst10)*(new_r21)), ((-1.00000000000000)*(gconst10)*(new_r20)));
sj6array[0]=IKsin(j6array[0]);
cj6array[0]=IKcos(j6array[0]);
if( j6array[0] > IKPI )
{
    j6array[0]-=IK2PI;
}
else if( j6array[0] < -IKPI )
{    j6array[0]+=IK2PI;
}
j6valid[0] = true;
for(int ij6 = 0; ij6 < 1; ++ij6)
{
if( !j6valid[ij6] )
{
    continue;
}
j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];

{
IKReal dummyeval[1];
IKReal gconst12;
gconst12=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 j4array[1], cj4array[1], sj4array[1];
bool j4valid[1]={false};
j4array[0]=IKatan2(((gconst12)*(new_r12)*(sj5)), ((gconst12)*(new_r02)*(sj5)));
sj4array[0]=IKsin(j4array[0]);
cj4array[0]=IKcos(j4array[0]);
if( j4array[0] > IKPI )
{
    j4array[0]-=IK2PI;
}
else if( j4array[0] < -IKPI )
{    j4array[0]+=IK2PI;
}
j4valid[0] = true;
for(int ij4 = 0; ij4 < 1; ++ij4)
{
if( !j4valid[ij4] )
{
    continue;
}
j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j0;
solution.basesol[1].foffset = j1;
solution.basesol[2].foffset = j2;
solution.basesol[3].foffset = j3;
solution.basesol[4].foffset = j4;
solution.basesol[5].foffset = j5;
solution.basesol[6].foffset = j6;
solution.vfree.resize(0);
}
}
}

}

}
}
}

}

}
}
}
}
}};


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 "97103586e77ffd381ff66ffa06718176"; }

#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