SIA20D_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>
#include <iostream>

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

#ifndef __PRETTY_FUNCTION__
#define __PRETTY_FUNCTION__ __func__
#endif

#define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }

#endif

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

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

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

// 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() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) {
            indices[0] = indices[1] = -1;
        }
        IKReal fmul, foffset; 
        signed char freeind; 
        unsigned char maxsolutions; 
        unsigned char indices[2]; 
    };

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

    bool 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 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 IKsqr(float f) { return f*f; }
inline double IKsqr(double f) { return f*f; }

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

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

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

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

inline float IKasin(float f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return -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]=((((0.420000000000000)*(x23)))+(((x26)*(((((0.180000000000000)*(x23)))+(((0.180000000000000)*(x22)))))))+(((x20)*(((((-0.180000000000000)*(x27)))+(((-0.180000000000000)*(x28)))))))+(((x5)*(((((-0.420000000000000)*(x7)))+(((0.420000000000000)*(x6)))))))+(((0.490000000000000)*(x0)*(x9))));
eerot[3]=((((x19)*(x59)))+(((x32)*(x55))));
eerot[4]=((((-1.00000000000000)*(x32)*(x59)))+(((x19)*(x55))));
eerot[5]=((((x26)*(x49)))+(((-1.00000000000000)*(x20)*(x53))));
eetrans[1]=((((x26)*(((((0.180000000000000)*(x47)))+(((0.180000000000000)*(x48)))))))+(((0.490000000000000)*(x3)*(x9)))+(((x5)*(((((-0.420000000000000)*(x37)))+(((-0.420000000000000)*(x38)))))))+(((x20)*(((((-0.180000000000000)*(x52)))+(((-0.180000000000000)*(x51)))))))+(((0.420000000000000)*(x48))));
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.410000000000000)+(((x26)*(((((0.180000000000000)*(x71)))+(((0.180000000000000)*(x70)))))))+(((x20)*(((((-0.180000000000000)*(x66)))+(((0.180000000000000)*(x67)))))))+(((0.420000000000000)*(x70)))+(((0.420000000000000)*(x71)))+(((0.490000000000000)*(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 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;
unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5,_ij6[2], _nj6,_ij2[2], _nj2;

bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
j0=numeric_limits<IKReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IKReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j3=numeric_limits<IKReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IKReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IKReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1; j6=numeric_limits<IKReal>::quiet_NaN(); _ij6[0] = -1; _ij6[1] = -1; _nj6 = -1;  _ij2[0] = -1; _ij2[1] = -1; _nj2 = 0; 
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.180000000000000)*(r02)))+(px));
new_r10=r10;
new_r11=((-1.00000000000000)*(r11));
new_r12=((-1.00000000000000)*(r12));
new_py=((py)+(((-0.180000000000000)*(r12))));
new_r20=r20;
new_r21=((-1.00000000000000)*(r21));
new_r22=((-1.00000000000000)*(r22));
new_pz=((-0.410000000000000)+(((-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 j3array[2], cj3array[2], sj3array[2];
bool j3valid[2]={false};
_nj3 = 2;
cj3array[0]=((-1.01190476190476)+(((2.42954324586978)*(pp))));
if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
{
    j3valid[0] = j3valid[1] = true;
    j3array[0] = IKacos(cj3array[0]);
    sj3array[0] = IKsin(j3array[0]);
    cj3array[1] = cj3array[0];
    j3array[1] = -j3array[0];
    sj3array[1] = -sj3array[0];
}
else if( isnan(cj3array[0]) )
{
    // probably any value will work
    j3valid[0] = true;
    cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
}
for(int ij3 = 0; ij3 < 2; ++ij3)
{
if( !j3valid[ij3] )
{
    continue;
}
_ij3[0] = ij3; _ij3[1] = -1;
for(int iij3 = ij3+1; iij3 < 2; ++iij3)
{
if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
{
    j3valid[iij3]=false; _ij3[1] = iij3; break; 
}
}
j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];

{
IKReal dummyeval[1];
dummyeval[0]=(((px)*(px))+((py)*(py)));
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.36111111111111)+((cj3)*(cj3))+((((cj2)*(cj2))*((sj3)*(sj3))))+(((2.33333333333333)*(cj3))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j1array[2], cj1array[2], sj1array[2];
bool j1valid[2]={false};
_nj1 = 2;
IKReal x75=((0.420000000000000)*(cj3));
IKReal x76=(cj2)*(cj2);
IKReal x77=(sj3)*(sj3);
IKReal x78=((0.176400000000000)*(x76)*(x77));
IKReal x79=((0.490000000000000)+(x75));
IKReal x80=(x79)*(x79);
IKReal x81=((x78)+(x80));
if( (x81) < (IKReal)-0.00001 )
    continue;
IKReal x82=IKsqrt(x81);
IKReal x83=IKabs(x82);
IKReal x84=((IKabs(x83) != 0)?((IKReal)1/(x83)):(IKReal)1.0e30);
IKReal x85=((pz)*(x84));
if( (x85) < -1-IKFAST_SINCOS_THRESH || (x85) > 1+IKFAST_SINCOS_THRESH )
    continue;
IKReal x86=IKasin(x85);
IKReal x87=((-0.490000000000000)+(((-1.00000000000000)*(x75))));
IKReal x88=((-0.420000000000000)*(cj2)*(sj3));
if( IKabs(x87) < IKFAST_ATAN2_MAGTHRESH && IKabs(x88) < IKFAST_ATAN2_MAGTHRESH )
    continue;
IKReal x89=IKatan2(x87, x88);
j1array[0]=((((-1.00000000000000)*(x89)))+(((-1.00000000000000)*(x86))));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
j1array[1]=((3.14159265358979)+(x86)+(((-1.00000000000000)*(x89))));
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;
for(int ij1 = 0; ij1 < 2; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
_ij1[0] = ij1; _ij1[1] = -1;
for(int iij1 = ij1+1; iij1 < 2; ++iij1)
{
if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
{
    j1valid[iij1]=false; _ij1[1] = iij1; break; 
}
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];

{
IKReal dummyeval[1];
IKReal gconst0;
gconst0=IKsign(((((-2450.00000000000)*(sj1)*((px)*(px))))+(((-2450.00000000000)*(sj1)*((py)*(py))))));
dummyeval[0]=((((-1.00000000000000)*(sj1)*((py)*(py))))+(((-1.00000000000000)*(sj1)*((px)*(px)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[5];
IKReal x90=((0.420000000000000)*(cj3));
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j1)), 6.28318530717959)));
evalcond[1]=((0.416500000000000)+(((0.411600000000000)*(cj3)))+(((-1.00000000000000)*(pp))));
evalcond[2]=((-0.490000000000000)+(pz)+(((-1.00000000000000)*(x90))));
evalcond[3]=((-0.0637000000000000)+(((0.980000000000000)*(pz)))+(((-1.00000000000000)*(pp))));
evalcond[4]=((0.490000000000000)+(x90)+(((-1.00000000000000)*(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 gconst1;
gconst1=IKsign(((((-50.0000000000000)*((px)*(px))))+(((-50.0000000000000)*((py)*(py))))));
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};
_nj0 = 1;
if( IKabs(((gconst1)*(((((-21.0000000000000)*(px)*(sj2)*(sj3)))+(((21.0000000000000)*(cj2)*(py)*(sj3))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((21.0000000000000)*(cj2)*(px)*(sj3)))+(((21.0000000000000)*(py)*(sj2)*(sj3))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j0array[0]=IKatan2(((gconst1)*(((((-21.0000000000000)*(px)*(sj2)*(sj3)))+(((21.0000000000000)*(cj2)*(py)*(sj3)))))), ((gconst1)*(((((21.0000000000000)*(cj2)*(px)*(sj3)))+(((21.0000000000000)*(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;
}
_ij0[0] = ij0; _ij0[1] = -1;
for(int iij0 = ij0+1; iij0 < 1; ++iij0)
{
if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
{
    j0valid[iij0]=false; _ij0[1] = iij0; break; 
}
}
j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
{
IKReal evalcond[4];
IKReal x91=IKsin(j0);
IKReal x92=IKcos(j0);
evalcond[0]=((((-0.420000000000000)*(sj2)*(sj3)))+(((px)*(x91)))+(((-1.00000000000000)*(py)*(x92))));
evalcond[1]=((((-0.420000000000000)*(cj2)*(sj3)))+(((-1.00000000000000)*(px)*(x92)))+(((-1.00000000000000)*(py)*(x91))));
evalcond[2]=((((-1.00000000000000)*(px)*(sj2)*(x92)))+(((cj2)*(py)*(x92)))+(((-1.00000000000000)*(py)*(sj2)*(x91)))+(((-1.00000000000000)*(cj2)*(px)*(x91))));
evalcond[3]=((((-0.420000000000000)*(sj3)))+(((-1.00000000000000)*(py)*(sj2)*(x92)))+(((-1.00000000000000)*(cj2)*(px)*(x92)))+(((-1.00000000000000)*(cj2)*(py)*(x91)))+(((px)*(sj2)*(x91))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

rotationfunction0(vsolutions);
}
}

}

}

} else
{
IKReal x154=((0.420000000000000)*(cj3));
IKReal x155=((0.490000000000000)+(x154)+(pz));
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j1)), 6.28318530717959)));
evalcond[1]=((0.416500000000000)+(((0.411600000000000)*(cj3)))+(((-1.00000000000000)*(pp))));
evalcond[2]=x155;
evalcond[3]=((-0.0637000000000000)+(((-0.980000000000000)*(pz)))+(((-1.00000000000000)*(pp))));
evalcond[4]=x155;
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(((((50.0000000000000)*((px)*(px))))+(((50.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};
_nj0 = 1;
if( IKabs(((gconst2)*(((((21.0000000000000)*(cj2)*(py)*(sj3)))+(((21.0000000000000)*(px)*(sj2)*(sj3))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((21.0000000000000)*(cj2)*(px)*(sj3)))+(((-21.0000000000000)*(py)*(sj2)*(sj3))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j0array[0]=IKatan2(((gconst2)*(((((21.0000000000000)*(cj2)*(py)*(sj3)))+(((21.0000000000000)*(px)*(sj2)*(sj3)))))), ((gconst2)*(((((21.0000000000000)*(cj2)*(px)*(sj3)))+(((-21.0000000000000)*(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;
}
_ij0[0] = ij0; _ij0[1] = -1;
for(int iij0 = ij0+1; iij0 < 1; ++iij0)
{
if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
{
    j0valid[iij0]=false; _ij0[1] = iij0; break; 
}
}
j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
{
IKReal evalcond[4];
IKReal x156=IKsin(j0);
IKReal x157=IKcos(j0);
evalcond[0]=((((-0.420000000000000)*(sj2)*(sj3)))+(((-1.00000000000000)*(py)*(x157)))+(((px)*(x156))));
evalcond[1]=((((-1.00000000000000)*(py)*(x156)))+(((-1.00000000000000)*(px)*(x157)))+(((0.420000000000000)*(cj2)*(sj3))));
evalcond[2]=((((cj2)*(py)*(x157)))+(((-1.00000000000000)*(cj2)*(px)*(x156)))+(((py)*(sj2)*(x156)))+(((px)*(sj2)*(x157))));
evalcond[3]=((((-0.420000000000000)*(sj3)))+(((cj2)*(py)*(x156)))+(((-1.00000000000000)*(py)*(sj2)*(x157)))+(((px)*(sj2)*(x156)))+(((cj2)*(px)*(x157))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

rotationfunction0(vsolutions);
}
}

}

}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j0array[1], cj0array[1], sj0array[1];
bool j0valid[1]={false};
_nj0 = 1;
if( IKabs(((gconst0)*(((((-2500.00000000000)*(pp)*(py)))+(((-1029.00000000000)*(px)*(sj1)*(sj2)*(sj3)))+(((2450.00000000000)*(cj1)*(py)*(pz)))+(((-159.250000000000)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((2450.00000000000)*(cj1)*(px)*(pz)))+(((1029.00000000000)*(py)*(sj1)*(sj2)*(sj3)))+(((-2500.00000000000)*(pp)*(px)))+(((-159.250000000000)*(px))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j0array[0]=IKatan2(((gconst0)*(((((-2500.00000000000)*(pp)*(py)))+(((-1029.00000000000)*(px)*(sj1)*(sj2)*(sj3)))+(((2450.00000000000)*(cj1)*(py)*(pz)))+(((-159.250000000000)*(py)))))), ((gconst0)*(((((2450.00000000000)*(cj1)*(px)*(pz)))+(((1029.00000000000)*(py)*(sj1)*(sj2)*(sj3)))+(((-2500.00000000000)*(pp)*(px)))+(((-159.250000000000)*(px)))))));
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;
}
_ij0[0] = ij0; _ij0[1] = -1;
for(int iij0 = ij0+1; iij0 < 1; ++iij0)
{
if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
{
    j0valid[iij0]=false; _ij0[1] = iij0; break; 
}
}
j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
{
IKReal evalcond[6];
IKReal x158=IKsin(j0);
IKReal x159=IKcos(j0);
evalcond[0]=((((-0.420000000000000)*(sj2)*(sj3)))+(((-1.00000000000000)*(py)*(x159)))+(((px)*(x158))));
evalcond[1]=((-0.0637000000000000)+(((0.980000000000000)*(cj1)*(pz)))+(((0.980000000000000)*(py)*(sj1)*(x158)))+(((-1.00000000000000)*(pp)))+(((0.980000000000000)*(px)*(sj1)*(x159))));
evalcond[2]=((0.490000000000000)+(((-1.00000000000000)*(cj1)*(pz)))+(((-1.00000000000000)*(px)*(sj1)*(x159)))+(((0.420000000000000)*(cj3)))+(((-1.00000000000000)*(py)*(sj1)*(x158))));
evalcond[3]=((((-1.00000000000000)*(py)*(x158)))+(((-0.420000000000000)*(cj1)*(cj2)*(sj3)))+(((-1.00000000000000)*(px)*(x159)))+(((0.490000000000000)*(sj1)))+(((0.420000000000000)*(cj3)*(sj1))));
evalcond[4]=((((cj2)*(py)*(x159)))+(((-1.00000000000000)*(cj2)*(px)*(x158)))+(((-1.00000000000000)*(cj1)*(py)*(sj2)*(x158)))+(((pz)*(sj1)*(sj2)))+(((-1.00000000000000)*(cj1)*(px)*(sj2)*(x159))));
evalcond[5]=((((-0.420000000000000)*(sj3)))+(((-1.00000000000000)*(py)*(sj2)*(x159)))+(((-1.00000000000000)*(cj1)*(cj2)*(py)*(x158)))+(((px)*(sj2)*(x158)))+(((cj2)*(pz)*(sj1)))+(((-1.00000000000000)*(cj1)*(cj2)*(px)*(x159))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
{
continue;
}
}

rotationfunction0(vsolutions);
}
}

}

}
}
}

}

}

} else
{
{
IKReal j0array[2], cj0array[2], sj0array[2];
bool j0valid[2]={false};
_nj0 = 2;
IKReal x160=(px)*(px);
IKReal x161=(py)*(py);
IKReal x162=((x160)+(x161));
if( (x162) < (IKReal)-0.00001 )
    continue;
IKReal x163=IKsqrt(x162);
IKReal x164=IKabs(x163);
IKReal x165=((IKabs(x164) != 0)?((IKReal)1/(x164)):(IKReal)1.0e30);
IKReal x166=((0.420000000000000)*(sj2)*(sj3)*(x165));
if( (x166) < -1-IKFAST_SINCOS_THRESH || (x166) > 1+IKFAST_SINCOS_THRESH )
    continue;
IKReal x167=IKasin(x166);
IKReal x168=((-1.00000000000000)*(py));
if( IKabs(x168) < IKFAST_ATAN2_MAGTHRESH && IKabs(px) < IKFAST_ATAN2_MAGTHRESH )
    continue;
IKReal x169=IKatan2(x168, px);
j0array[0]=((((-1.00000000000000)*(x169)))+(x167));
sj0array[0]=IKsin(j0array[0]);
cj0array[0]=IKcos(j0array[0]);
j0array[1]=((3.14159265358979)+(((-1.00000000000000)*(x169)))+(((-1.00000000000000)*(x167))));
sj0array[1]=IKsin(j0array[1]);
cj0array[1]=IKcos(j0array[1]);
if( j0array[0] > IKPI )
{
    j0array[0]-=IK2PI;
}
else if( j0array[0] < -IKPI )
{    j0array[0]+=IK2PI;
}
j0valid[0] = true;
if( j0array[1] > IKPI )
{
    j0array[1]-=IK2PI;
}
else if( j0array[1] < -IKPI )
{    j0array[1]+=IK2PI;
}
j0valid[1] = true;
for(int ij0 = 0; ij0 < 2; ++ij0)
{
if( !j0valid[ij0] )
{
    continue;
}
_ij0[0] = ij0; _ij0[1] = -1;
for(int iij0 = ij0+1; iij0 < 2; ++iij0)
{
if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
{
    j0valid[iij0]=false; _ij0[1] = iij0; break; 
}
}
j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
{
IKReal evalcond[2];
IKReal x170=(cj2)*(cj2);
IKReal x171=(py)*(py);
IKReal x172=(sj2)*(sj2);
IKReal x173=(px)*(px);
IKReal x174=IKsin(j0);
IKReal x175=IKcos(j0);
IKReal x176=((px)*(py)*(x172));
IKReal x177=((px)*(py)*(x170));
IKReal x178=((x177)+(x176));
evalcond[0]=((((x175)*(((((-1.00000000000000)*(x170)*(x171)))+(((-1.00000000000000)*(x171)*(x172)))))))+(((x174)*(x178)))+(((-0.420000000000000)*(py)*(sj2)*(sj3))));
evalcond[1]=((((-1.00000000000000)*(x175)*(x178)))+(((x174)*(((((x172)*(x173)))+(((x170)*(x173)))))))+(((-0.420000000000000)*(px)*(sj2)*(sj3))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
{
continue;
}
}

{
IKReal dummyeval[1];
IKReal gconst3;
gconst3=IKsign(((((-1029.00000000000)*(cj2)*(pz)*(sj3)))+(((1200.50000000000)*(cj0)*(px)))+(((1029.00000000000)*(cj0)*(cj3)*(px)))+(((1200.50000000000)*(py)*(sj0)))+(((1029.00000000000)*(cj3)*(py)*(sj0)))));
dummyeval[0]=((((1.16666666666667)*(cj0)*(px)))+(((1.16666666666667)*(py)*(sj0)))+(((-1.00000000000000)*(cj2)*(pz)*(sj3)))+(((cj3)*(py)*(sj0)))+(((cj0)*(cj3)*(px))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
_nj1 = 1;
if( IKabs(((gconst3)*(((78.0325000000000)+(((66.8850000000000)*(cj3)))+(((1050.00000000000)*(cj3)*(pp)))+(((-2450.00000000000)*((pz)*(pz))))+(((1225.00000000000)*(pp))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((2450.00000000000)*(cj0)*(px)*(pz)))+(((-1050.00000000000)*(cj2)*(pp)*(sj3)))+(((-66.8850000000000)*(cj2)*(sj3)))+(((2450.00000000000)*(py)*(pz)*(sj0))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j1array[0]=IKatan2(((gconst3)*(((78.0325000000000)+(((66.8850000000000)*(cj3)))+(((1050.00000000000)*(cj3)*(pp)))+(((-2450.00000000000)*((pz)*(pz))))+(((1225.00000000000)*(pp)))))), ((gconst3)*(((((2450.00000000000)*(cj0)*(px)*(pz)))+(((-1050.00000000000)*(cj2)*(pp)*(sj3)))+(((-66.8850000000000)*(cj2)*(sj3)))+(((2450.00000000000)*(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;
}
_ij1[0] = ij1; _ij1[1] = -1;
for(int iij1 = ij1+1; iij1 < 1; ++iij1)
{
if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
{
    j1valid[iij1]=false; _ij1[1] = iij1; break; 
}
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
{
IKReal evalcond[6];
IKReal x179=IKcos(j1);
IKReal x180=IKsin(j1);
evalcond[0]=((((-0.490000000000000)*(x179)))+(((-0.420000000000000)*(cj2)*(sj3)*(x180)))+(pz)+(((-0.420000000000000)*(cj3)*(x179))));
evalcond[1]=((-0.0637000000000000)+(((0.980000000000000)*(pz)*(x179)))+(((0.980000000000000)*(cj0)*(px)*(x180)))+(((0.980000000000000)*(py)*(sj0)*(x180)))+(((-1.00000000000000)*(pp))));
evalcond[2]=((0.490000000000000)+(((-1.00000000000000)*(pz)*(x179)))+(((-1.00000000000000)*(cj0)*(px)*(x180)))+(((0.420000000000000)*(cj3)))+(((-1.00000000000000)*(py)*(sj0)*(x180))));
evalcond[3]=((((-0.420000000000000)*(cj2)*(sj3)*(x179)))+(((0.420000000000000)*(cj3)*(x180)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((0.490000000000000)*(x180))));
evalcond[4]=((((-1.00000000000000)*(py)*(sj0)*(sj2)*(x179)))+(((-1.00000000000000)*(cj2)*(px)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)*(sj2)*(x179)))+(((cj0)*(cj2)*(py)))+(((pz)*(sj2)*(x180))));
evalcond[5]=((((-1.00000000000000)*(cj2)*(py)*(sj0)*(x179)))+(((px)*(sj0)*(sj2)))+(((-0.420000000000000)*(sj3)))+(((-1.00000000000000)*(cj0)*(py)*(sj2)))+(((cj2)*(pz)*(x180)))+(((-1.00000000000000)*(cj0)*(cj2)*(px)*(x179))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
{
continue;
}
}

rotationfunction0(vsolutions);
}
}

}

}
}
}

}

}

} else
{
{
IKReal j0array[2], cj0array[2], sj0array[2];
bool j0valid[2]={false};
_nj0 = 2;
IKReal x181=(px)*(px);
IKReal x182=(py)*(py);
IKReal x183=((x181)+(x182));
if( (x183) < (IKReal)-0.00001 )
    continue;
IKReal x184=IKsqrt(x183);
IKReal x185=IKabs(x184);
IKReal x186=((IKabs(x185) != 0)?((IKReal)1/(x185)):(IKReal)1.0e30);
IKReal x187=((0.420000000000000)*(sj2)*(sj3)*(x186));
if( (x187) < -1-IKFAST_SINCOS_THRESH || (x187) > 1+IKFAST_SINCOS_THRESH )
    continue;
IKReal x188=IKasin(x187);
IKReal x189=((-1.00000000000000)*(py));
if( IKabs(x189) < IKFAST_ATAN2_MAGTHRESH && IKabs(px) < IKFAST_ATAN2_MAGTHRESH )
    continue;
IKReal x190=IKatan2(x189, px);
j0array[0]=((((-1.00000000000000)*(x190)))+(x188));
sj0array[0]=IKsin(j0array[0]);
cj0array[0]=IKcos(j0array[0]);
j0array[1]=((3.14159265358979)+(((-1.00000000000000)*(x188)))+(((-1.00000000000000)*(x190))));
sj0array[1]=IKsin(j0array[1]);
cj0array[1]=IKcos(j0array[1]);
if( j0array[0] > IKPI )
{
    j0array[0]-=IK2PI;
}
else if( j0array[0] < -IKPI )
{    j0array[0]+=IK2PI;
}
j0valid[0] = true;
if( j0array[1] > IKPI )
{
    j0array[1]-=IK2PI;
}
else if( j0array[1] < -IKPI )
{    j0array[1]+=IK2PI;
}
j0valid[1] = true;
for(int ij0 = 0; ij0 < 2; ++ij0)
{
if( !j0valid[ij0] )
{
    continue;
}
_ij0[0] = ij0; _ij0[1] = -1;
for(int iij0 = ij0+1; iij0 < 2; ++iij0)
{
if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
{
    j0valid[iij0]=false; _ij0[1] = iij0; break; 
}
}
j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];

{
IKReal dummyeval[1];
IKReal gconst3;
gconst3=IKsign(((((-1029.00000000000)*(cj2)*(pz)*(sj3)))+(((1200.50000000000)*(cj0)*(px)))+(((1029.00000000000)*(cj0)*(cj3)*(px)))+(((1200.50000000000)*(py)*(sj0)))+(((1029.00000000000)*(cj3)*(py)*(sj0)))));
dummyeval[0]=((((1.16666666666667)*(cj0)*(px)))+(((1.16666666666667)*(py)*(sj0)))+(((-1.00000000000000)*(cj2)*(pz)*(sj3)))+(((cj3)*(py)*(sj0)))+(((cj0)*(cj3)*(px))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
_nj1 = 1;
if( IKabs(((gconst3)*(((78.0325000000000)+(((66.8850000000000)*(cj3)))+(((1050.00000000000)*(cj3)*(pp)))+(((-2450.00000000000)*((pz)*(pz))))+(((1225.00000000000)*(pp))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((2450.00000000000)*(cj0)*(px)*(pz)))+(((-1050.00000000000)*(cj2)*(pp)*(sj3)))+(((-66.8850000000000)*(cj2)*(sj3)))+(((2450.00000000000)*(py)*(pz)*(sj0))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j1array[0]=IKatan2(((gconst3)*(((78.0325000000000)+(((66.8850000000000)*(cj3)))+(((1050.00000000000)*(cj3)*(pp)))+(((-2450.00000000000)*((pz)*(pz))))+(((1225.00000000000)*(pp)))))), ((gconst3)*(((((2450.00000000000)*(cj0)*(px)*(pz)))+(((-1050.00000000000)*(cj2)*(pp)*(sj3)))+(((-66.8850000000000)*(cj2)*(sj3)))+(((2450.00000000000)*(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;
}
_ij1[0] = ij1; _ij1[1] = -1;
for(int iij1 = ij1+1; iij1 < 1; ++iij1)
{
if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
{
    j1valid[iij1]=false; _ij1[1] = iij1; break; 
}
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
{
IKReal evalcond[6];
IKReal x191=IKcos(j1);
IKReal x192=IKsin(j1);
evalcond[0]=((((-0.490000000000000)*(x191)))+(((-0.420000000000000)*(cj2)*(sj3)*(x192)))+(pz)+(((-0.420000000000000)*(cj3)*(x191))));
evalcond[1]=((-0.0637000000000000)+(((0.980000000000000)*(cj0)*(px)*(x192)))+(((0.980000000000000)*(pz)*(x191)))+(((0.980000000000000)*(py)*(sj0)*(x192)))+(((-1.00000000000000)*(pp))));
evalcond[2]=((0.490000000000000)+(((-1.00000000000000)*(pz)*(x191)))+(((-1.00000000000000)*(cj0)*(px)*(x192)))+(((0.420000000000000)*(cj3)))+(((-1.00000000000000)*(py)*(sj0)*(x192))));
evalcond[3]=((((-0.420000000000000)*(cj2)*(sj3)*(x191)))+(((0.420000000000000)*(cj3)*(x192)))+(((-1.00000000000000)*(py)*(sj0)))+(((0.490000000000000)*(x192)))+(((-1.00000000000000)*(cj0)*(px))));
evalcond[4]=((((-1.00000000000000)*(py)*(sj0)*(sj2)*(x191)))+(((-1.00000000000000)*(cj0)*(px)*(sj2)*(x191)))+(((-1.00000000000000)*(cj2)*(px)*(sj0)))+(((cj0)*(cj2)*(py)))+(((pz)*(sj2)*(x192))));
evalcond[5]=((((px)*(sj0)*(sj2)))+(((-0.420000000000000)*(sj3)))+(((-1.00000000000000)*(cj0)*(cj2)*(px)*(x191)))+(((-1.00000000000000)*(cj0)*(py)*(sj2)))+(((cj2)*(pz)*(x192)))+(((-1.00000000000000)*(cj2)*(py)*(sj0)*(x191))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
{
continue;
}
}

rotationfunction0(vsolutions);
}
}

}

}
}
}

}

}
}
}
}
return vsolutions.size()>0;
}
inline void rotationfunction0(std::vector<IKSolution>& vsolutions) {
for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
IKReal x93=((sj1)*(sj3));
IKReal x94=((cj1)*(cj2)*(cj3));
IKReal x95=((x93)+(x94));
IKReal x96=((cj0)*(cj3)*(sj2));
IKReal x97=((sj0)*(x95));
IKReal x98=((x97)+(x96));
IKReal x99=((cj0)*(x95));
IKReal x100=((cj3)*(sj0)*(sj2));
IKReal x101=((((-1.00000000000000)*(x100)))+(x99));
IKReal x102=((cj1)*(sj3));
IKReal x103=((cj2)*(cj3)*(sj1));
IKReal x104=((((-1.00000000000000)*(x103)))+(x102));
IKReal x105=((cj2)*(sj0));
IKReal x106=((cj0)*(cj1)*(sj2));
IKReal x107=((x105)+(x106));
IKReal x108=((cj1)*(sj0)*(sj2));
IKReal x109=((cj0)*(cj2));
IKReal x110=((((-1.00000000000000)*(x109)))+(x108));
IKReal x111=((cj2)*(x102));
IKReal x112=((cj3)*(sj1));
IKReal x113=((((-1.00000000000000)*(x112)))+(x111));
IKReal x114=((cj1)*(cj3));
IKReal x115=((cj2)*(x93));
IKReal x116=((x115)+(x114));
IKReal x117=((-1.00000000000000)*(x116));
IKReal x118=((sj0)*(x113));
IKReal x119=((cj0)*(sj2)*(sj3));
IKReal x120=((x119)+(x118));
IKReal x121=((cj0)*(x113));
IKReal x122=((-1.00000000000000)*(sj0)*(sj2)*(sj3));
IKReal x123=((x122)+(x121));
new_r00=((((r10)*(x98)))+(((r20)*(x104)))+(((r00)*(x101))));
new_r01=((((r11)*(x98)))+(((r21)*(x104)))+(((r01)*(x101))));
new_r02=((((r02)*(x101)))+(((r12)*(x98)))+(((r22)*(x104))));
new_r10=((((r10)*(x110)))+(((-1.00000000000000)*(r20)*(sj1)*(sj2)))+(((r00)*(x107))));
new_r11=((((-1.00000000000000)*(r21)*(sj1)*(sj2)))+(((r11)*(x110)))+(((r01)*(x107))));
new_r12=((((r02)*(x107)))+(((-1.00000000000000)*(r22)*(sj1)*(sj2)))+(((r12)*(x110))));
new_r20=((((r00)*(x123)))+(((r20)*(x117)))+(((r10)*(x120))));
new_r21=((((r01)*(x123)))+(((r21)*(x117)))+(((r11)*(x120))));
new_r22=((((r22)*(x117)))+(((r02)*(x123)))+(((r12)*(x120))));
{
IKReal j5array[2], cj5array[2], sj5array[2];
bool j5valid[2]={false};
_nj5 = 2;
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;
}
for(int ij5 = 0; ij5 < 2; ++ij5)
{
if( !j5valid[ij5] )
{
    continue;
}
_ij5[0] = ij5; _ij5[1] = -1;
for(int iij5 = ij5+1; iij5 < 2; ++iij5)
{
if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
{
    j5valid[iij5]=false; _ij5[1] = iij5; break; 
}
}
j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];

{
IKReal dummyeval[1];
IKReal gconst5;
gconst5=IKsign(sj5);
dummyeval[0]=sj5;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
IKReal gconst4;
gconst4=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};
_nj4 = 2;
if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
    continue;
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;
for(int ij4 = 0; ij4 < 2; ++ij4)
{
if( !j4valid[ij4] )
{
    continue;
}
_ij4[0] = ij4; _ij4[1] = -1;
for(int iij4 = ij4+1; iij4 < 2; ++iij4)
{
if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
{
    j4valid[iij4]=false; _ij4[1] = iij4; break; 
}
}
j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
{
IKReal evalcond[1];
evalcond[0]=((((new_r12)*(IKcos(j4))))+(((-1.00000000000000)*(new_r02)*(IKsin(j4)))));
if( IKabs(evalcond[0]) > 0.000001  )
{
continue;
}
}

{
IKReal j6array[1], cj6array[1], sj6array[1];
bool j6valid[1]={false};
_nj6 = 1;
if( IKabs(((((-1.00000000000000)*(cj4)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj4)*(new_r00)))+(((new_r10)*(sj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.00000000000000)*(cj4)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj4)))))+IKsqr(((((cj4)*(new_r00)))+(((new_r10)*(sj4)))))-1) <= IKFAST_SINCOS_THRESH )
    continue;
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;
}
_ij6[0] = ij6; _ij6[1] = -1;
for(int iij6 = ij6+1; iij6 < 1; ++iij6)
{
if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
{
    j6valid[iij6]=false; _ij6[1] = iij6; break; 
}
}
j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
{
IKReal evalcond[4];
IKReal x125=IKsin(j6);
IKReal x126=IKcos(j6);
evalcond[0]=((((cj4)*(new_r10)))+(((-1.00000000000000)*(x125)))+(((-1.00000000000000)*(new_r00)*(sj4))));
evalcond[1]=((((cj4)*(new_r11)))+(((-1.00000000000000)*(x126)))+(((-1.00000000000000)*(new_r01)*(sj4))));
evalcond[2]=((((cj4)*(new_r01)))+(((new_r11)*(sj4)))+(x125));
evalcond[3]=((((cj4)*(new_r00)))+(((new_r10)*(sj4)))+(((-1.00000000000000)*(x126))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j0;
solution.basesol[0].indices[0] = _ij0[0];
solution.basesol[0].indices[1] = _ij0[1];
solution.basesol[0].maxsolutions = _nj0;
solution.basesol[1].foffset = j1;
solution.basesol[1].indices[0] = _ij1[0];
solution.basesol[1].indices[1] = _ij1[1];
solution.basesol[1].maxsolutions = _nj1;
solution.basesol[2].foffset = j2;
solution.basesol[2].indices[0] = _ij2[0];
solution.basesol[2].indices[1] = _ij2[1];
solution.basesol[2].maxsolutions = _nj2;
solution.basesol[3].foffset = j3;
solution.basesol[3].indices[0] = _ij3[0];
solution.basesol[3].indices[1] = _ij3[1];
solution.basesol[3].maxsolutions = _nj3;
solution.basesol[4].foffset = j4;
solution.basesol[4].indices[0] = _ij4[0];
solution.basesol[4].indices[1] = _ij4[1];
solution.basesol[4].maxsolutions = _nj4;
solution.basesol[5].foffset = j5;
solution.basesol[5].indices[0] = _ij5[0];
solution.basesol[5].indices[1] = _ij5[1];
solution.basesol[5].maxsolutions = _nj5;
solution.basesol[6].foffset = j6;
solution.basesol[6].indices[0] = _ij6[0];
solution.basesol[6].indices[1] = _ij6[1];
solution.basesol[6].maxsolutions = _nj6;
solution.vfree.resize(0);
}
}
}
}
}

} else
{
IKReal x127=((1.00000000000000)+(new_r22));
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j5)), 6.28318530717959)));
evalcond[1]=x127;
evalcond[2]=new_r20;
evalcond[3]=new_r21;
evalcond[4]=new_r20;
evalcond[5]=new_r21;
evalcond[6]=x127;
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};
_nj4 = 2;
if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
    continue;
IKReal x128=IKatan2(new_r02, new_r12);
j4array[0]=((-1.00000000000000)*(x128));
sj4array[0]=IKsin(j4array[0]);
cj4array[0]=IKcos(j4array[0]);
j4array[1]=((3.14159265358979)+(((-1.00000000000000)*(x128))));
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;
for(int ij4 = 0; ij4 < 2; ++ij4)
{
if( !j4valid[ij4] )
{
    continue;
}
_ij4[0] = ij4; _ij4[1] = -1;
for(int iij4 = ij4+1; iij4 < 2; ++iij4)
{
if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
{
    j4valid[iij4]=false; _ij4[1] = iij4; break; 
}
}
j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
{
IKReal evalcond[1];
evalcond[0]=((((new_r12)*(IKcos(j4))))+(((-1.00000000000000)*(new_r02)*(IKsin(j4)))));
if( IKabs(evalcond[0]) > 0.000001  )
{
continue;
}
}

{
IKReal j6array[1], cj6array[1], sj6array[1];
bool j6valid[1]={false};
_nj6 = 1;
if( IKabs(((((cj4)*(new_r01)))+(((new_r11)*(sj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.00000000000000)*(new_r10)*(sj4)))+(((-1.00000000000000)*(cj4)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj4)*(new_r01)))+(((new_r11)*(sj4)))))+IKsqr(((((-1.00000000000000)*(new_r10)*(sj4)))+(((-1.00000000000000)*(cj4)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH )
    continue;
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;
}
_ij6[0] = ij6; _ij6[1] = -1;
for(int iij6 = ij6+1; iij6 < 1; ++iij6)
{
if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
{
    j6valid[iij6]=false; _ij6[1] = iij6; break; 
}
}
j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
{
IKReal evalcond[4];
IKReal x129=IKsin(j6);
IKReal x130=IKcos(j6);
evalcond[0]=((((cj4)*(new_r10)))+(((-1.00000000000000)*(x129)))+(((-1.00000000000000)*(new_r00)*(sj4))));
evalcond[1]=((((cj4)*(new_r11)))+(((-1.00000000000000)*(x130)))+(((-1.00000000000000)*(new_r01)*(sj4))));
evalcond[2]=((((cj4)*(new_r01)))+(((new_r11)*(sj4)))+(((-1.00000000000000)*(x129))));
evalcond[3]=((((cj4)*(new_r00)))+(((new_r10)*(sj4)))+(x130));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j0;
solution.basesol[0].indices[0] = _ij0[0];
solution.basesol[0].indices[1] = _ij0[1];
solution.basesol[0].maxsolutions = _nj0;
solution.basesol[1].foffset = j1;
solution.basesol[1].indices[0] = _ij1[0];
solution.basesol[1].indices[1] = _ij1[1];
solution.basesol[1].maxsolutions = _nj1;
solution.basesol[2].foffset = j2;
solution.basesol[2].indices[0] = _ij2[0];
solution.basesol[2].indices[1] = _ij2[1];
solution.basesol[2].maxsolutions = _nj2;
solution.basesol[3].foffset = j3;
solution.basesol[3].indices[0] = _ij3[0];
solution.basesol[3].indices[1] = _ij3[1];
solution.basesol[3].maxsolutions = _nj3;
solution.basesol[4].foffset = j4;
solution.basesol[4].indices[0] = _ij4[0];
solution.basesol[4].indices[1] = _ij4[1];
solution.basesol[4].maxsolutions = _nj4;
solution.basesol[5].foffset = j5;
solution.basesol[5].indices[0] = _ij5[0];
solution.basesol[5].indices[1] = _ij5[1];
solution.basesol[5].maxsolutions = _nj5;
solution.basesol[6].foffset = j6;
solution.basesol[6].indices[0] = _ij6[0];
solution.basesol[6].indices[1] = _ij6[1];
solution.basesol[6].maxsolutions = _nj6;
solution.vfree.resize(0);
}
}
}
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j4array[1], cj4array[1], sj4array[1];
bool j4valid[1]={false};
_nj4 = 1;
if( IKabs(((gconst4)*(new_r12)*(sj5))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(new_r02)*(sj5))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j4array[0]=IKatan2(((gconst4)*(new_r12)*(sj5)), ((gconst4)*(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;
}
_ij4[0] = ij4; _ij4[1] = -1;
for(int iij4 = ij4+1; iij4 < 1; ++iij4)
{
if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
{
    j4valid[iij4]=false; _ij4[1] = iij4; break; 
}
}
j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
{
IKReal evalcond[6];
IKReal x131=IKcos(j4);
IKReal x132=IKsin(j4);
evalcond[0]=((((new_r12)*(x131)))+(((-1.00000000000000)*(new_r02)*(x132))));
evalcond[1]=((((new_r12)*(x132)))+(((new_r02)*(x131)))+(((-1.00000000000000)*(sj5))));
evalcond[2]=((((cj5)*(new_r12)*(x132)))+(((-1.00000000000000)*(new_r22)*(sj5)))+(((cj5)*(new_r02)*(x131))));
evalcond[3]=((((-1.00000000000000)*(new_r10)*(sj5)*(x132)))+(((-1.00000000000000)*(new_r00)*(sj5)*(x131)))+(((-1.00000000000000)*(cj5)*(new_r20))));
evalcond[4]=((((-1.00000000000000)*(new_r11)*(sj5)*(x132)))+(((-1.00000000000000)*(new_r01)*(sj5)*(x131)))+(((-1.00000000000000)*(cj5)*(new_r21))));
evalcond[5]=((1.00000000000000)+(((-1.00000000000000)*(new_r02)*(sj5)*(x131)))+(((-1.00000000000000)*(new_r12)*(sj5)*(x132)))+(((-1.00000000000000)*(cj5)*(new_r22))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
{
continue;
}
}

{
IKReal dummyeval[1];
IKReal gconst6;
gconst6=IKsign(sj5);
dummyeval[0]=sj5;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[11];
IKReal x133=((cj4)*(new_r12));
IKReal x134=((new_r02)*(sj4));
IKReal x135=((x133)+(((-1.00000000000000)*(x134))));
IKReal x136=((new_r12)*(sj4));
IKReal x137=((cj4)*(new_r02));
IKReal x138=((x137)+(x136));
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]=x135;
evalcond[5]=x135;
evalcond[6]=x138;
evalcond[7]=x138;
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};
_nj6 = 1;
if( IKabs(((((-1.00000000000000)*(cj4)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj4)*(new_r00)))+(((new_r10)*(sj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.00000000000000)*(cj4)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj4)))))+IKsqr(((((cj4)*(new_r00)))+(((new_r10)*(sj4)))))-1) <= IKFAST_SINCOS_THRESH )
    continue;
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;
}
_ij6[0] = ij6; _ij6[1] = -1;
for(int iij6 = ij6+1; iij6 < 1; ++iij6)
{
if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
{
    j6valid[iij6]=false; _ij6[1] = iij6; break; 
}
}
j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
{
IKReal evalcond[4];
IKReal x139=IKsin(j6);
IKReal x140=IKcos(j6);
evalcond[0]=((((cj4)*(new_r10)))+(((-1.00000000000000)*(x139)))+(((-1.00000000000000)*(new_r00)*(sj4))));
evalcond[1]=((((-1.00000000000000)*(x140)))+(((cj4)*(new_r11)))+(((-1.00000000000000)*(new_r01)*(sj4))));
evalcond[2]=((((cj4)*(new_r01)))+(((new_r11)*(sj4)))+(x139));
evalcond[3]=((((-1.00000000000000)*(x140)))+(((cj4)*(new_r00)))+(((new_r10)*(sj4))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j0;
solution.basesol[0].indices[0] = _ij0[0];
solution.basesol[0].indices[1] = _ij0[1];
solution.basesol[0].maxsolutions = _nj0;
solution.basesol[1].foffset = j1;
solution.basesol[1].indices[0] = _ij1[0];
solution.basesol[1].indices[1] = _ij1[1];
solution.basesol[1].maxsolutions = _nj1;
solution.basesol[2].foffset = j2;
solution.basesol[2].indices[0] = _ij2[0];
solution.basesol[2].indices[1] = _ij2[1];
solution.basesol[2].maxsolutions = _nj2;
solution.basesol[3].foffset = j3;
solution.basesol[3].indices[0] = _ij3[0];
solution.basesol[3].indices[1] = _ij3[1];
solution.basesol[3].maxsolutions = _nj3;
solution.basesol[4].foffset = j4;
solution.basesol[4].indices[0] = _ij4[0];
solution.basesol[4].indices[1] = _ij4[1];
solution.basesol[4].maxsolutions = _nj4;
solution.basesol[5].foffset = j5;
solution.basesol[5].indices[0] = _ij5[0];
solution.basesol[5].indices[1] = _ij5[1];
solution.basesol[5].maxsolutions = _nj5;
solution.basesol[6].foffset = j6;
solution.basesol[6].indices[0] = _ij6[0];
solution.basesol[6].indices[1] = _ij6[1];
solution.basesol[6].maxsolutions = _nj6;
solution.vfree.resize(0);
}
}
}

} else
{
IKReal x141=((cj4)*(new_r12));
IKReal x142=((new_r02)*(sj4));
IKReal x143=((((-1.00000000000000)*(x142)))+(x141));
IKReal x144=((new_r12)*(sj4));
IKReal x145=((cj4)*(new_r02));
IKReal x146=((x144)+(x145));
IKReal x147=((1.00000000000000)+(new_r22));
evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j5)), 6.28318530717959)));
evalcond[1]=x147;
evalcond[2]=new_r20;
evalcond[3]=new_r21;
evalcond[4]=x143;
evalcond[5]=x143;
evalcond[6]=x146;
evalcond[7]=((-1.00000000000000)*(x146));
evalcond[8]=new_r20;
evalcond[9]=new_r21;
evalcond[10]=x147;
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};
_nj6 = 1;
if( IKabs(((((cj4)*(new_r01)))+(((new_r11)*(sj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.00000000000000)*(new_r10)*(sj4)))+(((-1.00000000000000)*(cj4)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj4)*(new_r01)))+(((new_r11)*(sj4)))))+IKsqr(((((-1.00000000000000)*(new_r10)*(sj4)))+(((-1.00000000000000)*(cj4)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH )
    continue;
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;
}
_ij6[0] = ij6; _ij6[1] = -1;
for(int iij6 = ij6+1; iij6 < 1; ++iij6)
{
if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
{
    j6valid[iij6]=false; _ij6[1] = iij6; break; 
}
}
j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
{
IKReal evalcond[4];
IKReal x148=IKsin(j6);
IKReal x149=IKcos(j6);
evalcond[0]=((((-1.00000000000000)*(x148)))+(((cj4)*(new_r10)))+(((-1.00000000000000)*(new_r00)*(sj4))));
evalcond[1]=((((-1.00000000000000)*(x149)))+(((cj4)*(new_r11)))+(((-1.00000000000000)*(new_r01)*(sj4))));
evalcond[2]=((((-1.00000000000000)*(x148)))+(((cj4)*(new_r01)))+(((new_r11)*(sj4))));
evalcond[3]=((((cj4)*(new_r00)))+(((new_r10)*(sj4)))+(x149));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j0;
solution.basesol[0].indices[0] = _ij0[0];
solution.basesol[0].indices[1] = _ij0[1];
solution.basesol[0].maxsolutions = _nj0;
solution.basesol[1].foffset = j1;
solution.basesol[1].indices[0] = _ij1[0];
solution.basesol[1].indices[1] = _ij1[1];
solution.basesol[1].maxsolutions = _nj1;
solution.basesol[2].foffset = j2;
solution.basesol[2].indices[0] = _ij2[0];
solution.basesol[2].indices[1] = _ij2[1];
solution.basesol[2].maxsolutions = _nj2;
solution.basesol[3].foffset = j3;
solution.basesol[3].indices[0] = _ij3[0];
solution.basesol[3].indices[1] = _ij3[1];
solution.basesol[3].maxsolutions = _nj3;
solution.basesol[4].foffset = j4;
solution.basesol[4].indices[0] = _ij4[0];
solution.basesol[4].indices[1] = _ij4[1];
solution.basesol[4].maxsolutions = _nj4;
solution.basesol[5].foffset = j5;
solution.basesol[5].indices[0] = _ij5[0];
solution.basesol[5].indices[1] = _ij5[1];
solution.basesol[5].maxsolutions = _nj5;
solution.basesol[6].foffset = j6;
solution.basesol[6].indices[0] = _ij6[0];
solution.basesol[6].indices[1] = _ij6[1];
solution.basesol[6].maxsolutions = _nj6;
solution.vfree.resize(0);
}
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j6array[1], cj6array[1], sj6array[1];
bool j6valid[1]={false};
_nj6 = 1;
if( IKabs(((gconst6)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.00000000000000)*(gconst6)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j6array[0]=IKatan2(((gconst6)*(new_r21)), ((-1.00000000000000)*(gconst6)*(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;
}
_ij6[0] = ij6; _ij6[1] = -1;
for(int iij6 = ij6+1; iij6 < 1; ++iij6)
{
if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
{
    j6valid[iij6]=false; _ij6[1] = iij6; break; 
}
}
j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
{
IKReal evalcond[8];
IKReal x150=IKsin(j6);
IKReal x151=IKcos(j6);
evalcond[0]=((((sj5)*(x151)))+(new_r20));
evalcond[1]=((new_r21)+(((-1.00000000000000)*(sj5)*(x150))));
evalcond[2]=((((-1.00000000000000)*(x150)))+(((cj4)*(new_r10)))+(((-1.00000000000000)*(new_r00)*(sj4))));
evalcond[3]=((((-1.00000000000000)*(x151)))+(((cj4)*(new_r11)))+(((-1.00000000000000)*(new_r01)*(sj4))));
evalcond[4]=((((cj4)*(new_r01)))+(((new_r11)*(sj4)))+(((cj5)*(x150))));
evalcond[5]=((((cj4)*(new_r00)))+(((new_r10)*(sj4)))+(((-1.00000000000000)*(cj5)*(x151))));
evalcond[6]=((((-1.00000000000000)*(new_r21)*(sj5)))+(x150)+(((cj4)*(cj5)*(new_r01)))+(((cj5)*(new_r11)*(sj4))));
evalcond[7]=((((-1.00000000000000)*(x151)))+(((cj4)*(cj5)*(new_r00)))+(((cj5)*(new_r10)*(sj4)))+(((-1.00000000000000)*(new_r20)*(sj5))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
{
continue;
}
}

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j0;
solution.basesol[0].indices[0] = _ij0[0];
solution.basesol[0].indices[1] = _ij0[1];
solution.basesol[0].maxsolutions = _nj0;
solution.basesol[1].foffset = j1;
solution.basesol[1].indices[0] = _ij1[0];
solution.basesol[1].indices[1] = _ij1[1];
solution.basesol[1].maxsolutions = _nj1;
solution.basesol[2].foffset = j2;
solution.basesol[2].indices[0] = _ij2[0];
solution.basesol[2].indices[1] = _ij2[1];
solution.basesol[2].maxsolutions = _nj2;
solution.basesol[3].foffset = j3;
solution.basesol[3].indices[0] = _ij3[0];
solution.basesol[3].indices[1] = _ij3[1];
solution.basesol[3].maxsolutions = _nj3;
solution.basesol[4].foffset = j4;
solution.basesol[4].indices[0] = _ij4[0];
solution.basesol[4].indices[1] = _ij4[1];
solution.basesol[4].maxsolutions = _nj4;
solution.basesol[5].foffset = j5;
solution.basesol[5].indices[0] = _ij5[0];
solution.basesol[5].indices[1] = _ij5[1];
solution.basesol[5].maxsolutions = _nj5;
solution.basesol[6].foffset = j6;
solution.basesol[6].indices[0] = _ij6[0];
solution.basesol[6].indices[1] = _ij6[1];
solution.basesol[6].maxsolutions = _nj6;
solution.vfree.resize(0);
}
}
}

}

}
}
}

}

}

} else
{
{
IKReal j6array[1], cj6array[1], sj6array[1];
bool j6valid[1]={false};
_nj6 = 1;
if( IKabs(((gconst5)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.00000000000000)*(gconst5)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j6array[0]=IKatan2(((gconst5)*(new_r21)), ((-1.00000000000000)*(gconst5)*(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;
}
_ij6[0] = ij6; _ij6[1] = -1;
for(int iij6 = ij6+1; iij6 < 1; ++iij6)
{
if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
{
    j6valid[iij6]=false; _ij6[1] = iij6; break; 
}
}
j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
{
IKReal evalcond[2];
evalcond[0]=((new_r20)+(((sj5)*(IKcos(j6)))));
evalcond[1]=((new_r21)+(((-1.00000000000000)*(sj5)*(IKsin(j6)))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
{
continue;
}
}

{
IKReal dummyeval[1];
IKReal gconst7;
gconst7=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};
_nj4 = 1;
if( IKabs(((gconst7)*(new_r12)*(sj5))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(new_r02)*(sj5))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j4array[0]=IKatan2(((gconst7)*(new_r12)*(sj5)), ((gconst7)*(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;
}
_ij4[0] = ij4; _ij4[1] = -1;
for(int iij4 = ij4+1; iij4 < 1; ++iij4)
{
if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
{
    j4valid[iij4]=false; _ij4[1] = iij4; break; 
}
}
j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
{
IKReal evalcond[12];
IKReal x152=IKcos(j4);
IKReal x153=IKsin(j4);
evalcond[0]=((((new_r12)*(x152)))+(((-1.00000000000000)*(new_r02)*(x153))));
evalcond[1]=((((new_r12)*(x153)))+(((new_r02)*(x152)))+(((-1.00000000000000)*(sj5))));
evalcond[2]=((((new_r10)*(x152)))+(((-1.00000000000000)*(new_r00)*(x153)))+(((-1.00000000000000)*(sj6))));
evalcond[3]=((((new_r11)*(x152)))+(((-1.00000000000000)*(cj6)))+(((-1.00000000000000)*(new_r01)*(x153))));
evalcond[4]=((((new_r01)*(x152)))+(((new_r11)*(x153)))+(((cj5)*(sj6))));
evalcond[5]=((((new_r00)*(x152)))+(((new_r10)*(x153)))+(((-1.00000000000000)*(cj5)*(cj6))));
evalcond[6]=((((cj5)*(new_r02)*(x152)))+(((-1.00000000000000)*(new_r22)*(sj5)))+(((cj5)*(new_r12)*(x153))));
evalcond[7]=((((-1.00000000000000)*(new_r10)*(sj5)*(x153)))+(((-1.00000000000000)*(new_r00)*(sj5)*(x152)))+(((-1.00000000000000)*(cj5)*(new_r20))));
evalcond[8]=((((-1.00000000000000)*(new_r01)*(sj5)*(x152)))+(((-1.00000000000000)*(new_r11)*(sj5)*(x153)))+(((-1.00000000000000)*(cj5)*(new_r21))));
evalcond[9]=((((-1.00000000000000)*(new_r21)*(sj5)))+(sj6)+(((cj5)*(new_r11)*(x153)))+(((cj5)*(new_r01)*(x152))));
evalcond[10]=((1.00000000000000)+(((-1.00000000000000)*(new_r02)*(sj5)*(x152)))+(((-1.00000000000000)*(new_r12)*(sj5)*(x153)))+(((-1.00000000000000)*(cj5)*(new_r22))));
evalcond[11]=((((-1.00000000000000)*(cj6)))+(((cj5)*(new_r00)*(x152)))+(((-1.00000000000000)*(new_r20)*(sj5)))+(((cj5)*(new_r10)*(x153))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  || IKabs(evalcond[8]) > 0.000001  || IKabs(evalcond[9]) > 0.000001  || IKabs(evalcond[10]) > 0.000001  || IKabs(evalcond[11]) > 0.000001  )
{
continue;
}
}

{
vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
solution.basesol.resize(7);
solution.basesol[0].foffset = j0;
solution.basesol[0].indices[0] = _ij0[0];
solution.basesol[0].indices[1] = _ij0[1];
solution.basesol[0].maxsolutions = _nj0;
solution.basesol[1].foffset = j1;
solution.basesol[1].indices[0] = _ij1[0];
solution.basesol[1].indices[1] = _ij1[1];
solution.basesol[1].maxsolutions = _nj1;
solution.basesol[2].foffset = j2;
solution.basesol[2].indices[0] = _ij2[0];
solution.basesol[2].indices[1] = _ij2[1];
solution.basesol[2].maxsolutions = _nj2;
solution.basesol[3].foffset = j3;
solution.basesol[3].indices[0] = _ij3[0];
solution.basesol[3].indices[1] = _ij3[1];
solution.basesol[3].maxsolutions = _nj3;
solution.basesol[4].foffset = j4;
solution.basesol[4].indices[0] = _ij4[0];
solution.basesol[4].indices[1] = _ij4[1];
solution.basesol[4].maxsolutions = _nj4;
solution.basesol[5].foffset = j5;
solution.basesol[5].indices[0] = _ij5[0];
solution.basesol[5].indices[1] = _ij5[1];
solution.basesol[5].maxsolutions = _nj5;
solution.basesol[6].foffset = j6;
solution.basesol[6].indices[0] = _ij6[0];
solution.basesol[6].indices[1] = _ij6[1];
solution.basesol[6].maxsolutions = _nj6;
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 "2f0978eec3507105782751e56c521b6b"; }

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

#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