/// autogenerated analytical inverse kinematics code from ikfast program part of OpenRAVE
/// \author Rosen Diankov
///
/// Licensed under the Apache License, Version 2.0 (the "License");
/// you may not use this file except in compliance with the License.
/// You may obtain a copy of the License at
///     http://www.apache.org/licenses/LICENSE-2.0
/// 
/// Unless required by applicable law or agreed to in writing, software
/// distributed under the License is distributed on an "AS IS" BASIS,
/// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
/// See the License for the specific language governing permissions and
/// limitations under the License.
///
/// ikfast version 59 generated on 2012-06-28 22:35:55.833278
/// To compile with gcc:
///     gcc -lstdc++ ik.cpp
/// To compile without any main function as a shared object (might need -llapack):
///     gcc -fPIC -lstdc++ -DIKFAST_NO_MAIN -DIKFAST_CLIBRARY -shared -Wl,-soname,libik.so -o libik.so ik.cpp
#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:
    /// Gets a solution given its free parameters
    /// \param pfree The free parameters required, range is in [-pi,pi]
    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;
                }
            }
        }
    }

    /// Gets the free parameters the solution requires to be set before a full solution can be returned
    /// \return vector of indices indicating the free parameters
    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; ///< joint value is fmul*sol[freeind]+foffset
        signed char freeind; ///< if >= 0, mimics another joint
        unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself
        unsigned char indices[2]; ///< unique index of the solution used to keep track on what part it came from. sometimes a solution can be repeated for different indices. store at least another repeated root
    };

    std::vector<VARIABLE> basesol;       ///< solution and their offsets if joints are mimiced
    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 float(-IKPI_2);
else if( f >= 1 ) return float(IKPI_2);
return asinf(f);
}
inline double IKasin(double f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return -IKPI_2;
else if( f >= 1 ) return IKPI_2;
return asin(f);
}

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

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

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

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

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

/// solves the forward kinematics equations.
/// \param pfree is an array specifying the free joints of the chain.
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;
x0=IKcos(j[0]);
x1=IKcos(j[1]);
x2=IKsin(j[2]);
x3=IKcos(j[2]);
x4=IKsin(j[1]);
x5=IKsin(j[3]);
x6=IKcos(j[3]);
x7=IKsin(j[0]);
x8=IKcos(j[5]);
x9=IKsin(j[5]);
x10=IKsin(j[4]);
x11=IKcos(j[4]);
x12=((IKReal(0.0203000000000000))*(x3));
x13=((IKReal(1.00000000000000))*(x11));
x14=((IKReal(0.433100000000000))*(x0));
x15=((IKReal(0.0940000000000000))*(x1));
x16=((IKReal(0.0940000000000000))*(x0));
x17=((IKReal(1.00000000000000))*(x10));
x18=((IKReal(1.00000000000000))*(x7));
x19=((IKReal(0.0940000000000000))*(x7));
x20=((IKReal(1.00000000000000))*(x3));
x21=((IKReal(0.0940000000000000))*(x6));
x22=((IKReal(1.00000000000000))*(x2));
x23=((IKReal(1.00000000000000))*(x0));
x24=((IKReal(0.0940000000000000))*(x2));
x25=((x1)*(x7));
x26=((x10)*(x6));
x27=((x3)*(x4));
x28=((x0)*(x1));
x29=((x2)*(x4));
x30=((x1)*(x2));
x31=((x1)*(x20));
x32=((x0)*(x22)*(x4));
x33=((x18)*(x29));
x34=((((IKReal(-1.00000000000000))*(x31)))+(x29));
x35=((((x1)*(x22)))+(((x20)*(x4))));
x36=((IKReal(-1.00000000000000))*(x35));
x37=((((IKReal(-1.00000000000000))*(x32)))+(((x28)*(x3))));
x38=((((IKReal(-1.00000000000000))*(x33)))+(((x25)*(x3))));
x39=((x36)*(x6));
x40=((x35)*(x5));
x41=((((IKReal(-1.00000000000000))*(x0)*(x20)*(x4)))+(((IKReal(-1.00000000000000))*(x22)*(x28))));
x42=((((x22)*(x28)))+(((x0)*(x20)*(x4))));
x43=((x18)*(((((IKReal(-1.00000000000000))*(x30)))+(((IKReal(-1.00000000000000))*(x27))))));
x44=((x18)*(((x30)+(x27))));
x45=((x37)*(x6));
x46=((x38)*(x6));
x47=((x10)*(x41));
x48=((x10)*(x43));
x49=((((IKReal(-1.00000000000000))*(x18)*(x5)))+(x45));
x50=((((x0)*(x5)))+(x46));
x51=((((IKReal(-1.00000000000000))*(x18)*(x6)))+(((x5)*(((x32)+(((IKReal(-1.00000000000000))*(x20)*(x28))))))));
x52=((((x0)*(x6)))+(((x5)*(((x33)+(((IKReal(-1.00000000000000))*(x1)*(x18)*(x3))))))));
x53=((x11)*(x49));
eerot[0]=((((x8)*(((x47)+(x53)))))+(((x51)*(x9))));
eerot[1]=((((x51)*(x8)))+(((x9)*(((((IKReal(-1.00000000000000))*(x17)*(x41)))+(((IKReal(-1.00000000000000))*(x13)*(x49))))))));
eerot[2]=((((x10)*(x49)))+(((x11)*(x42))));
eetrans[0]=((((IKReal(0.431800000000000))*(x28)))+(((IKReal(-1.00000000000000))*(x12)*(x28)))+(((x14)*(x27)))+(((IKReal(0.0203000000000000))*(x0)*(x29)))+(((x14)*(x30)))+(((IKReal(-0.150100000000000))*(x7)))+(((x10)*(((((IKReal(-1.00000000000000))*(x19)*(x5)))+(((x21)*(x37)))))))+(((x11)*(((((x16)*(x27)))+(((x0)*(x15)*(x2))))))));
eerot[3]=((((x8)*(((((x11)*(x50)))+(x48)))))+(((x52)*(x9))));
eerot[4]=((((x9)*(((((IKReal(-1.00000000000000))*(x17)*(x43)))+(((IKReal(-1.00000000000000))*(x13)*(x50)))))))+(((x52)*(x8))));
eerot[5]=((((x11)*(x44)))+(((x10)*(x50))));
eetrans[1]=((((IKReal(0.150100000000000))*(x0)))+(((x11)*(((((x19)*(x27)))+(((x15)*(x2)*(x7)))))))+(((IKReal(0.431800000000000))*(x25)))+(((IKReal(-1.00000000000000))*(x12)*(x25)))+(((IKReal(0.433100000000000))*(x27)*(x7)))+(((IKReal(0.0203000000000000))*(x29)*(x7)))+(((IKReal(0.433100000000000))*(x2)*(x25)))+(((x10)*(((((x21)*(x38)))+(((x16)*(x5))))))));
eerot[6]=((((x8)*(((((x10)*(x34)))+(((x11)*(x39)))))))+(((x40)*(x9))));
eerot[7]=((((x9)*(((((IKReal(-1.00000000000000))*(x17)*(x34)))+(((IKReal(-1.00000000000000))*(x13)*(x39)))))))+(((x40)*(x8))));
eerot[8]=((((x26)*(x36)))+(((x11)*(((x31)+(((IKReal(-1.00000000000000))*(x22)*(x4))))))));
eetrans[2]=((IKReal(1.37100000000000))+(((x26)*(((((IKReal(-0.0940000000000000))*(x27)))+(((IKReal(-1.00000000000000))*(x15)*(x2)))))))+(((IKReal(-0.431800000000000))*(x4)))+(((IKReal(0.0203000000000000))*(x30)))+(((IKReal(0.433100000000000))*(x1)*(x3)))+(((x12)*(x4)))+(((IKReal(-0.433100000000000))*(x29)))+(((x11)*(((((IKReal(-1.00000000000000))*(x24)*(x4)))+(((x15)*(x3))))))));
}

IKFAST_API int getNumFreeParameters() { return 0; }
IKFAST_API int* getFreeParameters() { return NULL; }
IKFAST_API int getNumJoints() { return 6; }

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,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,j5,cj5,sj5,htj5,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,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;

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; j2=numeric_limits<IKReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IKReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IKReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IKReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1; 
for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
    vsolutions.resize(0); vsolutions.reserve(8);
r00 = eerot[0*3+0];
r01 = eerot[0*3+1];
r02 = eerot[0*3+2];
r10 = eerot[1*3+0];
r11 = eerot[1*3+1];
r12 = eerot[1*3+2];
r20 = eerot[2*3+0];
r21 = eerot[2*3+1];
r22 = eerot[2*3+2];
px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];

new_r00=r00;
new_r01=r01;
new_r02=r02;
new_px=((((IKReal(-0.0940000000000000))*(r02)))+(px));
new_r10=r10;
new_r11=r11;
new_r12=r12;
new_py=((py)+(((IKReal(-0.0940000000000000))*(r12))));
new_r20=r20;
new_r21=r21;
new_r22=r22;
new_pz=((IKReal(-1.37100000000000))+(pz)+(((IKReal(-0.0940000000000000))*(r22))));
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=((((IKReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10))));
rxp0_1=((((px)*(r20)))+(((IKReal(-1.00000000000000))*(pz)*(r00))));
rxp0_2=((((IKReal(-1.00000000000000))*(px)*(r10)))+(((py)*(r00))));
rxp1_0=((((IKReal(-1.00000000000000))*(py)*(r21)))+(((pz)*(r11))));
rxp1_1=((((px)*(r21)))+(((IKReal(-1.00000000000000))*(pz)*(r01))));
rxp1_2=((((IKReal(-1.00000000000000))*(px)*(r11)))+(((py)*(r01))));
rxp2_0=((((IKReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12))));
rxp2_1=((((px)*(r22)))+(((IKReal(-1.00000000000000))*(pz)*(r02))));
rxp2_2=((((IKReal(-1.00000000000000))*(px)*(r12)))+(((py)*(r02))));
{
IKReal j2array[2], cj2array[2], sj2array[2];
bool j2valid[2]={false};
_nj2 = 2;
if( (((IKReal(-1.06017897616850))+(((IKReal(2.67068488900329))*(pp))))) < -1-IKFAST_SINCOS_THRESH || (((IKReal(-1.06017897616850))+(((IKReal(2.67068488900329))*(pp))))) > 1+IKFAST_SINCOS_THRESH )
    continue;
IKReal x54=IKasin(((IKReal(-1.06017897616850))+(((IKReal(2.67068488900329))*(pp)))));
j2array[0]=((IKReal(0.0468371131132389))+(((IKReal(1.00000000000000))*(x54))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
j2array[1]=((IKReal(3.18842976670303))+(((IKReal(-1.00000000000000))*(x54))));
sj2array[1]=IKsin(j2array[1]);
cj2array[1]=IKcos(j2array[1]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
if( j2array[1] > IKPI )
{
    j2array[1]-=IK2PI;
}
else if( j2array[1] < -IKPI )
{    j2array[1]+=IK2PI;
}
j2valid[1] = true;
for(int ij2 = 0; ij2 < 2; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 2; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];

{
IKReal 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]=((IKReal(10.6354679802956))+(((IKReal(10.7231100422792))*((cj2)*(cj2))))+(((IKReal(-1.00000000000000))*(cj2)))+(((IKReal(21.3349753694581))*(sj2)))+(((IKReal(10.7231100422792))*((sj2)*(sj2)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IKReal j1array[2], cj1array[2], sj1array[2];
bool j1valid[2]={false};
_nj1 = 2;
IKReal x55=((((IKReal(-0.433100000000000))*(cj2)))+(((IKReal(-0.0203000000000000))*(sj2))));
IKReal x56=((IKReal(0.431800000000000))+(((IKReal(-0.0203000000000000))*(cj2)))+(((IKReal(0.433100000000000))*(sj2))));
if( IKabs(x55) < IKFAST_ATAN2_MAGTHRESH && IKabs(x56) < IKFAST_ATAN2_MAGTHRESH )
    continue;
IKReal x57=((IKReal(1.00000000000000))*(IKatan2(x55, x56)));
if( ((((x55)*(x55))+((x56)*(x56)))) < (IKReal)-0.00001 )
    continue;
if( (((pz)*(((IKabs(IKabs(IKsqrt((((x55)*(x55))+((x56)*(x56)))))) != 0)?((IKReal)1/(IKabs(IKsqrt((((x55)*(x55))+((x56)*(x56))))))):(IKReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((pz)*(((IKabs(IKabs(IKsqrt((((x55)*(x55))+((x56)*(x56)))))) != 0)?((IKReal)1/(IKabs(IKsqrt((((x55)*(x55))+((x56)*(x56))))))):(IKReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH )
    continue;
IKReal x58=IKasin(((pz)*(((IKabs(IKabs(IKsqrt((((x55)*(x55))+((x56)*(x56)))))) != 0)?((IKReal)1/(IKabs(IKsqrt((((x55)*(x55))+((x56)*(x56))))))):(IKReal)1.0e30))));
j1array[0]=((((IKReal(-1.00000000000000))*(x57)))+(((IKReal(-1.00000000000000))*(x58))));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
j1array[1]=((IKReal(3.14159265358979))+(x58)+(((IKReal(-1.00000000000000))*(x57))));
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;
IKReal x59=((IKReal(10000.0000000000))*(sj1));
gconst0=IKsign(((((x59)*((py)*(py))))+(((x59)*((px)*(px))))));
dummyeval[0]=((((sj1)*((py)*(py))))+(((sj1)*((px)*(px)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[4];
IKReal x60=((((IKReal(-0.433100000000000))*(cj2)))+(pz)+(((IKReal(-0.0203000000000000))*(sj2))));
evalcond[0]=((IKReal(-3.14159265358979))+(IKfmod(((IKReal(3.14159265358979))+(j1)), IKReal(6.28318530717959))));
evalcond[1]=((IKReal(0.396968950000000))+(((IKReal(-0.0175310800000000))*(cj2)))+(((IKReal(-1.00000000000000))*(pp)))+(((IKReal(0.374025160000000))*(sj2))));
evalcond[2]=x60;
evalcond[3]=x60;
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
IKReal gconst1;
gconst1=IKsign(((((IKReal(-21590000.0000000))*((px)*(px))))+(((IKReal(-21590000.0000000))*((py)*(py))))));
dummyeval[0]=((((IKReal(-1.00000000000000))*((px)*(px))))+(((IKReal(-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;
IKReal x61=((IKReal(25000000.0000000))*(pp));
if( IKabs(((gconst1)*(((((IKReal(601661.750000000))*(py)))+(((IKReal(-1.00000000000000))*(py)*(x61)))+(((IKReal(3240659.00000000))*(px))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IKReal(-3240659.00000000))*(py)))+(((IKReal(-1.00000000000000))*(px)*(x61)))+(((IKReal(601661.750000000))*(px))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j0array[0]=IKatan2(((gconst1)*(((((IKReal(601661.750000000))*(py)))+(((IKReal(-1.00000000000000))*(py)*(x61)))+(((IKReal(3240659.00000000))*(px)))))), ((gconst1)*(((((IKReal(-3240659.00000000))*(py)))+(((IKReal(-1.00000000000000))*(px)*(x61)))+(((IKReal(601661.750000000))*(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[3];
IKReal x62=IKsin(j0);
IKReal x63=IKcos(j0);
IKReal x64=((px)*(x63));
IKReal x65=((py)*(x62));
evalcond[0]=((IKReal(0.150100000000000))+(((px)*(x62)))+(((IKReal(-1.00000000000000))*(py)*(x63))));
evalcond[1]=((IKReal(0.0240664700000000))+(((IKReal(-1.00000000000000))*(pp)))+(((IKReal(0.863600000000000))*(x64)))+(((IKReal(0.863600000000000))*(x65))));
evalcond[2]=((IKReal(0.431800000000000))+(((IKReal(-0.0203000000000000))*(cj2)))+(((IKReal(0.433100000000000))*(sj2)))+(((IKReal(-1.00000000000000))*(x64)))+(((IKReal(-1.00000000000000))*(x65))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
{
continue;
}
}

rotationfunction0(vsolutions);
}
}

}

}

} else
{
IKReal x131=((IKReal(0.0203000000000000))*(sj2));
IKReal x132=((IKReal(0.433100000000000))*(cj2));
IKReal x133=((x131)+(x132));
evalcond[0]=((IKReal(-3.14159265358979))+(IKfmod(((IKReal(1.11022302462516e-16))+(j1)), IKReal(6.28318530717959))));
evalcond[1]=((IKReal(0.396968950000000))+(((IKReal(-0.0175310800000000))*(cj2)))+(((IKReal(-1.00000000000000))*(pp)))+(((IKReal(0.374025160000000))*(sj2))));
evalcond[2]=((((IKReal(-1.00000000000000))*(pz)))+(((IKReal(-1.00000000000000))*(x133))));
evalcond[3]=((x133)+(pz));
if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
{
{
IKReal dummyeval[1];
IKReal gconst2;
gconst2=IKsign(((((IKReal(21590000.0000000))*((px)*(px))))+(((IKReal(21590000.0000000))*((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;
IKReal x134=((IKReal(25000000.0000000))*(pp));
if( IKabs(((gconst2)*(((((IKReal(-3240659.00000000))*(px)))+(((IKReal(-1.00000000000000))*(py)*(x134)))+(((IKReal(601661.750000000))*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((IKReal(-1.00000000000000))*(px)*(x134)))+(((IKReal(601661.750000000))*(px)))+(((IKReal(3240659.00000000))*(py))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j0array[0]=IKatan2(((gconst2)*(((((IKReal(-3240659.00000000))*(px)))+(((IKReal(-1.00000000000000))*(py)*(x134)))+(((IKReal(601661.750000000))*(py)))))), ((gconst2)*(((((IKReal(-1.00000000000000))*(px)*(x134)))+(((IKReal(601661.750000000))*(px)))+(((IKReal(3240659.00000000))*(py)))))));
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[3];
IKReal x135=IKsin(j0);
IKReal x136=IKcos(j0);
IKReal x137=((px)*(x136));
IKReal x138=((py)*(x135));
evalcond[0]=((IKReal(0.150100000000000))+(((px)*(x135)))+(((IKReal(-1.00000000000000))*(py)*(x136))));
evalcond[1]=((IKReal(0.0240664700000000))+(((IKReal(-1.00000000000000))*(pp)))+(((IKReal(-0.863600000000000))*(x138)))+(((IKReal(-0.863600000000000))*(x137))));
evalcond[2]=((IKReal(0.431800000000000))+(x137)+(x138)+(((IKReal(-0.0203000000000000))*(cj2)))+(((IKReal(0.433100000000000))*(sj2))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
{
continue;
}
}

rotationfunction0(vsolutions);
}
}

}

}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j0array[1], cj0array[1], sj0array[1];
bool j0valid[1]={false};
_nj0 = 1;
IKReal x139=((IKReal(203.000000000000))*(sj2));
IKReal x140=((IKReal(4331.00000000000))*(cj2));
IKReal x141=((IKReal(1501.00000000000))*(sj1));
IKReal x142=((IKReal(10000.0000000000))*(cj1)*(pz));
if( IKabs(((gconst0)*(((((IKReal(-1.00000000000000))*(py)*(x142)))+(((py)*(x139)))+(((IKReal(-1.00000000000000))*(px)*(x141)))+(((py)*(x140))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((px)*(x139)))+(((IKReal(-1.00000000000000))*(px)*(x142)))+(((px)*(x140)))+(((py)*(x141))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j0array[0]=IKatan2(((gconst0)*(((((IKReal(-1.00000000000000))*(py)*(x142)))+(((py)*(x139)))+(((IKReal(-1.00000000000000))*(px)*(x141)))+(((py)*(x140)))))), ((gconst0)*(((((px)*(x139)))+(((IKReal(-1.00000000000000))*(px)*(x142)))+(((px)*(x140)))+(((py)*(x141)))))));
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[5];
IKReal x143=IKsin(j0);
IKReal x144=IKcos(j0);
IKReal x145=((IKReal(0.433100000000000))*(sj2));
IKReal x146=((IKReal(0.433100000000000))*(cj2));
IKReal x147=((pz)*(sj1));
IKReal x148=((IKReal(0.0203000000000000))*(cj2));
IKReal x149=((IKReal(0.863600000000000))*(cj1));
IKReal x150=((IKReal(1.00000000000000))*(cj1));
IKReal x151=((IKReal(0.0203000000000000))*(sj2));
IKReal x152=((px)*(x144));
IKReal x153=((py)*(x143));
evalcond[0]=((IKReal(0.150100000000000))+(((IKReal(-1.00000000000000))*(py)*(x144)))+(((px)*(x143))));
evalcond[1]=((IKReal(0.0240664700000000))+(((x149)*(x152)))+(((x149)*(x153)))+(((IKReal(-1.00000000000000))*(pp)))+(((IKReal(-0.863600000000000))*(x147))));
evalcond[2]=((((IKReal(-1.00000000000000))*(x146)))+(((IKReal(-1.00000000000000))*(x151)))+(((sj1)*(x152)))+(((sj1)*(x153)))+(((cj1)*(pz))));
evalcond[3]=((IKReal(0.431800000000000))+(((IKReal(-1.00000000000000))*(x148)))+(((IKReal(-1.00000000000000))*(x150)*(x153)))+(((IKReal(-1.00000000000000))*(x150)*(x152)))+(x145)+(x147));
evalcond[4]=((((IKReal(0.431800000000000))*(cj1)))+(((sj1)*(x151)))+(((sj1)*(x146)))+(((IKReal(-1.00000000000000))*(x152)))+(((IKReal(-1.00000000000000))*(x153)))+(((IKReal(-1.00000000000000))*(cj1)*(x148)))+(((cj1)*(x145))));
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  )
{
continue;
}
}

rotationfunction0(vsolutions);
}
}

}

}
}
}

}

}

} else
{
{
IKReal j0array[2], cj0array[2], sj0array[2];
bool j0valid[2]={false};
_nj0 = 2;
if( IKabs(((IKReal(-1.00000000000000))*(py))) < IKFAST_ATAN2_MAGTHRESH && IKabs(px) < IKFAST_ATAN2_MAGTHRESH )
    continue;
IKReal x154=((IKReal(1.00000000000000))*(IKatan2(((IKReal(-1.00000000000000))*(py)), px)));
if( ((((px)*(px))+((py)*(py)))) < (IKReal)-0.00001 )
    continue;
if( (((IKReal(0.150100000000000))*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IKReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IKReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((IKReal(0.150100000000000))*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IKReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IKReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH )
    continue;
IKReal x155=IKasin(((IKReal(0.150100000000000))*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IKReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IKReal)1.0e30))));
j0array[0]=((((IKReal(-1.00000000000000))*(x154)))+(((IKReal(-1.00000000000000))*(x155))));
sj0array[0]=IKsin(j0array[0]);
cj0array[0]=IKcos(j0array[0]);
j0array[1]=((IKReal(3.14159265358979))+(((IKReal(-1.00000000000000))*(x154)))+(x155));
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(((((IKReal(2159.00000000000))*((pz)*(pz))))+(((IKReal(4318.00000000000))*(cj0)*(px)*(py)*(sj0)))+(((IKReal(2159.00000000000))*((cj0)*(cj0))*((px)*(px))))+(((IKReal(2159.00000000000))*((py)*(py))*((sj0)*(sj0))))));
dummyeval[0]=(((((py)*(py))*((sj0)*(sj0))))+((pz)*(pz))+((((cj0)*(cj0))*((px)*(px))))+(((IKReal(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};
_nj1 = 1;
IKReal x156=((IKReal(43.8277000000000))*(sj2));
IKReal x157=((cj0)*(px));
IKReal x158=((py)*(sj0));
IKReal x159=((IKReal(2500.00000000000))*(pp));
IKReal x160=((IKReal(935.062900000000))*(cj2));
if( IKabs(((gconst3)*(((((x157)*(x160)))+(((IKReal(60.1661750000000))*(pz)))+(((IKReal(-1.00000000000000))*(pz)*(x159)))+(((x156)*(x158)))+(((x156)*(x157)))+(((x158)*(x160))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((pz)*(x160)))+(((x157)*(x159)))+(((IKReal(-60.1661750000000))*(x157)))+(((IKReal(-60.1661750000000))*(x158)))+(((x158)*(x159)))+(((pz)*(x156))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j1array[0]=IKatan2(((gconst3)*(((((x157)*(x160)))+(((IKReal(60.1661750000000))*(pz)))+(((IKReal(-1.00000000000000))*(pz)*(x159)))+(((x156)*(x158)))+(((x156)*(x157)))+(((x158)*(x160)))))), ((gconst3)*(((((pz)*(x160)))+(((x157)*(x159)))+(((IKReal(-60.1661750000000))*(x157)))+(((IKReal(-60.1661750000000))*(x158)))+(((x158)*(x159)))+(((pz)*(x156)))))));
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[5];
IKReal x161=IKcos(j1);
IKReal x162=IKsin(j1);
IKReal x163=((IKReal(0.433100000000000))*(sj2));
IKReal x164=((cj0)*(px));
IKReal x165=((IKReal(0.433100000000000))*(cj2));
IKReal x166=((py)*(sj0));
IKReal x167=((IKReal(0.0203000000000000))*(cj2));
IKReal x168=((IKReal(0.0203000000000000))*(sj2));
IKReal x169=((IKReal(1.00000000000000))*(x161));
IKReal x170=((IKReal(0.0203000000000000))*(x162));
IKReal x171=((pz)*(x162));
IKReal x172=((IKReal(0.863600000000000))*(x161));
evalcond[0]=((IKReal(0.0240664700000000))+(((IKReal(-0.863600000000000))*(x171)))+(((x166)*(x172)))+(((IKReal(-1.00000000000000))*(pp)))+(((x164)*(x172))));
evalcond[1]=((((IKReal(-1.00000000000000))*(x168)))+(((IKReal(-1.00000000000000))*(x165)))+(((pz)*(x161)))+(((x162)*(x164)))+(((x162)*(x166))));
evalcond[2]=((IKReal(0.431800000000000))+(((IKReal(-1.00000000000000))*(x166)*(x169)))+(((IKReal(-1.00000000000000))*(x167)))+(x171)+(x163)+(((IKReal(-1.00000000000000))*(x164)*(x169))));
evalcond[3]=((((x162)*(x163)))+(((IKReal(-1.00000000000000))*(x161)*(x165)))+(((IKReal(-1.00000000000000))*(x161)*(x168)))+(((IKReal(-1.00000000000000))*(x162)*(x167)))+(pz)+(((IKReal(0.431800000000000))*(x162))));
evalcond[4]=((((x161)*(x163)))+(((x162)*(x165)))+(((x162)*(x168)))+(((IKReal(-1.00000000000000))*(x161)*(x167)))+(((IKReal(-1.00000000000000))*(x166)))+(((IKReal(-1.00000000000000))*(x164)))+(((IKReal(0.431800000000000))*(x161))));
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  )
{
continue;
}
}

rotationfunction0(vsolutions);
}
}

}

}
}
}

}

}

} else
{
{
IKReal j0array[2], cj0array[2], sj0array[2];
bool j0valid[2]={false};
_nj0 = 2;
if( IKabs(((IKReal(-1.00000000000000))*(py))) < IKFAST_ATAN2_MAGTHRESH && IKabs(px) < IKFAST_ATAN2_MAGTHRESH )
    continue;
IKReal x173=((IKReal(1.00000000000000))*(IKatan2(((IKReal(-1.00000000000000))*(py)), px)));
if( ((((px)*(px))+((py)*(py)))) < (IKReal)-0.00001 )
    continue;
if( (((IKReal(0.150100000000000))*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IKReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IKReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((IKReal(0.150100000000000))*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IKReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IKReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH )
    continue;
IKReal x174=IKasin(((IKReal(0.150100000000000))*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IKReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IKReal)1.0e30))));
j0array[0]=((((IKReal(-1.00000000000000))*(x173)))+(((IKReal(-1.00000000000000))*(x174))));
sj0array[0]=IKsin(j0array[0]);
cj0array[0]=IKcos(j0array[0]);
j0array[1]=((IKReal(3.14159265358979))+(((IKReal(-1.00000000000000))*(x173)))+(x174));
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(((((IKReal(2159.00000000000))*((pz)*(pz))))+(((IKReal(4318.00000000000))*(cj0)*(px)*(py)*(sj0)))+(((IKReal(2159.00000000000))*((cj0)*(cj0))*((px)*(px))))+(((IKReal(2159.00000000000))*((py)*(py))*((sj0)*(sj0))))));
dummyeval[0]=(((((py)*(py))*((sj0)*(sj0))))+((pz)*(pz))+((((cj0)*(cj0))*((px)*(px))))+(((IKReal(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};
_nj1 = 1;
IKReal x175=((IKReal(43.8277000000000))*(sj2));
IKReal x176=((cj0)*(px));
IKReal x177=((py)*(sj0));
IKReal x178=((IKReal(2500.00000000000))*(pp));
IKReal x179=((IKReal(935.062900000000))*(cj2));
if( IKabs(((gconst3)*(((((IKReal(60.1661750000000))*(pz)))+(((x176)*(x179)))+(((IKReal(-1.00000000000000))*(pz)*(x178)))+(((x175)*(x176)))+(((x175)*(x177)))+(((x177)*(x179))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((pz)*(x175)))+(((pz)*(x179)))+(((x176)*(x178)))+(((IKReal(-60.1661750000000))*(x177)))+(((IKReal(-60.1661750000000))*(x176)))+(((x177)*(x178))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j1array[0]=IKatan2(((gconst3)*(((((IKReal(60.1661750000000))*(pz)))+(((x176)*(x179)))+(((IKReal(-1.00000000000000))*(pz)*(x178)))+(((x175)*(x176)))+(((x175)*(x177)))+(((x177)*(x179)))))), ((gconst3)*(((((pz)*(x175)))+(((pz)*(x179)))+(((x176)*(x178)))+(((IKReal(-60.1661750000000))*(x177)))+(((IKReal(-60.1661750000000))*(x176)))+(((x177)*(x178)))))));
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[5];
IKReal x180=IKcos(j1);
IKReal x181=IKsin(j1);
IKReal x182=((IKReal(0.433100000000000))*(sj2));
IKReal x183=((cj0)*(px));
IKReal x184=((IKReal(0.433100000000000))*(cj2));
IKReal x185=((py)*(sj0));
IKReal x186=((IKReal(0.0203000000000000))*(cj2));
IKReal x187=((IKReal(0.0203000000000000))*(sj2));
IKReal x188=((IKReal(1.00000000000000))*(x180));
IKReal x189=((IKReal(0.0203000000000000))*(x181));
IKReal x190=((pz)*(x181));
IKReal x191=((IKReal(0.863600000000000))*(x180));
evalcond[0]=((IKReal(0.0240664700000000))+(((IKReal(-0.863600000000000))*(x190)))+(((x185)*(x191)))+(((IKReal(-1.00000000000000))*(pp)))+(((x183)*(x191))));
evalcond[1]=((((pz)*(x180)))+(((IKReal(-1.00000000000000))*(x187)))+(((IKReal(-1.00000000000000))*(x184)))+(((x181)*(x183)))+(((x181)*(x185))));
evalcond[2]=((IKReal(0.431800000000000))+(((IKReal(-1.00000000000000))*(x186)))+(x190)+(x182)+(((IKReal(-1.00000000000000))*(x185)*(x188)))+(((IKReal(-1.00000000000000))*(x183)*(x188))));
evalcond[3]=((((IKReal(-1.00000000000000))*(x180)*(x184)))+(((IKReal(-1.00000000000000))*(x180)*(x187)))+(pz)+(((IKReal(0.431800000000000))*(x181)))+(((x181)*(x182)))+(((IKReal(-1.00000000000000))*(x181)*(x186))));
evalcond[4]=((((x180)*(x182)))+(((IKReal(-1.00000000000000))*(x183)))+(((IKReal(-1.00000000000000))*(x185)))+(((IKReal(-1.00000000000000))*(x180)*(x186)))+(((IKReal(0.431800000000000))*(x180)))+(((x181)*(x187)))+(((x181)*(x184))));
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  )
{
continue;
}
}

rotationfunction0(vsolutions);
}
}

}

}
}
}

}

}
}
}
}
return vsolutions.size()>0;
}
inline void rotationfunction0(std::vector<IKSolution>& vsolutions) {
for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
IKReal x66=((cj0)*(r00));
IKReal x67=((cj0)*(r01));
IKReal x68=((IKReal(1.00000000000000))*(sj1));
IKReal x69=((IKReal(1.00000000000000))*(sj0));
IKReal x70=((r11)*(sj0));
IKReal x71=((cj1)*(sj2));
IKReal x72=((cj0)*(r02));
IKReal x73=((((IKReal(-1.00000000000000))*(sj2)*(x68)))+(((cj1)*(cj2))));
IKReal x74=((((cj2)*(sj1)))+(x71));
IKReal x75=((cj0)*(x73));
IKReal x76=((sj0)*(x73));
IKReal x77=((sj0)*(x74));
IKReal x78=((((IKReal(-1.00000000000000))*(x71)))+(((IKReal(-1.00000000000000))*(cj2)*(x68))));
new_r00=((((r10)*(x76)))+(((r20)*(x78)))+(((x66)*(x73))));
new_r01=((((r21)*(x78)))+(((x67)*(x73)))+(((x70)*(x73))));
new_r02=((((r22)*(x78)))+(((x72)*(x73)))+(((r12)*(x76))));
new_r10=((((IKReal(-1.00000000000000))*(r00)*(x69)))+(((cj0)*(r10))));
new_r11=((((IKReal(-1.00000000000000))*(r01)*(x69)))+(((cj0)*(r11))));
new_r12=((((IKReal(-1.00000000000000))*(r02)*(x69)))+(((cj0)*(r12))));
new_r20=((((r10)*(x77)))+(((r20)*(x73)))+(((x66)*(x74))));
new_r21=((((r21)*(x73)))+(((x67)*(x74)))+(((x70)*(x74))));
new_r22=((((r22)*(x73)))+(((x72)*(x74)))+(((r12)*(x77))));
{
IKReal j4array[2], cj4array[2], sj4array[2];
bool j4valid[2]={false};
_nj4 = 2;
cj4array[0]=new_r22;
if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
{
    j4valid[0] = j4valid[1] = true;
    j4array[0] = IKacos(cj4array[0]);
    sj4array[0] = IKsin(j4array[0]);
    cj4array[1] = cj4array[0];
    j4array[1] = -j4array[0];
    sj4array[1] = -sj4array[0];
}
else if( isnan(cj4array[0]) )
{
    // probably any value will work
    j4valid[0] = true;
    cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
}
for(int ij4 = 0; ij4 < 2; ++ij4)
{
if( !j4valid[ij4] )
{
    continue;
}
_ij4[0] = ij4; _ij4[1] = -1;
for(int iij4 = ij4+1; iij4 < 2; ++iij4)
{
if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
{
    j4valid[iij4]=false; _ij4[1] = iij4; break; 
}
}
j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];

{
IKReal dummyeval[1];
IKReal gconst5;
gconst5=IKsign(sj4);
dummyeval[0]=sj4;
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]=((IKReal(-3.14159265358979))+(IKfmod(((IKReal(3.14159265358979))+(j4)), IKReal(6.28318530717959))));
evalcond[1]=((IKReal(-1.00000000000000))+(new_r22));
evalcond[2]=new_r20;
evalcond[3]=new_r21;
evalcond[4]=((IKReal(-1.00000000000000))*(new_r20));
evalcond[5]=((IKReal(-1.00000000000000))*(new_r21));
evalcond[6]=((IKReal(1.00000000000000))+(((IKReal(-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 j3array[2], cj3array[2], sj3array[2];
bool j3valid[2]={false};
_nj3 = 2;
if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
    continue;
IKReal x79=IKatan2(new_r02, new_r12);
j3array[0]=((IKReal(-1.00000000000000))*(x79));
sj3array[0]=IKsin(j3array[0]);
cj3array[0]=IKcos(j3array[0]);
j3array[1]=((IKReal(3.14159265358979))+(((IKReal(-1.00000000000000))*(x79))));
sj3array[1]=IKsin(j3array[1]);
cj3array[1]=IKcos(j3array[1]);
if( j3array[0] > IKPI )
{
    j3array[0]-=IK2PI;
}
else if( j3array[0] < -IKPI )
{    j3array[0]+=IK2PI;
}
j3valid[0] = true;
if( j3array[1] > IKPI )
{
    j3array[1]-=IK2PI;
}
else if( j3array[1] < -IKPI )
{    j3array[1]+=IK2PI;
}
j3valid[1] = true;
for(int ij3 = 0; ij3 < 2; ++ij3)
{
if( !j3valid[ij3] )
{
    continue;
}
_ij3[0] = ij3; _ij3[1] = -1;
for(int iij3 = ij3+1; iij3 < 2; ++iij3)
{
if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
{
    j3valid[iij3]=false; _ij3[1] = iij3; break; 
}
}
j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
{
IKReal evalcond[1];
evalcond[0]=((((IKReal(-1.00000000000000))*(new_r02)*(IKsin(j3))))+(((new_r12)*(IKcos(j3)))));
if( IKabs(evalcond[0]) > 0.000001  )
{
continue;
}
}

{
IKReal j5array[1], cj5array[1], sj5array[1];
bool j5valid[1]={false};
_nj5 = 1;
if( IKabs(((((IKReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IKReal(-1.00000000000000))*(new_r11)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IKReal(-1.00000000000000))*(new_r11)*(sj3)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH )
    continue;
j5array[0]=IKatan2(((((IKReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IKReal(-1.00000000000000))*(new_r11)*(sj3)))), ((((new_r10)*(sj3)))+(((cj3)*(new_r00)))));
sj5array[0]=IKsin(j5array[0]);
cj5array[0]=IKcos(j5array[0]);
if( j5array[0] > IKPI )
{
    j5array[0]-=IK2PI;
}
else if( j5array[0] < -IKPI )
{    j5array[0]+=IK2PI;
}
j5valid[0] = true;
for(int ij5 = 0; ij5 < 1; ++ij5)
{
if( !j5valid[ij5] )
{
    continue;
}
_ij5[0] = ij5; _ij5[1] = -1;
for(int iij5 = ij5+1; iij5 < 1; ++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 evalcond[4];
IKReal x80=IKsin(j5);
IKReal x81=((IKReal(1.00000000000000))*(sj3));
IKReal x82=((IKReal(1.00000000000000))*(IKcos(j5)));
evalcond[0]=((((IKReal(-1.00000000000000))*(new_r00)*(x81)))+(((IKReal(-1.00000000000000))*(x80)))+(((cj3)*(new_r10))));
evalcond[1]=((((IKReal(-1.00000000000000))*(new_r01)*(x81)))+(((cj3)*(new_r11)))+(((IKReal(-1.00000000000000))*(x82))));
evalcond[2]=((((new_r11)*(sj3)))+(x80)+(((cj3)*(new_r01))));
evalcond[3]=((((new_r10)*(sj3)))+(((cj3)*(new_r00)))+(((IKReal(-1.00000000000000))*(x82))));
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(6);
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.vfree.resize(0);
}
}
}
}
}

} else
{
IKReal x83=((IKReal(1.00000000000000))+(new_r22));
evalcond[0]=((IKReal(-3.14159265358979))+(IKfmod(((IKReal(1.11022302462516e-16))+(j4)), IKReal(6.28318530717959))));
evalcond[1]=x83;
evalcond[2]=new_r20;
evalcond[3]=new_r21;
evalcond[4]=new_r20;
evalcond[5]=new_r21;
evalcond[6]=x83;
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 j3array[2], cj3array[2], sj3array[2];
bool j3valid[2]={false};
_nj3 = 2;
if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
    continue;
IKReal x84=IKatan2(new_r02, new_r12);
j3array[0]=((IKReal(-1.00000000000000))*(x84));
sj3array[0]=IKsin(j3array[0]);
cj3array[0]=IKcos(j3array[0]);
j3array[1]=((IKReal(3.14159265358979))+(((IKReal(-1.00000000000000))*(x84))));
sj3array[1]=IKsin(j3array[1]);
cj3array[1]=IKcos(j3array[1]);
if( j3array[0] > IKPI )
{
    j3array[0]-=IK2PI;
}
else if( j3array[0] < -IKPI )
{    j3array[0]+=IK2PI;
}
j3valid[0] = true;
if( j3array[1] > IKPI )
{
    j3array[1]-=IK2PI;
}
else if( j3array[1] < -IKPI )
{    j3array[1]+=IK2PI;
}
j3valid[1] = true;
for(int ij3 = 0; ij3 < 2; ++ij3)
{
if( !j3valid[ij3] )
{
    continue;
}
_ij3[0] = ij3; _ij3[1] = -1;
for(int iij3 = ij3+1; iij3 < 2; ++iij3)
{
if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
{
    j3valid[iij3]=false; _ij3[1] = iij3; break; 
}
}
j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
{
IKReal evalcond[1];
evalcond[0]=((((IKReal(-1.00000000000000))*(new_r02)*(IKsin(j3))))+(((new_r12)*(IKcos(j3)))));
if( IKabs(evalcond[0]) > 0.000001  )
{
continue;
}
}

{
IKReal j5array[1], cj5array[1], sj5array[1];
bool j5valid[1]={false};
_nj5 = 1;
if( IKabs(((((new_r11)*(sj3)))+(((cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IKReal(-1.00000000000000))*(new_r10)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))))+IKsqr(((((IKReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IKReal(-1.00000000000000))*(new_r10)*(sj3)))))-1) <= IKFAST_SINCOS_THRESH )
    continue;
j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IKReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IKReal(-1.00000000000000))*(new_r10)*(sj3)))));
sj5array[0]=IKsin(j5array[0]);
cj5array[0]=IKcos(j5array[0]);
if( j5array[0] > IKPI )
{
    j5array[0]-=IK2PI;
}
else if( j5array[0] < -IKPI )
{    j5array[0]+=IK2PI;
}
j5valid[0] = true;
for(int ij5 = 0; ij5 < 1; ++ij5)
{
if( !j5valid[ij5] )
{
    continue;
}
_ij5[0] = ij5; _ij5[1] = -1;
for(int iij5 = ij5+1; iij5 < 1; ++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 evalcond[4];
IKReal x85=IKcos(j5);
IKReal x86=((IKReal(1.00000000000000))*(sj3));
IKReal x87=((IKReal(1.00000000000000))*(IKsin(j5)));
evalcond[0]=((((IKReal(-1.00000000000000))*(new_r00)*(x86)))+(((cj3)*(new_r10)))+(((IKReal(-1.00000000000000))*(x87))));
evalcond[1]=((((IKReal(-1.00000000000000))*(new_r01)*(x86)))+(((IKReal(-1.00000000000000))*(x85)))+(((cj3)*(new_r11))));
evalcond[2]=((((new_r11)*(sj3)))+(((cj3)*(new_r01)))+(((IKReal(-1.00000000000000))*(x87))));
evalcond[3]=((((new_r10)*(sj3)))+(x85)+(((cj3)*(new_r00))));
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(6);
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.vfree.resize(0);
}
}
}
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j3array[1], cj3array[1], sj3array[1];
bool j3valid[1]={false};
_nj3 = 1;
IKReal x88=((gconst4)*(sj4));
if( IKabs(((new_r12)*(x88))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x88))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j3array[0]=IKatan2(((new_r12)*(x88)), ((new_r02)*(x88)));
sj3array[0]=IKsin(j3array[0]);
cj3array[0]=IKcos(j3array[0]);
if( j3array[0] > IKPI )
{
    j3array[0]-=IK2PI;
}
else if( j3array[0] < -IKPI )
{    j3array[0]+=IK2PI;
}
j3valid[0] = true;
for(int ij3 = 0; ij3 < 1; ++ij3)
{
if( !j3valid[ij3] )
{
    continue;
}
_ij3[0] = ij3; _ij3[1] = -1;
for(int iij3 = ij3+1; iij3 < 1; ++iij3)
{
if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
{
    j3valid[iij3]=false; _ij3[1] = iij3; break; 
}
}
j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
{
IKReal evalcond[6];
IKReal x89=IKsin(j3);
IKReal x90=IKcos(j3);
IKReal x91=((IKReal(1.00000000000000))*(sj4));
IKReal x92=((IKReal(1.00000000000000))*(cj4));
IKReal x93=((new_r02)*(x90));
IKReal x94=((new_r12)*(x89));
evalcond[0]=((((IKReal(-1.00000000000000))*(new_r02)*(x89)))+(((new_r12)*(x90))));
evalcond[1]=((x93)+(x94)+(((IKReal(-1.00000000000000))*(x91))));
evalcond[2]=((((IKReal(-1.00000000000000))*(new_r22)*(x91)))+(((cj4)*(x94)))+(((cj4)*(x93))));
evalcond[3]=((((IKReal(-1.00000000000000))*(new_r10)*(x89)*(x91)))+(((IKReal(-1.00000000000000))*(new_r00)*(x90)*(x91)))+(((IKReal(-1.00000000000000))*(new_r20)*(x92))));
evalcond[4]=((((IKReal(-1.00000000000000))*(new_r01)*(x90)*(x91)))+(((IKReal(-1.00000000000000))*(new_r21)*(x92)))+(((IKReal(-1.00000000000000))*(new_r11)*(x89)*(x91))));
evalcond[5]=((IKReal(1.00000000000000))+(((IKReal(-1.00000000000000))*(x91)*(x93)))+(((IKReal(-1.00000000000000))*(x91)*(x94)))+(((IKReal(-1.00000000000000))*(new_r22)*(x92))));
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(sj4);
dummyeval[0]=sj4;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IKReal evalcond[11];
IKReal x95=((((IKReal(-1.00000000000000))*(new_r02)*(sj3)))+(((cj3)*(new_r12))));
IKReal x96=((((new_r12)*(sj3)))+(((cj3)*(new_r02))));
evalcond[0]=((IKReal(-3.14159265358979))+(IKfmod(((IKReal(3.14159265358979))+(j4)), IKReal(6.28318530717959))));
evalcond[1]=((IKReal(-1.00000000000000))+(new_r22));
evalcond[2]=new_r20;
evalcond[3]=new_r21;
evalcond[4]=x95;
evalcond[5]=x95;
evalcond[6]=x96;
evalcond[7]=x96;
evalcond[8]=((IKReal(-1.00000000000000))*(new_r20));
evalcond[9]=((IKReal(-1.00000000000000))*(new_r21));
evalcond[10]=((IKReal(1.00000000000000))+(((IKReal(-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 j5array[1], cj5array[1], sj5array[1];
bool j5valid[1]={false};
_nj5 = 1;
if( IKabs(((((IKReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IKReal(-1.00000000000000))*(new_r11)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IKReal(-1.00000000000000))*(new_r11)*(sj3)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH )
    continue;
j5array[0]=IKatan2(((((IKReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IKReal(-1.00000000000000))*(new_r11)*(sj3)))), ((((new_r10)*(sj3)))+(((cj3)*(new_r00)))));
sj5array[0]=IKsin(j5array[0]);
cj5array[0]=IKcos(j5array[0]);
if( j5array[0] > IKPI )
{
    j5array[0]-=IK2PI;
}
else if( j5array[0] < -IKPI )
{    j5array[0]+=IK2PI;
}
j5valid[0] = true;
for(int ij5 = 0; ij5 < 1; ++ij5)
{
if( !j5valid[ij5] )
{
    continue;
}
_ij5[0] = ij5; _ij5[1] = -1;
for(int iij5 = ij5+1; iij5 < 1; ++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 evalcond[4];
IKReal x97=IKsin(j5);
IKReal x98=((IKReal(1.00000000000000))*(sj3));
IKReal x99=((IKReal(1.00000000000000))*(IKcos(j5)));
evalcond[0]=((((cj3)*(new_r10)))+(((IKReal(-1.00000000000000))*(new_r00)*(x98)))+(((IKReal(-1.00000000000000))*(x97))));
evalcond[1]=((((IKReal(-1.00000000000000))*(new_r01)*(x98)))+(((cj3)*(new_r11)))+(((IKReal(-1.00000000000000))*(x99))));
evalcond[2]=((((new_r11)*(sj3)))+(x97)+(((cj3)*(new_r01))));
evalcond[3]=((((new_r10)*(sj3)))+(((IKReal(-1.00000000000000))*(x99)))+(((cj3)*(new_r00))));
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(6);
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.vfree.resize(0);
}
}
}

} else
{
IKReal x100=((IKReal(1.00000000000000))+(new_r22));
IKReal x101=((new_r12)*(sj3));
IKReal x102=((IKReal(1.00000000000000))*(new_r02));
IKReal x103=((((IKReal(-1.00000000000000))*(sj3)*(x102)))+(((cj3)*(new_r12))));
evalcond[0]=((IKReal(-3.14159265358979))+(IKfmod(((IKReal(1.11022302462516e-16))+(j4)), IKReal(6.28318530717959))));
evalcond[1]=x100;
evalcond[2]=new_r20;
evalcond[3]=new_r21;
evalcond[4]=x103;
evalcond[5]=x103;
evalcond[6]=((x101)+(((cj3)*(new_r02))));
evalcond[7]=((((IKReal(-1.00000000000000))*(cj3)*(x102)))+(((IKReal(-1.00000000000000))*(x101))));
evalcond[8]=new_r20;
evalcond[9]=new_r21;
evalcond[10]=x100;
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 j5array[1], cj5array[1], sj5array[1];
bool j5valid[1]={false};
_nj5 = 1;
if( IKabs(((((new_r11)*(sj3)))+(((cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IKReal(-1.00000000000000))*(new_r10)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))))+IKsqr(((((IKReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IKReal(-1.00000000000000))*(new_r10)*(sj3)))))-1) <= IKFAST_SINCOS_THRESH )
    continue;
j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IKReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IKReal(-1.00000000000000))*(new_r10)*(sj3)))));
sj5array[0]=IKsin(j5array[0]);
cj5array[0]=IKcos(j5array[0]);
if( j5array[0] > IKPI )
{
    j5array[0]-=IK2PI;
}
else if( j5array[0] < -IKPI )
{    j5array[0]+=IK2PI;
}
j5valid[0] = true;
for(int ij5 = 0; ij5 < 1; ++ij5)
{
if( !j5valid[ij5] )
{
    continue;
}
_ij5[0] = ij5; _ij5[1] = -1;
for(int iij5 = ij5+1; iij5 < 1; ++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 evalcond[4];
IKReal x104=IKcos(j5);
IKReal x105=((IKReal(1.00000000000000))*(sj3));
IKReal x106=((IKReal(1.00000000000000))*(IKsin(j5)));
evalcond[0]=((((IKReal(-1.00000000000000))*(new_r00)*(x105)))+(((IKReal(-1.00000000000000))*(x106)))+(((cj3)*(new_r10))));
evalcond[1]=((((IKReal(-1.00000000000000))*(new_r01)*(x105)))+(((cj3)*(new_r11)))+(((IKReal(-1.00000000000000))*(x104))));
evalcond[2]=((((new_r11)*(sj3)))+(((IKReal(-1.00000000000000))*(x106)))+(((cj3)*(new_r01))));
evalcond[3]=((((new_r10)*(sj3)))+(x104)+(((cj3)*(new_r00))));
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(6);
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.vfree.resize(0);
}
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IKReal j5array[1], cj5array[1], sj5array[1];
bool j5valid[1]={false};
_nj5 = 1;
if( IKabs(((gconst6)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IKReal(-1.00000000000000))*(gconst6)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j5array[0]=IKatan2(((gconst6)*(new_r21)), ((IKReal(-1.00000000000000))*(gconst6)*(new_r20)));
sj5array[0]=IKsin(j5array[0]);
cj5array[0]=IKcos(j5array[0]);
if( j5array[0] > IKPI )
{
    j5array[0]-=IK2PI;
}
else if( j5array[0] < -IKPI )
{    j5array[0]+=IK2PI;
}
j5valid[0] = true;
for(int ij5 = 0; ij5 < 1; ++ij5)
{
if( !j5valid[ij5] )
{
    continue;
}
_ij5[0] = ij5; _ij5[1] = -1;
for(int iij5 = ij5+1; iij5 < 1; ++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 evalcond[8];
IKReal x107=IKsin(j5);
IKReal x108=IKcos(j5);
IKReal x109=((IKReal(1.00000000000000))*(sj3));
IKReal x110=((new_r11)*(sj3));
IKReal x111=((new_r10)*(sj3));
IKReal x112=((cj3)*(cj4));
IKReal x113=((IKReal(1.00000000000000))*(sj4));
IKReal x114=((IKReal(1.00000000000000))*(x108));
IKReal x115=((IKReal(1.00000000000000))*(x107));
evalcond[0]=((((sj4)*(x108)))+(new_r20));
evalcond[1]=((((IKReal(-1.00000000000000))*(x107)*(x113)))+(new_r21));
evalcond[2]=((((IKReal(-1.00000000000000))*(new_r00)*(x109)))+(((IKReal(-1.00000000000000))*(x115)))+(((cj3)*(new_r10))));
evalcond[3]=((((IKReal(-1.00000000000000))*(new_r01)*(x109)))+(((IKReal(-1.00000000000000))*(x114)))+(((cj3)*(new_r11))));
evalcond[4]=((x110)+(((cj4)*(x107)))+(((cj3)*(new_r01))));
evalcond[5]=((x111)+(((IKReal(-1.00000000000000))*(cj4)*(x114)))+(((cj3)*(new_r00))));
evalcond[6]=((x107)+(((new_r01)*(x112)))+(((IKReal(-1.00000000000000))*(new_r21)*(x113)))+(((cj4)*(x110))));
evalcond[7]=((((IKReal(-1.00000000000000))*(x114)))+(((IKReal(-1.00000000000000))*(new_r20)*(x113)))+(((new_r00)*(x112)))+(((cj4)*(x111))));
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(6);
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.vfree.resize(0);
}
}
}

}

}
}
}

}

}

} else
{
{
IKReal j5array[1], cj5array[1], sj5array[1];
bool j5valid[1]={false};
_nj5 = 1;
if( IKabs(((gconst5)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IKReal(-1.00000000000000))*(gconst5)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j5array[0]=IKatan2(((gconst5)*(new_r21)), ((IKReal(-1.00000000000000))*(gconst5)*(new_r20)));
sj5array[0]=IKsin(j5array[0]);
cj5array[0]=IKcos(j5array[0]);
if( j5array[0] > IKPI )
{
    j5array[0]-=IK2PI;
}
else if( j5array[0] < -IKPI )
{    j5array[0]+=IK2PI;
}
j5valid[0] = true;
for(int ij5 = 0; ij5 < 1; ++ij5)
{
if( !j5valid[ij5] )
{
    continue;
}
_ij5[0] = ij5; _ij5[1] = -1;
for(int iij5 = ij5+1; iij5 < 1; ++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 evalcond[2];
evalcond[0]=((((sj4)*(IKcos(j5))))+(new_r20));
evalcond[1]=((((IKReal(-1.00000000000000))*(sj4)*(IKsin(j5))))+(new_r21));
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 j3array[1], cj3array[1], sj3array[1];
bool j3valid[1]={false};
_nj3 = 1;
IKReal x116=((gconst7)*(sj4));
if( IKabs(((new_r12)*(x116))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x116))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j3array[0]=IKatan2(((new_r12)*(x116)), ((new_r02)*(x116)));
sj3array[0]=IKsin(j3array[0]);
cj3array[0]=IKcos(j3array[0]);
if( j3array[0] > IKPI )
{
    j3array[0]-=IK2PI;
}
else if( j3array[0] < -IKPI )
{    j3array[0]+=IK2PI;
}
j3valid[0] = true;
for(int ij3 = 0; ij3 < 1; ++ij3)
{
if( !j3valid[ij3] )
{
    continue;
}
_ij3[0] = ij3; _ij3[1] = -1;
for(int iij3 = ij3+1; iij3 < 1; ++iij3)
{
if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
{
    j3valid[iij3]=false; _ij3[1] = iij3; break; 
}
}
j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
{
IKReal evalcond[12];
IKReal x117=IKsin(j3);
IKReal x118=IKcos(j3);
IKReal x119=((IKReal(1.00000000000000))*(cj5));
IKReal x120=((IKReal(1.00000000000000))*(cj4));
IKReal x121=((IKReal(1.00000000000000))*(sj4));
IKReal x122=((cj4)*(x118));
IKReal x123=((cj4)*(x117));
IKReal x124=((new_r00)*(x118));
IKReal x125=((new_r11)*(x117));
IKReal x126=((new_r02)*(x118));
IKReal x127=((new_r01)*(x118));
IKReal x128=((new_r12)*(x117));
IKReal x129=((IKReal(1.00000000000000))*(x117));
IKReal x130=((new_r10)*(x117));
evalcond[0]=((((IKReal(-1.00000000000000))*(new_r02)*(x129)))+(((new_r12)*(x118))));
evalcond[1]=((((IKReal(-1.00000000000000))*(x121)))+(x126)+(x128));
evalcond[2]=((((new_r10)*(x118)))+(((IKReal(-1.00000000000000))*(sj5)))+(((IKReal(-1.00000000000000))*(new_r00)*(x129))));
evalcond[3]=((((new_r11)*(x118)))+(((IKReal(-1.00000000000000))*(new_r01)*(x129)))+(((IKReal(-1.00000000000000))*(x119))));
evalcond[4]=((x127)+(x125)+(((cj4)*(sj5))));
evalcond[5]=((x130)+(x124)+(((IKReal(-1.00000000000000))*(cj4)*(x119))));
evalcond[6]=((((new_r02)*(x122)))+(((IKReal(-1.00000000000000))*(new_r22)*(x121)))+(((new_r12)*(x123))));
evalcond[7]=((((IKReal(-1.00000000000000))*(new_r20)*(x120)))+(((IKReal(-1.00000000000000))*(x121)*(x124)))+(((IKReal(-1.00000000000000))*(x121)*(x130))));
evalcond[8]=((((IKReal(-1.00000000000000))*(new_r21)*(x120)))+(((IKReal(-1.00000000000000))*(x121)*(x127)))+(((IKReal(-1.00000000000000))*(x121)*(x125))));
evalcond[9]=((sj5)+(((new_r01)*(x122)))+(((IKReal(-1.00000000000000))*(new_r21)*(x121)))+(((new_r11)*(x123))));
evalcond[10]=((IKReal(1.00000000000000))+(((IKReal(-1.00000000000000))*(x121)*(x126)))+(((IKReal(-1.00000000000000))*(x121)*(x128)))+(((IKReal(-1.00000000000000))*(new_r22)*(x120))));
evalcond[11]=((((new_r00)*(x122)))+(((IKReal(-1.00000000000000))*(x119)))+(((IKReal(-1.00000000000000))*(new_r20)*(x121)))+(((new_r10)*(x123))));
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(6);
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.vfree.resize(0);
}
}
}

}

}
}
}

}

}
}
}
}
}};


/// solves the inverse kinematics equations.
/// \param pfree is an array specifying the free joints of the chain.
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 "560fcf56964ec92e4e01b8b6c12313af"; }

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

#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