/// 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 54 generated on 2012-03-27 13:04:03.830721 /// 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 -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; } /// 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=((x0)*(x1)*(x3)); x7=((x0)*(x2)*(x4)); x8=((x0)*(x1)*(x2)); x9=((x0)*(x3)*(x4)); x10=((x9)+(x8)); x11=IKcos(j[3]); x12=IKsin(j[4]); x13=((x7)+(((-1.00000000000000)*(x6)))); x14=((x11)*(x13)); x15=((x10)*(x5)); x16=((x15)+(x14)); x17=IKcos(j[4]); x18=IKsin(j[0]); x19=IKsin(j[5]); x20=((x16)*(x17)); x21=((x12)*(x18)); x22=((((-1.00000000000000)*(x21)))+(x20)); x23=IKcos(j[5]); x24=((x6)+(((-1.00000000000000)*(x7)))); x25=((x24)*(x5)); x26=((x10)*(x11)); x27=((x25)+(x26)); x28=((x1)*(x18)*(x2)); x29=((x18)*(x3)*(x4)); x30=((x28)+(x29)); x31=((x18)*(x2)*(x4)); x32=((x1)*(x18)*(x3)); x33=((x30)*(x5)); x34=((((-1.00000000000000)*(x32)))+(x31)); x35=((x11)*(x34)); x36=((x33)+(x35)); x37=((x0)*(x12)); x38=((x17)*(x36)); x39=((x38)+(x37)); x40=((((-1.00000000000000)*(x31)))+(x32)); x41=((x40)*(x5)); x42=((x11)*(x30)); x43=((x42)+(x41)); x44=((x1)*(x3)); x45=((x2)*(x4)); x46=((((-1.00000000000000)*(x45)))+(x44)); x47=((x1)*(x2)); x48=((x3)*(x4)); x49=((x48)+(x47)); x50=((x46)*(x5)); x51=((x11)*(x49)); x52=((x51)+(x50)); x53=((-1.00000000000000)*(x49)); eerot[0]=((((x22)*(x23)))+(((x19)*(x27)))); eerot[1]=((((-1.00000000000000)*(x17)*(x18)))+(((-1.00000000000000)*(x12)*(x16)))); eerot[2]=((((-1.00000000000000)*(x23)*(x27)))+(((x19)*(x22)))); eetrans[0]=((((-0.109150000000000)*(x18)))+(((x11)*(((((-0.0946500000000000)*(x9)))+(((-0.0946500000000000)*(x8)))))))+(((0.392250000000000)*(x6)))+(((0.425000000000000)*(x0)*(x1)))+(((x5)*(((((0.0946500000000000)*(x7)))+(((-0.0946500000000000)*(x6)))))))+(((-0.392250000000000)*(x7)))); eerot[3]=((((x19)*(x43)))+(((x23)*(x39)))); eerot[4]=((((x0)*(x17)))+(((-1.00000000000000)*(x12)*(x36)))); eerot[5]=((((x19)*(x39)))+(((-1.00000000000000)*(x23)*(x43)))); eetrans[1]=((((x5)*(((((-0.0946500000000000)*(x32)))+(((0.0946500000000000)*(x31)))))))+(((0.425000000000000)*(x1)*(x18)))+(((0.392250000000000)*(x32)))+(((0.109150000000000)*(x0)))+(((x11)*(((((-0.0946500000000000)*(x29)))+(((-0.0946500000000000)*(x28)))))))+(((-0.392250000000000)*(x31)))); eerot[6]=((((x17)*(x23)*(x52)))+(((x19)*(((((x11)*(x46)))+(((x5)*(x53)))))))); eerot[7]=((-1.00000000000000)*(x12)*(x52)); eerot[8]=((((x23)*(((((-1.00000000000000)*(x5)*(x53)))+(((-1.00000000000000)*(x11)*(x46)))))))+(((x17)*(x19)*(x52)))); eetrans[2]=((0.0891590000000000)+(((x11)*(((((-0.0946500000000000)*(x44)))+(((0.0946500000000000)*(x45)))))))+(((-0.425000000000000)*(x4)))+(((x5)*(((((0.0946500000000000)*(x47)))+(((0.0946500000000000)*(x48)))))))+(((-0.392250000000000)*(x48)))+(((-0.392250000000000)*(x47)))); } 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=((-1.00000000000000)*(r02)); new_r02=r01; new_px=px; new_r10=r10; new_r11=((-1.00000000000000)*(r12)); new_r12=r11; new_py=py; new_r20=r20; new_r21=((-1.00000000000000)*(r22)); new_r22=r21; new_pz=((-0.0891590000000000)+(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 op[8+1], zeror[8]; int numroots; IKReal gconst0; gconst0=((-1.00000000000000)*(r11)); IKReal gconst1; gconst1=((-2.00000000000000)*(r10)); IKReal gconst2; gconst2=r11; IKReal gconst3; gconst3=((-0.109150000000000)+(py)+(((0.0946500000000000)*(r11)))); IKReal gconst4; gconst4=((0.189300000000000)*(r10)); IKReal gconst5; gconst5=((-0.109150000000000)+(((-0.0946500000000000)*(r11)))+(py)); IKReal gconst6; gconst6=((-1.00000000000000)*(r11)); IKReal gconst7; gconst7=((-2.00000000000000)*(r10)); IKReal gconst8; gconst8=r11; IKReal gconst9; gconst9=((-0.109150000000000)+(py)+(((0.0946500000000000)*(r11)))); IKReal gconst10; gconst10=((0.189300000000000)*(r10)); IKReal gconst11; gconst11=((-0.109150000000000)+(((-0.0946500000000000)*(r11)))+(py)); IKReal gconst12; gconst12=((2.00000000000000)*(r01)); IKReal gconst13; gconst13=((4.00000000000000)*(r00)); IKReal gconst14; gconst14=((-2.00000000000000)*(r01)); IKReal gconst15; gconst15=((((-0.189300000000000)*(r01)))+(((-2.00000000000000)*(px)))); IKReal gconst16; gconst16=((-0.378600000000000)*(r00)); IKReal gconst17; gconst17=((((0.189300000000000)*(r01)))+(((-2.00000000000000)*(px)))); IKReal gconst18; gconst18=((2.00000000000000)*(r01)); IKReal gconst19; gconst19=((4.00000000000000)*(r00)); IKReal gconst20; gconst20=((-2.00000000000000)*(r01)); IKReal gconst21; gconst21=((((-0.189300000000000)*(r01)))+(((-2.00000000000000)*(px)))); IKReal gconst22; gconst22=((-0.378600000000000)*(r00)); IKReal gconst23; gconst23=((((0.189300000000000)*(r01)))+(((-2.00000000000000)*(px)))); IKReal gconst24; gconst24=r11; IKReal gconst25; gconst25=((2.00000000000000)*(r10)); IKReal gconst26; gconst26=((-1.00000000000000)*(r11)); IKReal gconst27; gconst27=((-0.109150000000000)+(((-0.0946500000000000)*(r11)))+(((-1.00000000000000)*(py)))); IKReal gconst28; gconst28=((-0.189300000000000)*(r10)); IKReal gconst29; gconst29=((-0.109150000000000)+(((-1.00000000000000)*(py)))+(((0.0946500000000000)*(r11)))); IKReal gconst30; gconst30=r11; IKReal gconst31; gconst31=((2.00000000000000)*(r10)); IKReal gconst32; gconst32=((-1.00000000000000)*(r11)); IKReal gconst33; gconst33=((-0.109150000000000)+(((-0.0946500000000000)*(r11)))+(((-1.00000000000000)*(py)))); IKReal gconst34; gconst34=((-0.189300000000000)*(r10)); IKReal gconst35; gconst35=((-0.109150000000000)+(((-1.00000000000000)*(py)))+(((0.0946500000000000)*(r11)))); op[0]=((((gconst24)*(gconst29)*(gconst32)*(gconst33)))+(((-1.00000000000000)*(gconst26)*(gconst27)*(gconst32)*(gconst33)))+(((gconst26)*(gconst28)*(gconst31)*(gconst33)))+(((-1.00000000000000)*(gconst24)*(gconst29)*(gconst30)*(gconst35)))+(((gconst26)*(gconst27)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst26)*(gconst28)*(gconst30)*(gconst34)))+(((gconst25)*(gconst29)*(gconst30)*(gconst34)))+(((-1.00000000000000)*(gconst25)*(gconst29)*(gconst31)*(gconst33)))); op[1]=((((gconst12)*(gconst29)*(gconst32)*(gconst33)))+(((-1.00000000000000)*(gconst16)*(gconst26)*(gconst30)*(gconst34)))+(((-1.00000000000000)*(gconst21)*(gconst26)*(gconst27)*(gconst32)))+(((gconst21)*(gconst26)*(gconst28)*(gconst31)))+(((-1.00000000000000)*(gconst18)*(gconst26)*(gconst28)*(gconst34)))+(((gconst23)*(gconst26)*(gconst27)*(gconst30)))+(((gconst15)*(gconst26)*(gconst30)*(gconst35)))+(((gconst13)*(gconst29)*(gconst30)*(gconst34)))+(((-1.00000000000000)*(gconst18)*(gconst24)*(gconst29)*(gconst35)))+(((gconst18)*(gconst26)*(gconst27)*(gconst35)))+(((-1.00000000000000)*(gconst13)*(gconst29)*(gconst31)*(gconst33)))+(((gconst17)*(gconst25)*(gconst30)*(gconst34)))+(((gconst16)*(gconst26)*(gconst31)*(gconst33)))+(((-1.00000000000000)*(gconst21)*(gconst25)*(gconst29)*(gconst31)))+(((-1.00000000000000)*(gconst17)*(gconst24)*(gconst30)*(gconst35)))+(((gconst22)*(gconst25)*(gconst29)*(gconst30)))+(((gconst14)*(gconst27)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst17)*(gconst25)*(gconst31)*(gconst33)))+(((gconst19)*(gconst26)*(gconst28)*(gconst33)))+(((-1.00000000000000)*(gconst22)*(gconst26)*(gconst28)*(gconst30)))+(((-1.00000000000000)*(gconst20)*(gconst26)*(gconst27)*(gconst33)))+(((gconst14)*(gconst28)*(gconst31)*(gconst33)))+(((-1.00000000000000)*(gconst15)*(gconst26)*(gconst32)*(gconst33)))+(((gconst17)*(gconst24)*(gconst32)*(gconst33)))+(((gconst20)*(gconst24)*(gconst29)*(gconst33)))+(((-1.00000000000000)*(gconst12)*(gconst29)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst23)*(gconst24)*(gconst29)*(gconst30)))+(((gconst18)*(gconst25)*(gconst29)*(gconst34)))+(((-1.00000000000000)*(gconst14)*(gconst27)*(gconst32)*(gconst33)))+(((gconst21)*(gconst24)*(gconst29)*(gconst32)))+(((-1.00000000000000)*(gconst19)*(gconst25)*(gconst29)*(gconst33)))+(((-1.00000000000000)*(gconst14)*(gconst28)*(gconst30)*(gconst34)))); op[2]=((((-1.00000000000000)*(gconst20)*(gconst21)*(gconst26)*(gconst27)))+(((gconst14)*(gconst19)*(gconst28)*(gconst33)))+(((gconst13)*(gconst22)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst14)*(gconst22)*(gconst28)*(gconst30)))+(((-1.00000000000000)*(gconst16)*(gconst18)*(gconst26)*(gconst34)))+(((-1.00000000000000)*(gconst24)*(gconst29)*(gconst35)*(gconst6)))+(((gconst24)*(gconst29)*(gconst33)*(gconst8)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst31)*(gconst33)))+(((gconst16)*(gconst19)*(gconst26)*(gconst33)))+(((-1.00000000000000)*(gconst0)*(gconst29)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst16)*(gconst22)*(gconst26)*(gconst30)))+(((gconst24)*(gconst32)*(gconst33)*(gconst5)))+(((gconst26)*(gconst3)*(gconst30)*(gconst35)))+(((gconst12)*(gconst17)*(gconst32)*(gconst33)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst26)*(gconst33)))+(((gconst26)*(gconst28)*(gconst31)*(gconst9)))+(((-1.00000000000000)*(gconst17)*(gconst19)*(gconst25)*(gconst33)))+(((-1.00000000000000)*(gconst10)*(gconst26)*(gconst28)*(gconst30)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst28)*(gconst34)))+(((-1.00000000000000)*(gconst24)*(gconst30)*(gconst35)*(gconst5)))+(((gconst26)*(gconst28)*(gconst33)*(gconst7)))+(((gconst15)*(gconst18)*(gconst26)*(gconst35)))+(((-1.00000000000000)*(gconst12)*(gconst17)*(gconst30)*(gconst35)))+(((gconst12)*(gconst21)*(gconst29)*(gconst32)))+(((gconst13)*(gconst18)*(gconst29)*(gconst34)))+(((-1.00000000000000)*(gconst26)*(gconst30)*(gconst34)*(gconst4)))+(((-1.00000000000000)*(gconst14)*(gconst20)*(gconst27)*(gconst33)))+(((-1.00000000000000)*(gconst2)*(gconst28)*(gconst30)*(gconst34)))+(((-1.00000000000000)*(gconst26)*(gconst28)*(gconst34)*(gconst6)))+(((-1.00000000000000)*(gconst25)*(gconst29)*(gconst31)*(gconst9)))+(((gconst16)*(gconst21)*(gconst26)*(gconst31)))+(((gconst17)*(gconst18)*(gconst25)*(gconst34)))+(((-1.00000000000000)*(gconst14)*(gconst21)*(gconst27)*(gconst32)))+(((gconst24)*(gconst29)*(gconst32)*(gconst9)))+(((gconst12)*(gconst20)*(gconst29)*(gconst33)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst32)*(gconst33)))+(((gconst14)*(gconst21)*(gconst28)*(gconst31)))+(((-1.00000000000000)*(gconst11)*(gconst24)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst26)*(gconst27)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst26)*(gconst27)*(gconst33)*(gconst8)))+(((gconst17)*(gconst22)*(gconst25)*(gconst30)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst29)*(gconst33)))+(((gconst14)*(gconst16)*(gconst31)*(gconst33)))+(((gconst18)*(gconst23)*(gconst26)*(gconst27)))+(((-1.00000000000000)*(gconst18)*(gconst23)*(gconst24)*(gconst29)))+(((gconst14)*(gconst23)*(gconst27)*(gconst30)))+(((-1.00000000000000)*(gconst17)*(gconst18)*(gconst24)*(gconst35)))+(((gconst14)*(gconst18)*(gconst27)*(gconst35)))+(((-1.00000000000000)*(gconst17)*(gconst21)*(gconst25)*(gconst31)))+(((gconst1)*(gconst29)*(gconst30)*(gconst34)))+(((gconst2)*(gconst28)*(gconst31)*(gconst33)))+(((gconst13)*(gconst17)*(gconst30)*(gconst34)))+(((gconst2)*(gconst27)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst13)*(gconst21)*(gconst29)*(gconst31)))+(((gconst20)*(gconst21)*(gconst24)*(gconst29)))+(((gconst18)*(gconst22)*(gconst25)*(gconst29)))+(((gconst25)*(gconst30)*(gconst34)*(gconst5)))+(((gconst19)*(gconst21)*(gconst26)*(gconst28)))+(((gconst25)*(gconst29)*(gconst34)*(gconst6)))+(((-1.00000000000000)*(gconst12)*(gconst18)*(gconst29)*(gconst35)))+(((-1.00000000000000)*(gconst14)*(gconst16)*(gconst30)*(gconst34)))+(((gconst10)*(gconst25)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst12)*(gconst23)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst26)*(gconst3)*(gconst32)*(gconst33)))+(((gconst17)*(gconst20)*(gconst24)*(gconst33)))+(((-1.00000000000000)*(gconst2)*(gconst27)*(gconst32)*(gconst33)))+(((gconst26)*(gconst31)*(gconst33)*(gconst4)))+(((-1.00000000000000)*(gconst15)*(gconst21)*(gconst26)*(gconst32)))+(((gconst26)*(gconst27)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst26)*(gconst28)))+(((gconst15)*(gconst23)*(gconst26)*(gconst30)))+(((-1.00000000000000)*(gconst19)*(gconst21)*(gconst25)*(gconst29)))+(((-1.00000000000000)*(gconst25)*(gconst29)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst25)*(gconst31)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst17)*(gconst23)*(gconst24)*(gconst30)))+(((gconst17)*(gconst21)*(gconst24)*(gconst32)))+(((-1.00000000000000)*(gconst1)*(gconst29)*(gconst31)*(gconst33)))+(((gconst14)*(gconst15)*(gconst30)*(gconst35)))+(((gconst0)*(gconst29)*(gconst32)*(gconst33)))+(((gconst11)*(gconst26)*(gconst27)*(gconst30)))); op[3]=((((gconst12)*(gconst17)*(gconst21)*(gconst32)))+(((gconst12)*(gconst20)*(gconst21)*(gconst29)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst21)*(gconst29)))+(((-1.00000000000000)*(gconst18)*(gconst26)*(gconst34)*(gconst4)))+(((gconst12)*(gconst29)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst29)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst12)*(gconst18)*(gconst23)*(gconst29)))+(((gconst16)*(gconst26)*(gconst33)*(gconst7)))+(((gconst15)*(gconst18)*(gconst23)*(gconst26)))+(((gconst14)*(gconst28)*(gconst33)*(gconst7)))+(((gconst20)*(gconst24)*(gconst29)*(gconst9)))+(((gconst14)*(gconst15)*(gconst23)*(gconst30)))+(((gconst13)*(gconst30)*(gconst34)*(gconst5)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst21)*(gconst32)))+(((gconst17)*(gconst24)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst18)*(gconst24)*(gconst35)*(gconst5)))+(((-1.00000000000000)*(gconst14)*(gconst16)*(gconst18)*(gconst34)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst20)*(gconst33)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst28)*(gconst30)))+(((gconst14)*(gconst16)*(gconst21)*(gconst31)))+(((gconst2)*(gconst21)*(gconst28)*(gconst31)))+(((gconst15)*(gconst26)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst17)*(gconst24)*(gconst30)))+(((-1.00000000000000)*(gconst12)*(gconst17)*(gconst18)*(gconst35)))+(((gconst18)*(gconst25)*(gconst34)*(gconst5)))+(((-1.00000000000000)*(gconst12)*(gconst17)*(gconst23)*(gconst30)))+(((-1.00000000000000)*(gconst17)*(gconst25)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst17)*(gconst18)*(gconst23)*(gconst24)))+(((-1.00000000000000)*(gconst23)*(gconst24)*(gconst30)*(gconst5)))+(((gconst10)*(gconst18)*(gconst25)*(gconst29)))+(((gconst14)*(gconst19)*(gconst21)*(gconst28)))+(((gconst11)*(gconst14)*(gconst27)*(gconst30)))+(((-1.00000000000000)*(gconst0)*(gconst23)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst19)*(gconst25)*(gconst29)*(gconst9)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst21)*(gconst26)))+(((gconst19)*(gconst26)*(gconst33)*(gconst4)))+(((gconst16)*(gconst19)*(gconst21)*(gconst26)))+(((-1.00000000000000)*(gconst13)*(gconst29)*(gconst31)*(gconst9)))+(((gconst17)*(gconst18)*(gconst22)*(gconst25)))+(((gconst20)*(gconst24)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst21)*(gconst26)*(gconst27)*(gconst8)))+(((-1.00000000000000)*(gconst14)*(gconst20)*(gconst21)*(gconst27)))+(((gconst12)*(gconst17)*(gconst20)*(gconst33)))+(((-1.00000000000000)*(gconst21)*(gconst25)*(gconst31)*(gconst5)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst30)*(gconst34)))+(((gconst22)*(gconst25)*(gconst30)*(gconst5)))+(((gconst16)*(gconst2)*(gconst31)*(gconst33)))+(((-1.00000000000000)*(gconst0)*(gconst18)*(gconst29)*(gconst35)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst21)*(gconst31)))+(((-1.00000000000000)*(gconst12)*(gconst30)*(gconst35)*(gconst5)))+(((-1.00000000000000)*(gconst14)*(gconst28)*(gconst34)*(gconst6)))+(((gconst14)*(gconst27)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst17)*(gconst24)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst29)*(gconst30)))+(((gconst12)*(gconst29)*(gconst33)*(gconst8)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst19)*(gconst33)))+(((-1.00000000000000)*(gconst23)*(gconst24)*(gconst29)*(gconst6)))+(((gconst17)*(gconst25)*(gconst34)*(gconst6)))+(((gconst11)*(gconst15)*(gconst26)*(gconst30)))+(((gconst18)*(gconst26)*(gconst3)*(gconst35)))+(((-1.00000000000000)*(gconst10)*(gconst18)*(gconst26)*(gconst28)))+(((gconst21)*(gconst26)*(gconst28)*(gconst7)))+(((-1.00000000000000)*(gconst22)*(gconst26)*(gconst28)*(gconst6)))+(((gconst21)*(gconst24)*(gconst32)*(gconst5)))+(((gconst10)*(gconst13)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst17)*(gconst25)*(gconst31)*(gconst9)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst32)*(gconst33)))+(((-1.00000000000000)*(gconst14)*(gconst3)*(gconst32)*(gconst33)))+(((gconst19)*(gconst2)*(gconst28)*(gconst33)))+(((-1.00000000000000)*(gconst11)*(gconst18)*(gconst24)*(gconst29)))+(((gconst22)*(gconst25)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst10)*(gconst16)*(gconst26)*(gconst30)))+(((-1.00000000000000)*(gconst15)*(gconst26)*(gconst32)*(gconst9)))+(((gconst1)*(gconst17)*(gconst30)*(gconst34)))+(((-1.00000000000000)*(gconst16)*(gconst18)*(gconst22)*(gconst26)))+(((gconst18)*(gconst2)*(gconst27)*(gconst35)))+(((-1.00000000000000)*(gconst15)*(gconst26)*(gconst33)*(gconst8)))+(((gconst23)*(gconst26)*(gconst3)*(gconst30)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst27)*(gconst32)))+(((gconst0)*(gconst17)*(gconst32)*(gconst33)))+(((gconst0)*(gconst20)*(gconst29)*(gconst33)))+(((gconst12)*(gconst32)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst21)*(gconst26)*(gconst3)*(gconst32)))+(((gconst2)*(gconst23)*(gconst27)*(gconst30)))+(((-1.00000000000000)*(gconst19)*(gconst25)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst1)*(gconst19)*(gconst29)*(gconst33)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst31)*(gconst33)))+(((-1.00000000000000)*(gconst21)*(gconst25)*(gconst29)*(gconst7)))+(((gconst14)*(gconst28)*(gconst31)*(gconst9)))+(((gconst14)*(gconst3)*(gconst30)*(gconst35)))+(((gconst1)*(gconst18)*(gconst29)*(gconst34)))+(((-1.00000000000000)*(gconst13)*(gconst31)*(gconst33)*(gconst5)))+(((gconst14)*(gconst18)*(gconst23)*(gconst27)))+(((gconst14)*(gconst15)*(gconst18)*(gconst35)))+(((-1.00000000000000)*(gconst20)*(gconst26)*(gconst27)*(gconst9)))+(((gconst14)*(gconst16)*(gconst19)*(gconst33)))+(((gconst17)*(gconst24)*(gconst33)*(gconst8)))+(((gconst13)*(gconst29)*(gconst34)*(gconst6)))+(((-1.00000000000000)*(gconst14)*(gconst30)*(gconst34)*(gconst4)))+(((gconst0)*(gconst21)*(gconst29)*(gconst32)))+(((-1.00000000000000)*(gconst17)*(gconst19)*(gconst21)*(gconst25)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst22)*(gconst28)))+(((gconst10)*(gconst17)*(gconst25)*(gconst30)))+(((-1.00000000000000)*(gconst14)*(gconst16)*(gconst22)*(gconst30)))+(((-1.00000000000000)*(gconst16)*(gconst26)*(gconst34)*(gconst6)))+(((gconst14)*(gconst31)*(gconst33)*(gconst4)))+(((gconst13)*(gconst18)*(gconst22)*(gconst29)))+(((-1.00000000000000)*(gconst14)*(gconst27)*(gconst33)*(gconst8)))+(((gconst17)*(gconst20)*(gconst21)*(gconst24)))+(((-1.00000000000000)*(gconst22)*(gconst26)*(gconst30)*(gconst4)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst30)*(gconst35)))+(((gconst23)*(gconst26)*(gconst27)*(gconst6)))+(((-1.00000000000000)*(gconst12)*(gconst29)*(gconst35)*(gconst6)))+(((gconst19)*(gconst26)*(gconst28)*(gconst9)))+(((gconst21)*(gconst26)*(gconst31)*(gconst4)))+(((gconst13)*(gconst17)*(gconst18)*(gconst34)))+(((gconst15)*(gconst2)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst20)*(gconst26)*(gconst3)*(gconst33)))+(((gconst16)*(gconst26)*(gconst31)*(gconst9)))+(((gconst21)*(gconst24)*(gconst29)*(gconst8)))+(((-1.00000000000000)*(gconst18)*(gconst2)*(gconst28)*(gconst34)))+(((gconst11)*(gconst18)*(gconst26)*(gconst27)))+(((-1.00000000000000)*(gconst14)*(gconst27)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst21)*(gconst29)*(gconst31)))+(((-1.00000000000000)*(gconst2)*(gconst20)*(gconst27)*(gconst33)))+(((gconst13)*(gconst17)*(gconst22)*(gconst30)))+(((gconst1)*(gconst22)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst2)*(gconst22)*(gconst28)*(gconst30)))); op[4]=((((gconst14)*(gconst15)*(gconst35)*(gconst6)))+(((gconst12)*(gconst20)*(gconst29)*(gconst9)))+(((gconst12)*(gconst21)*(gconst32)*(gconst5)))+(((-1.00000000000000)*(gconst2)*(gconst20)*(gconst21)*(gconst27)))+(((gconst10)*(gconst13)*(gconst18)*(gconst29)))+(((gconst0)*(gconst20)*(gconst21)*(gconst29)))+(((gconst14)*(gconst23)*(gconst27)*(gconst6)))+(((gconst14)*(gconst16)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst1)*(gconst29)*(gconst31)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst23)*(gconst30)))+(((-1.00000000000000)*(gconst12)*(gconst18)*(gconst35)*(gconst5)))+(((gconst10)*(gconst13)*(gconst17)*(gconst30)))+(((gconst15)*(gconst23)*(gconst26)*(gconst6)))+(((gconst13)*(gconst17)*(gconst18)*(gconst22)))+(((gconst26)*(gconst28)*(gconst7)*(gconst9)))+(((gconst1)*(gconst29)*(gconst34)*(gconst6)))+(((-1.00000000000000)*(gconst13)*(gconst21)*(gconst31)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst2)*(gconst28)*(gconst30)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst17)*(gconst30)))+(((gconst2)*(gconst28)*(gconst31)*(gconst9)))+(((gconst0)*(gconst17)*(gconst20)*(gconst33)))+(((-1.00000000000000)*(gconst14)*(gconst22)*(gconst28)*(gconst6)))+(((-1.00000000000000)*(gconst1)*(gconst29)*(gconst33)*(gconst7)))+(((gconst12)*(gconst21)*(gconst29)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst30)*(gconst35)*(gconst5)))+(((gconst16)*(gconst2)*(gconst21)*(gconst31)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst21)*(gconst32)))+(((-1.00000000000000)*(gconst24)*(gconst35)*(gconst5)*(gconst6)))+(((gconst25)*(gconst34)*(gconst5)*(gconst6)))+(((gconst0)*(gconst17)*(gconst21)*(gconst32)))+(((gconst12)*(gconst17)*(gconst33)*(gconst8)))+(((gconst1)*(gconst17)*(gconst18)*(gconst34)))+(((-1.00000000000000)*(gconst18)*(gconst23)*(gconst24)*(gconst5)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst22)*(gconst30)))+(((-1.00000000000000)*(gconst1)*(gconst31)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst17)*(gconst19)*(gconst25)*(gconst9)))+(((gconst11)*(gconst2)*(gconst27)*(gconst30)))+(((gconst13)*(gconst17)*(gconst34)*(gconst6)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst16)*(gconst30)))+(((gconst0)*(gconst29)*(gconst33)*(gconst8)))+(((-1.00000000000000)*(gconst14)*(gconst22)*(gconst30)*(gconst4)))+(((-1.00000000000000)*(gconst26)*(gconst3)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst1)*(gconst19)*(gconst21)*(gconst29)))+(((-1.00000000000000)*(gconst14)*(gconst20)*(gconst3)*(gconst33)))+(((-1.00000000000000)*(gconst15)*(gconst21)*(gconst26)*(gconst8)))+(((gconst14)*(gconst16)*(gconst31)*(gconst9)))+(((gconst2)*(gconst27)*(gconst35)*(gconst6)))+(((gconst18)*(gconst23)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst26)*(gconst9)))+(((gconst14)*(gconst21)*(gconst31)*(gconst4)))+(((-1.00000000000000)*(gconst0)*(gconst29)*(gconst35)*(gconst6)))+(((gconst2)*(gconst3)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst19)*(gconst21)*(gconst25)*(gconst5)))+(((gconst19)*(gconst2)*(gconst21)*(gconst28)))+(((-1.00000000000000)*(gconst10)*(gconst26)*(gconst30)*(gconst4)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst18)*(gconst28)))+(((gconst19)*(gconst21)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst26)*(gconst34)*(gconst4)*(gconst6)))+(((-1.00000000000000)*(gconst12)*(gconst23)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst12)*(gconst17)*(gconst18)*(gconst23)))+(((gconst10)*(gconst17)*(gconst18)*(gconst25)))+(((gconst26)*(gconst3)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst27)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst19)*(gconst33)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst20)*(gconst21)))+(((gconst0)*(gconst32)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst17)*(gconst21)*(gconst25)*(gconst7)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst21)*(gconst29)*(gconst7)))+(((gconst11)*(gconst14)*(gconst18)*(gconst27)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst20)*(gconst33)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst14)*(gconst21)*(gconst3)*(gconst32)))+(((gconst11)*(gconst14)*(gconst15)*(gconst30)))+(((-1.00000000000000)*(gconst26)*(gconst27)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst25)*(gconst31)*(gconst5)*(gconst9)))+(((gconst12)*(gconst17)*(gconst20)*(gconst21)))+(((gconst12)*(gconst20)*(gconst33)*(gconst5)))+(((gconst18)*(gconst22)*(gconst25)*(gconst5)))+(((gconst2)*(gconst28)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst19)*(gconst21)))+(((gconst26)*(gconst33)*(gconst4)*(gconst7)))+(((gconst16)*(gconst21)*(gconst26)*(gconst7)))+(((gconst16)*(gconst19)*(gconst26)*(gconst9)))+(((-1.00000000000000)*(gconst26)*(gconst3)*(gconst33)*(gconst8)))+(((-1.00000000000000)*(gconst16)*(gconst22)*(gconst26)*(gconst6)))+(((gconst17)*(gconst21)*(gconst24)*(gconst8)))+(((gconst14)*(gconst18)*(gconst3)*(gconst35)))+(((-1.00000000000000)*(gconst2)*(gconst27)*(gconst33)*(gconst8)))+(((gconst18)*(gconst2)*(gconst23)*(gconst27)))+(((gconst26)*(gconst31)*(gconst4)*(gconst9)))+(((gconst24)*(gconst32)*(gconst5)*(gconst9)))+(((gconst10)*(gconst25)*(gconst30)*(gconst5)))+(((-1.00000000000000)*(gconst20)*(gconst21)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst14)*(gconst20)*(gconst27)*(gconst9)))+(((-1.00000000000000)*(gconst12)*(gconst17)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst17)*(gconst23)*(gconst24)*(gconst6)))+(((gconst13)*(gconst22)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst28)*(gconst34)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst30)*(gconst34)*(gconst4)))+(((gconst1)*(gconst30)*(gconst34)*(gconst5)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst31)*(gconst9)))+(((gconst14)*(gconst15)*(gconst18)*(gconst23)))+(((-1.00000000000000)*(gconst12)*(gconst23)*(gconst30)*(gconst5)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst21)*(gconst31)))+(((gconst24)*(gconst33)*(gconst5)*(gconst8)))+(((gconst15)*(gconst2)*(gconst23)*(gconst30)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst18)*(gconst35)))+(((-1.00000000000000)*(gconst10)*(gconst26)*(gconst28)*(gconst6)))+(((gconst14)*(gconst19)*(gconst28)*(gconst9)))+(((gconst10)*(gconst25)*(gconst29)*(gconst6)))+(((gconst15)*(gconst18)*(gconst2)*(gconst35)))+(((gconst17)*(gconst22)*(gconst25)*(gconst6)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst29)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst24)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst24)*(gconst30)*(gconst5)))+(((-1.00000000000000)*(gconst16)*(gconst18)*(gconst2)*(gconst34)))+(((-1.00000000000000)*(gconst14)*(gconst21)*(gconst27)*(gconst8)))+(((gconst14)*(gconst23)*(gconst3)*(gconst30)))+(((gconst24)*(gconst29)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst18)*(gconst29)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst18)*(gconst2)*(gconst22)*(gconst28)))+(((-1.00000000000000)*(gconst25)*(gconst33)*(gconst5)*(gconst7)))+(((-1.00000000000000)*(gconst14)*(gconst16)*(gconst34)*(gconst6)))+(((gconst2)*(gconst31)*(gconst33)*(gconst4)))+(((gconst1)*(gconst18)*(gconst22)*(gconst29)))+(((gconst17)*(gconst20)*(gconst24)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst17)*(gconst18)*(gconst24)))+(((-1.00000000000000)*(gconst25)*(gconst29)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst14)*(gconst16)*(gconst18)*(gconst22)))+(((-1.00000000000000)*(gconst0)*(gconst18)*(gconst23)*(gconst29)))+(((gconst14)*(gconst19)*(gconst33)*(gconst4)))+(((gconst20)*(gconst21)*(gconst24)*(gconst5)))+(((gconst11)*(gconst26)*(gconst27)*(gconst6)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst33)*(gconst8)))+(((gconst11)*(gconst15)*(gconst18)*(gconst26)))+(((gconst1)*(gconst17)*(gconst22)*(gconst30)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst34)*(gconst4)))+(((-1.00000000000000)*(gconst2)*(gconst3)*(gconst32)*(gconst33)))+(((gconst13)*(gconst18)*(gconst34)*(gconst5)))+(((gconst1)*(gconst10)*(gconst29)*(gconst30)))+(((gconst16)*(gconst19)*(gconst2)*(gconst33)))+(((-1.00000000000000)*(gconst10)*(gconst16)*(gconst18)*(gconst26)))+(((gconst14)*(gconst16)*(gconst19)*(gconst21)))+(((gconst12)*(gconst17)*(gconst32)*(gconst9)))+(((gconst0)*(gconst29)*(gconst32)*(gconst9)))+(((gconst13)*(gconst22)*(gconst30)*(gconst5)))+(((gconst11)*(gconst26)*(gconst3)*(gconst30)))+(((gconst14)*(gconst21)*(gconst28)*(gconst7)))); op[5]=((((gconst13)*(gconst18)*(gconst22)*(gconst5)))+(((gconst14)*(gconst3)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst19)*(gconst25)*(gconst5)*(gconst9)))+(((gconst2)*(gconst23)*(gconst3)*(gconst30)))+(((-1.00000000000000)*(gconst10)*(gconst16)*(gconst26)*(gconst6)))+(((gconst12)*(gconst20)*(gconst21)*(gconst5)))+(((gconst16)*(gconst19)*(gconst2)*(gconst21)))+(((gconst14)*(gconst15)*(gconst23)*(gconst6)))+(((gconst1)*(gconst17)*(gconst18)*(gconst22)))+(((gconst2)*(gconst23)*(gconst27)*(gconst6)))+(((-1.00000000000000)*(gconst12)*(gconst17)*(gconst23)*(gconst6)))+(((gconst0)*(gconst21)*(gconst32)*(gconst5)))+(((-1.00000000000000)*(gconst13)*(gconst33)*(gconst5)*(gconst7)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst21)*(gconst7)))+(((-1.00000000000000)*(gconst14)*(gconst3)*(gconst33)*(gconst8)))+(((gconst14)*(gconst19)*(gconst21)*(gconst4)))+(((gconst21)*(gconst26)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst17)*(gconst24)*(gconst6)))+(((-1.00000000000000)*(gconst15)*(gconst26)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst34)*(gconst6)))+(((gconst10)*(gconst17)*(gconst25)*(gconst6)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst19)*(gconst21)))+(((gconst2)*(gconst21)*(gconst28)*(gconst7)))+(((gconst14)*(gconst16)*(gconst21)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst30)*(gconst5)))+(((gconst14)*(gconst28)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst27)*(gconst8)))+(((-1.00000000000000)*(gconst22)*(gconst26)*(gconst4)*(gconst6)))+(((gconst16)*(gconst2)*(gconst33)*(gconst7)))+(((gconst11)*(gconst14)*(gconst27)*(gconst6)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst21)*(gconst8)))+(((gconst1)*(gconst18)*(gconst34)*(gconst5)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst20)*(gconst9)))+(((-1.00000000000000)*(gconst14)*(gconst27)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst29)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst18)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst14)*(gconst34)*(gconst4)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst17)*(gconst18)))+(((gconst17)*(gconst24)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst20)*(gconst21)))+(((-1.00000000000000)*(gconst13)*(gconst31)*(gconst5)*(gconst9)))+(((gconst13)*(gconst17)*(gconst22)*(gconst6)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst32)*(gconst9)))+(((gconst10)*(gconst13)*(gconst29)*(gconst6)))+(((gconst1)*(gconst22)*(gconst29)*(gconst6)))+(((gconst11)*(gconst14)*(gconst15)*(gconst18)))+(((gconst19)*(gconst2)*(gconst33)*(gconst4)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst16)*(gconst18)))+(((gconst0)*(gconst21)*(gconst29)*(gconst8)))+(((gconst15)*(gconst2)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst20)*(gconst27)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst19)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst14)*(gconst20)*(gconst21)*(gconst3)))+(((gconst16)*(gconst2)*(gconst31)*(gconst9)))+(((gconst10)*(gconst13)*(gconst30)*(gconst5)))+(((gconst0)*(gconst20)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst33)*(gconst8)))+(((-1.00000000000000)*(gconst1)*(gconst19)*(gconst29)*(gconst9)))+(((gconst19)*(gconst26)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst18)*(gconst29)))+(((gconst11)*(gconst14)*(gconst3)*(gconst30)))+(((gconst22)*(gconst25)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst18)*(gconst23)))+(((-1.00000000000000)*(gconst12)*(gconst18)*(gconst23)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst16)*(gconst2)*(gconst30)))+(((gconst2)*(gconst21)*(gconst31)*(gconst4)))+(((-1.00000000000000)*(gconst2)*(gconst20)*(gconst3)*(gconst33)))+(((gconst0)*(gconst17)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst18)*(gconst35)*(gconst5)))+(((-1.00000000000000)*(gconst21)*(gconst26)*(gconst3)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst17)*(gconst30)))+(((gconst13)*(gconst34)*(gconst5)*(gconst6)))+(((gconst16)*(gconst26)*(gconst7)*(gconst9)))+(((gconst14)*(gconst31)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst19)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst21)*(gconst29)*(gconst7)))+(((gconst10)*(gconst18)*(gconst25)*(gconst5)))+(((-1.00000000000000)*(gconst12)*(gconst35)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst31)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst18)*(gconst2)*(gconst28)))+(((gconst12)*(gconst17)*(gconst21)*(gconst8)))+(((gconst11)*(gconst15)*(gconst2)*(gconst30)))+(((-1.00000000000000)*(gconst14)*(gconst3)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst18)*(gconst2)*(gconst34)*(gconst4)))+(((gconst11)*(gconst18)*(gconst2)*(gconst27)))+(((gconst0)*(gconst20)*(gconst29)*(gconst9)))+(((gconst0)*(gconst17)*(gconst33)*(gconst8)))+(((-1.00000000000000)*(gconst17)*(gconst25)*(gconst7)*(gconst9)))+(((gconst20)*(gconst24)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst20)*(gconst26)*(gconst3)*(gconst9)))+(((gconst23)*(gconst26)*(gconst3)*(gconst6)))+(((gconst12)*(gconst33)*(gconst5)*(gconst8)))+(((gconst12)*(gconst32)*(gconst5)*(gconst9)))+(((gconst11)*(gconst18)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst21)*(gconst25)*(gconst5)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst22)*(gconst30)*(gconst4)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst22)*(gconst4)))+(((-1.00000000000000)*(gconst0)*(gconst23)*(gconst30)*(gconst5)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst21)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst28)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst22)*(gconst28)*(gconst6)))+(((gconst1)*(gconst22)*(gconst30)*(gconst5)))+(((gconst15)*(gconst18)*(gconst2)*(gconst23)))+(((gconst14)*(gconst16)*(gconst19)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst18)*(gconst24)*(gconst5)))+(((gconst1)*(gconst17)*(gconst34)*(gconst6)))+(((gconst10)*(gconst13)*(gconst17)*(gconst18)))+(((-1.00000000000000)*(gconst23)*(gconst24)*(gconst5)*(gconst6)))+(((gconst0)*(gconst17)*(gconst20)*(gconst21)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst3)*(gconst32)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst30)*(gconst4)))+(((gconst12)*(gconst17)*(gconst20)*(gconst9)))+(((gconst12)*(gconst29)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst14)*(gconst16)*(gconst22)*(gconst6)))+(((-1.00000000000000)*(gconst16)*(gconst18)*(gconst2)*(gconst22)))+(((gconst19)*(gconst2)*(gconst28)*(gconst9)))+(((gconst18)*(gconst2)*(gconst3)*(gconst35)))+(((gconst14)*(gconst33)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst0)*(gconst23)*(gconst29)*(gconst6)))+(((gconst11)*(gconst15)*(gconst26)*(gconst6)))+(((gconst1)*(gconst10)*(gconst17)*(gconst30)))+(((-1.00000000000000)*(gconst1)*(gconst21)*(gconst31)*(gconst5)))+(((gconst21)*(gconst24)*(gconst5)*(gconst8)))+(((gconst1)*(gconst10)*(gconst18)*(gconst29)))+(((gconst14)*(gconst18)*(gconst23)*(gconst3)))); op[6]=((((gconst14)*(gconst21)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst21)*(gconst5)*(gconst7)))+(((gconst0)*(gconst33)*(gconst5)*(gconst8)))+(((gconst1)*(gconst10)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst3)*(gconst33)*(gconst8)))+(((gconst14)*(gconst23)*(gconst3)*(gconst6)))+(((gconst1)*(gconst10)*(gconst17)*(gconst18)))+(((gconst13)*(gconst22)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst12)*(gconst23)*(gconst5)*(gconst6)))+(((gconst12)*(gconst20)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst23)*(gconst6)))+(((-1.00000000000000)*(gconst14)*(gconst21)*(gconst3)*(gconst8)))+(((-1.00000000000000)*(gconst2)*(gconst34)*(gconst4)*(gconst6)))+(((-1.00000000000000)*(gconst1)*(gconst33)*(gconst5)*(gconst7)))+(((-1.00000000000000)*(gconst0)*(gconst35)*(gconst5)*(gconst6)))+(((gconst24)*(gconst5)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst19)*(gconst21)*(gconst5)))+(((gconst2)*(gconst3)*(gconst35)*(gconst6)))+(((gconst0)*(gconst20)*(gconst21)*(gconst5)))+(((-1.00000000000000)*(gconst0)*(gconst18)*(gconst23)*(gconst5)))+(((gconst15)*(gconst2)*(gconst23)*(gconst6)))+(((gconst1)*(gconst18)*(gconst22)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst20)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst19)*(gconst9)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst2)*(gconst30)*(gconst4)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst21)*(gconst7)))+(((gconst10)*(gconst25)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst17)*(gconst6)))+(((gconst14)*(gconst16)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst27)*(gconst8)*(gconst9)))+(((gconst11)*(gconst2)*(gconst27)*(gconst6)))+(((gconst0)*(gconst32)*(gconst5)*(gconst9)))+(((gconst19)*(gconst2)*(gconst21)*(gconst4)))+(((gconst11)*(gconst26)*(gconst3)*(gconst6)))+(((gconst12)*(gconst21)*(gconst5)*(gconst8)))+(((gconst11)*(gconst15)*(gconst18)*(gconst2)))+(((gconst1)*(gconst17)*(gconst22)*(gconst6)))+(((gconst10)*(gconst13)*(gconst17)*(gconst6)))+(((-1.00000000000000)*(gconst10)*(gconst26)*(gconst4)*(gconst6)))+(((gconst16)*(gconst19)*(gconst2)*(gconst9)))+(((gconst18)*(gconst2)*(gconst23)*(gconst3)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst18)*(gconst4)))+(((-1.00000000000000)*(gconst26)*(gconst3)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst3)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst20)*(gconst21)*(gconst3)))+(((gconst16)*(gconst2)*(gconst21)*(gconst7)))+(((-1.00000000000000)*(gconst1)*(gconst31)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst29)*(gconst7)*(gconst9)))+(((gconst1)*(gconst34)*(gconst5)*(gconst6)))+(((gconst10)*(gconst13)*(gconst18)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst16)*(gconst6)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst17)*(gconst18)))+(((-1.00000000000000)*(gconst14)*(gconst20)*(gconst3)*(gconst9)))+(((gconst26)*(gconst4)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst18)*(gconst2)*(gconst22)*(gconst4)))+(((gconst1)*(gconst10)*(gconst30)*(gconst5)))+(((gconst2)*(gconst28)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst24)*(gconst5)*(gconst6)))+(((gconst11)*(gconst14)*(gconst15)*(gconst6)))+(((gconst12)*(gconst17)*(gconst8)*(gconst9)))+(((gconst11)*(gconst2)*(gconst3)*(gconst30)))+(((-1.00000000000000)*(gconst10)*(gconst16)*(gconst18)*(gconst2)))+(((-1.00000000000000)*(gconst25)*(gconst5)*(gconst7)*(gconst9)))+(((gconst0)*(gconst29)*(gconst8)*(gconst9)))+(((gconst14)*(gconst19)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst22)*(gconst6)))+(((gconst11)*(gconst14)*(gconst18)*(gconst3)))+(((gconst0)*(gconst17)*(gconst20)*(gconst9)))+(((-1.00000000000000)*(gconst14)*(gconst22)*(gconst4)*(gconst6)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst30)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst2)*(gconst28)*(gconst6)))+(((gconst2)*(gconst31)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst18)*(gconst5)))+(((gconst2)*(gconst33)*(gconst4)*(gconst7)))+(((gconst0)*(gconst17)*(gconst21)*(gconst8)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst21)*(gconst8)))); op[7]=((((-1.00000000000000)*(gconst1)*(gconst21)*(gconst5)*(gconst7)))+(((gconst0)*(gconst20)*(gconst5)*(gconst9)))+(((gconst2)*(gconst23)*(gconst3)*(gconst6)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst8)*(gconst9)))+(((gconst1)*(gconst22)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst3)*(gconst8)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst7)*(gconst9)))+(((gconst0)*(gconst17)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst5)*(gconst7)*(gconst9)))+(((gconst14)*(gconst4)*(gconst7)*(gconst9)))+(((gconst11)*(gconst18)*(gconst2)*(gconst3)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst4)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst20)*(gconst3)*(gconst9)))+(((gconst2)*(gconst21)*(gconst4)*(gconst7)))+(((gconst1)*(gconst10)*(gconst18)*(gconst5)))+(((-1.00000000000000)*(gconst1)*(gconst19)*(gconst5)*(gconst9)))+(((gconst0)*(gconst21)*(gconst5)*(gconst8)))+(((-1.00000000000000)*(gconst10)*(gconst18)*(gconst2)*(gconst4)))+(((gconst10)*(gconst13)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst14)*(gconst3)*(gconst8)*(gconst9)))+(((gconst19)*(gconst2)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst22)*(gconst4)*(gconst6)))+(((gconst1)*(gconst10)*(gconst17)*(gconst6)))+(((gconst11)*(gconst14)*(gconst3)*(gconst6)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst17)*(gconst6)))+(((gconst11)*(gconst15)*(gconst2)*(gconst6)))+(((-1.00000000000000)*(gconst0)*(gconst23)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst5)*(gconst6)))+(((gconst12)*(gconst5)*(gconst8)*(gconst9)))+(((gconst16)*(gconst2)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst18)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst16)*(gconst2)*(gconst6)))); op[8]=((((-1.00000000000000)*(gconst0)*(gconst11)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst3)*(gconst8)*(gconst9)))+(((gconst0)*(gconst5)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst2)*(gconst4)*(gconst6)))+(((gconst11)*(gconst2)*(gconst3)*(gconst6)))+(((-1.00000000000000)*(gconst1)*(gconst5)*(gconst7)*(gconst9)))+(((gconst2)*(gconst4)*(gconst7)*(gconst9)))+(((gconst1)*(gconst10)*(gconst5)*(gconst6)))); polyroots8(op,zeror,numroots); IKReal j0array[8], cj0array[8], sj0array[8], tempj0array[1]; int numsolutions = 0; for(int ij0 = 0; ij0 < numroots; ++ij0) { IKReal htj0 = zeror[ij0]; tempj0array[0]=((2.00000000000000)*(atan(htj0))); for(int kj0 = 0; kj0 < 1; ++kj0) { j0array[numsolutions] = tempj0array[kj0]; if( j0array[numsolutions] > IKPI ) { j0array[numsolutions]-=IK2PI; } else if( j0array[numsolutions] < -IKPI ) { j0array[numsolutions]+=IK2PI; } sj0array[numsolutions] = IKsin(j0array[numsolutions]); cj0array[numsolutions] = IKcos(j0array[numsolutions]); numsolutions++; } } bool j0valid[8]={true,true,true,true,true,true,true,true}; _nj0 = 8; for(int ij0 = 0; ij0 < numsolutions; ++ij0) { if( !j0valid[ij0] ) { continue; } j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; htj0 = IKtan(j0/2); _ij0[0] = ij0; _ij0[1] = -1; for(int iij0 = ij0+1; iij0 < numsolutions; ++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; } } { IKReal j5array[2], cj5array[2], sj5array[2]; bool j5valid[2]={false}; _nj5 = 2; IKReal x54=((r01)*(sj0)); IKReal x55=((cj0)*(r11)); IKReal x56=((x54)+(((-1.00000000000000)*(x55)))); IKReal x57=((r00)*(sj0)); IKReal x58=((cj0)*(r10)); IKReal x59=((x57)+(((-1.00000000000000)*(x58)))); if( IKabs(x56) < IKFAST_ATAN2_MAGTHRESH && IKabs(x59) < IKFAST_ATAN2_MAGTHRESH ) continue; IKReal x60=IKatan2(x56, x59); j5array[0]=((-1.00000000000000)*(x60)); sj5array[0]=IKsin(j5array[0]); cj5array[0]=IKcos(j5array[0]); j5array[1]=((3.14159265358979)+(((-1.00000000000000)*(x60)))); sj5array[1]=IKsin(j5array[1]); cj5array[1]=IKcos(j5array[1]); if( j5array[0] > IKPI ) { j5array[0]-=IK2PI; } else if( j5array[0] < -IKPI ) { j5array[0]+=IK2PI; } j5valid[0] = true; if( j5array[1] > IKPI ) { j5array[1]-=IK2PI; } else if( j5array[1] < -IKPI ) { j5array[1]+=IK2PI; } j5valid[1] = true; 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 evalcond[1]; IKReal x61=IKsin(j5); IKReal x62=IKcos(j5); evalcond[0]=((-0.109150000000000)+(((0.0946500000000000)*(cj0)*(r11)*(x62)))+(((-1.00000000000000)*(px)*(sj0)))+(((-0.0946500000000000)*(r00)*(sj0)*(x61)))+(((0.0946500000000000)*(cj0)*(r10)*(x61)))+(((-0.0946500000000000)*(r01)*(sj0)*(x62)))+(((cj0)*(py)))); if( IKabs(evalcond[0]) > 0.000001 ) { continue; } } { IKReal dummyeval[1]; IKReal gconst52; gconst52=IKsign(((((r01)*(r22)*(sj0)*(sj5)))+(((-1.00000000000000)*(r02)*(r21)*(sj0)*(sj5)))+(((cj5)*(r02)*(r20)*(sj0)))+(((-1.00000000000000)*(cj5)*(r00)*(r22)*(sj0)))+(((cj0)*(cj5)*(r10)*(r22)))+(((cj0)*(r12)*(r21)*(sj5)))+(((-1.00000000000000)*(cj0)*(r11)*(r22)*(sj5)))+(((-1.00000000000000)*(cj0)*(cj5)*(r12)*(r20))))); dummyeval[0]=((((r01)*(r22)*(sj0)*(sj5)))+(((-1.00000000000000)*(r02)*(r21)*(sj0)*(sj5)))+(((cj5)*(r02)*(r20)*(sj0)))+(((-1.00000000000000)*(cj5)*(r00)*(r22)*(sj0)))+(((cj0)*(cj5)*(r10)*(r22)))+(((cj0)*(r12)*(r21)*(sj5)))+(((-1.00000000000000)*(cj0)*(r11)*(r22)*(sj5)))+(((-1.00000000000000)*(cj0)*(cj5)*(r12)*(r20)))); if( IKabs(dummyeval[0]) < 0.0000010000000000 ) { continue; } else { { IKReal j4array[1], cj4array[1], sj4array[1]; bool j4valid[1]={false}; _nj4 = 1; if( IKabs(((gconst52)*(r22))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst52)*(((((r21)*(sj5)))+(((-1.00000000000000)*(cj5)*(r20))))))) < IKFAST_ATAN2_MAGTHRESH ) continue; j4array[0]=IKatan2(((gconst52)*(r22)), ((gconst52)*(((((r21)*(sj5)))+(((-1.00000000000000)*(cj5)*(r20))))))); 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[4]; IKReal x63=IKsin(j4); IKReal x64=IKcos(j4); evalcond[0]=((((r22)*(x64)))+(((-1.00000000000000)*(r21)*(sj5)*(x63)))+(((cj5)*(r20)*(x63)))); evalcond[1]=((((-1.00000000000000)*(cj0)*(cj5)*(r10)*(x64)))+(((cj0)*(r11)*(sj5)*(x64)))+(((cj0)*(r12)*(x63)))+(((-1.00000000000000)*(r02)*(sj0)*(x63)))+(((-1.00000000000000)*(r01)*(sj0)*(sj5)*(x64)))+(((cj5)*(r00)*(sj0)*(x64)))); evalcond[2]=((((r11)*(sj0)*(sj5)*(x63)))+(((-1.00000000000000)*(cj0)*(cj5)*(r00)*(x63)))+(((-1.00000000000000)*(cj5)*(r10)*(sj0)*(x63)))+(((-1.00000000000000)*(cj0)*(r02)*(x64)))+(((-1.00000000000000)*(r12)*(sj0)*(x64)))+(((cj0)*(r01)*(sj5)*(x63)))); evalcond[3]=((1.00000000000000)+(((-1.00000000000000)*(cj0)*(cj5)*(r10)*(x63)))+(((r02)*(sj0)*(x64)))+(((cj0)*(r11)*(sj5)*(x63)))+(((-1.00000000000000)*(r01)*(sj0)*(sj5)*(x63)))+(((cj5)*(r00)*(sj0)*(x63)))+(((-1.00000000000000)*(cj0)*(r12)*(x64)))); if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) { continue; } } { IKReal dummyeval[1]; IKReal gconst53; gconst53=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz)); IKReal gconst54; gconst54=0.784500000000000; IKReal gconst55; gconst55=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz)); IKReal gconst56; gconst56=((0.817250000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5)))); IKReal gconst57; gconst57=((0.0327500000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5)))); IKReal gconst58; gconst58=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz)); IKReal gconst59; gconst59=0.784500000000000; IKReal gconst60; gconst60=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz)); IKReal gconst61; gconst61=((0.817250000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5)))); IKReal gconst62; gconst62=((0.0327500000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5)))); IKReal gconst63; gconst63=1.63450000000000; IKReal gconst64; gconst64=0.0655000000000000; IKReal gconst65; gconst65=-1.56900000000000; IKReal gconst66; gconst66=1.63450000000000; IKReal gconst67; gconst67=0.0655000000000000; IKReal gconst68; gconst68=-1.56900000000000; IKReal gconst69; gconst69=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz)); IKReal gconst70; gconst70=-0.784500000000000; IKReal gconst71; gconst71=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz)); IKReal gconst72; gconst72=((-0.817250000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5)))); IKReal gconst73; gconst73=((-0.0327500000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5)))); IKReal gconst74; gconst74=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz)); IKReal gconst75; gconst75=-0.784500000000000; IKReal gconst76; gconst76=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz)); IKReal gconst77; gconst77=((-0.817250000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5)))); IKReal gconst78; gconst78=((-0.0327500000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5)))); dummyeval[0]=((((gconst69)*(gconst73)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst69)*(gconst73)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst70)*(gconst73)*(gconst75)*(gconst77)))+(((gconst71)*(gconst72)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst71)*(gconst72)*(gconst76)*(gconst77)))); if( IKabs(dummyeval[0]) < 0.0000001000000000 ) { continue; } else { IKReal op[8+1], zeror[8]; int numroots; op[0]=((((gconst69)*(gconst73)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst69)*(gconst73)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst70)*(gconst73)*(gconst75)*(gconst77)))+(((gconst71)*(gconst72)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst71)*(gconst72)*(gconst76)*(gconst77)))); op[1]=((((gconst65)*(gconst71)*(gconst75)*(gconst77)))+(((-1.00000000000000)*(gconst66)*(gconst69)*(gconst73)*(gconst78)))+(((-1.00000000000000)*(gconst64)*(gconst72)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst63)*(gconst73)*(gconst74)*(gconst78)))+(((gconst67)*(gconst69)*(gconst73)*(gconst77)))+(((-1.00000000000000)*(gconst67)*(gconst71)*(gconst72)*(gconst77)))+(((gconst64)*(gconst72)*(gconst74)*(gconst78)))+(((gconst66)*(gconst71)*(gconst72)*(gconst78)))+(((gconst63)*(gconst73)*(gconst76)*(gconst77)))+(((gconst68)*(gconst70)*(gconst73)*(gconst74)))); op[2]=((((gconst58)*(gconst71)*(gconst72)*(gconst78)))+(((-1.00000000000000)*(gconst64)*(gconst67)*(gconst72)*(gconst77)))+(((gconst62)*(gconst71)*(gconst72)*(gconst74)))+(((-1.00000000000000)*(gconst57)*(gconst69)*(gconst74)*(gconst78)))+(((gconst60)*(gconst69)*(gconst73)*(gconst77)))+(((gconst53)*(gconst73)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst57)*(gconst70)*(gconst75)*(gconst77)))+(((-1.00000000000000)*(gconst62)*(gconst69)*(gconst73)*(gconst74)))+(((gconst63)*(gconst67)*(gconst73)*(gconst77)))+(((gconst64)*(gconst65)*(gconst75)*(gconst77)))+(((-1.00000000000000)*(gconst60)*(gconst71)*(gconst72)*(gconst77)))+(((gconst64)*(gconst66)*(gconst72)*(gconst78)))+(((gconst66)*(gconst68)*(gconst70)*(gconst73)))+(((-1.00000000000000)*(gconst65)*(gconst68)*(gconst71)*(gconst74)))+(((-1.00000000000000)*(gconst58)*(gconst69)*(gconst73)*(gconst78)))+(((gconst55)*(gconst72)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst53)*(gconst73)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst56)*(gconst71)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst54)*(gconst73)*(gconst75)*(gconst77)))+(((gconst61)*(gconst69)*(gconst73)*(gconst76)))+(((-1.00000000000000)*(gconst61)*(gconst71)*(gconst72)*(gconst76)))+(((-1.00000000000000)*(gconst61)*(gconst70)*(gconst73)*(gconst75)))+(((-1.00000000000000)*(gconst55)*(gconst72)*(gconst76)*(gconst77)))+(((gconst57)*(gconst69)*(gconst76)*(gconst77)))+(((gconst56)*(gconst71)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst63)*(gconst66)*(gconst73)*(gconst78)))+(((-1.00000000000000)*(gconst59)*(gconst70)*(gconst73)*(gconst77)))); op[3]=((((gconst62)*(gconst64)*(gconst72)*(gconst74)))+(((gconst61)*(gconst67)*(gconst69)*(gconst73)))+(((gconst58)*(gconst64)*(gconst72)*(gconst78)))+(((-1.00000000000000)*(gconst61)*(gconst64)*(gconst72)*(gconst76)))+(((gconst53)*(gconst67)*(gconst73)*(gconst77)))+(((-1.00000000000000)*(gconst62)*(gconst66)*(gconst69)*(gconst73)))+(((gconst57)*(gconst67)*(gconst69)*(gconst77)))+(((gconst55)*(gconst65)*(gconst75)*(gconst77)))+(((gconst57)*(gconst68)*(gconst70)*(gconst74)))+(((-1.00000000000000)*(gconst60)*(gconst64)*(gconst72)*(gconst77)))+(((-1.00000000000000)*(gconst65)*(gconst66)*(gconst68)*(gconst71)))+(((-1.00000000000000)*(gconst57)*(gconst63)*(gconst74)*(gconst78)))+(((gconst56)*(gconst64)*(gconst74)*(gconst78)))+(((gconst60)*(gconst63)*(gconst73)*(gconst77)))+(((gconst55)*(gconst66)*(gconst72)*(gconst78)))+(((-1.00000000000000)*(gconst53)*(gconst66)*(gconst73)*(gconst78)))+(((-1.00000000000000)*(gconst58)*(gconst63)*(gconst73)*(gconst78)))+(((-1.00000000000000)*(gconst56)*(gconst67)*(gconst71)*(gconst77)))+(((-1.00000000000000)*(gconst64)*(gconst65)*(gconst68)*(gconst74)))+(((gconst61)*(gconst63)*(gconst73)*(gconst76)))+(((-1.00000000000000)*(gconst55)*(gconst67)*(gconst72)*(gconst77)))+(((-1.00000000000000)*(gconst62)*(gconst63)*(gconst73)*(gconst74)))+(((gconst62)*(gconst66)*(gconst71)*(gconst72)))+(((gconst54)*(gconst68)*(gconst73)*(gconst74)))+(((-1.00000000000000)*(gconst61)*(gconst67)*(gconst71)*(gconst72)))+(((gconst61)*(gconst65)*(gconst71)*(gconst75)))+(((gconst56)*(gconst66)*(gconst71)*(gconst78)))+(((gconst59)*(gconst65)*(gconst71)*(gconst77)))+(((gconst58)*(gconst68)*(gconst70)*(gconst73)))+(((gconst57)*(gconst63)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst56)*(gconst64)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst57)*(gconst66)*(gconst69)*(gconst78)))); op[4]=((((-1.00000000000000)*(gconst62)*(gconst63)*(gconst66)*(gconst73)))+(((gconst53)*(gconst57)*(gconst76)*(gconst77)))+(((gconst53)*(gconst60)*(gconst73)*(gconst77)))+(((gconst56)*(gconst58)*(gconst71)*(gconst78)))+(((gconst58)*(gconst62)*(gconst71)*(gconst72)))+(((gconst57)*(gconst61)*(gconst69)*(gconst76)))+(((gconst54)*(gconst66)*(gconst68)*(gconst73)))+(((gconst61)*(gconst64)*(gconst65)*(gconst75)))+(((-1.00000000000000)*(gconst58)*(gconst65)*(gconst68)*(gconst71)))+(((-1.00000000000000)*(gconst54)*(gconst59)*(gconst73)*(gconst77)))+(((-1.00000000000000)*(gconst53)*(gconst57)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst57)*(gconst59)*(gconst70)*(gconst77)))+(((-1.00000000000000)*(gconst54)*(gconst57)*(gconst75)*(gconst77)))+(((-1.00000000000000)*(gconst57)*(gconst63)*(gconst66)*(gconst78)))+(((-1.00000000000000)*(gconst57)*(gconst61)*(gconst70)*(gconst75)))+(((gconst55)*(gconst56)*(gconst74)*(gconst78)))+(((gconst57)*(gconst66)*(gconst68)*(gconst70)))+(((gconst62)*(gconst64)*(gconst66)*(gconst72)))+(((gconst55)*(gconst62)*(gconst72)*(gconst74)))+(((gconst61)*(gconst63)*(gconst67)*(gconst73)))+(((-1.00000000000000)*(gconst58)*(gconst62)*(gconst69)*(gconst73)))+(((-1.00000000000000)*(gconst53)*(gconst58)*(gconst73)*(gconst78)))+(((-1.00000000000000)*(gconst57)*(gconst58)*(gconst69)*(gconst78)))+(((gconst59)*(gconst64)*(gconst65)*(gconst77)))+(((-1.00000000000000)*(gconst55)*(gconst61)*(gconst72)*(gconst76)))+(((gconst60)*(gconst61)*(gconst69)*(gconst73)))+(((-1.00000000000000)*(gconst56)*(gconst60)*(gconst71)*(gconst77)))+(((-1.00000000000000)*(gconst56)*(gconst61)*(gconst71)*(gconst76)))+(((-1.00000000000000)*(gconst54)*(gconst61)*(gconst73)*(gconst75)))+(((-1.00000000000000)*(gconst55)*(gconst60)*(gconst72)*(gconst77)))+(((-1.00000000000000)*(gconst61)*(gconst64)*(gconst67)*(gconst72)))+(((-1.00000000000000)*(gconst53)*(gconst62)*(gconst73)*(gconst74)))+(((-1.00000000000000)*(gconst57)*(gconst62)*(gconst69)*(gconst74)))+(((-1.00000000000000)*(gconst56)*(gconst64)*(gconst67)*(gconst77)))+(((-1.00000000000000)*(gconst55)*(gconst56)*(gconst76)*(gconst77)))+(((gconst56)*(gconst62)*(gconst71)*(gconst74)))+(((-1.00000000000000)*(gconst60)*(gconst61)*(gconst71)*(gconst72)))+(((-1.00000000000000)*(gconst55)*(gconst65)*(gconst68)*(gconst74)))+(((-1.00000000000000)*(gconst64)*(gconst65)*(gconst66)*(gconst68)))+(((gconst57)*(gconst63)*(gconst67)*(gconst77)))+(((-1.00000000000000)*(gconst59)*(gconst61)*(gconst70)*(gconst73)))+(((gconst53)*(gconst61)*(gconst73)*(gconst76)))+(((gconst57)*(gconst60)*(gconst69)*(gconst77)))+(((gconst56)*(gconst64)*(gconst66)*(gconst78)))+(((gconst55)*(gconst58)*(gconst72)*(gconst78)))); op[5]=((((gconst55)*(gconst62)*(gconst66)*(gconst72)))+(((-1.00000000000000)*(gconst56)*(gconst61)*(gconst64)*(gconst76)))+(((gconst53)*(gconst57)*(gconst67)*(gconst77)))+(((-1.00000000000000)*(gconst53)*(gconst57)*(gconst66)*(gconst78)))+(((gconst54)*(gconst57)*(gconst68)*(gconst74)))+(((gconst56)*(gconst58)*(gconst64)*(gconst78)))+(((gconst57)*(gconst60)*(gconst63)*(gconst77)))+(((gconst57)*(gconst61)*(gconst67)*(gconst69)))+(((gconst60)*(gconst61)*(gconst63)*(gconst73)))+(((gconst57)*(gconst58)*(gconst68)*(gconst70)))+(((gconst54)*(gconst58)*(gconst68)*(gconst73)))+(((gconst55)*(gconst61)*(gconst65)*(gconst75)))+(((gconst59)*(gconst61)*(gconst65)*(gconst71)))+(((gconst53)*(gconst61)*(gconst67)*(gconst73)))+(((-1.00000000000000)*(gconst53)*(gconst62)*(gconst66)*(gconst73)))+(((-1.00000000000000)*(gconst58)*(gconst64)*(gconst65)*(gconst68)))+(((-1.00000000000000)*(gconst56)*(gconst60)*(gconst64)*(gconst77)))+(((-1.00000000000000)*(gconst55)*(gconst56)*(gconst67)*(gconst77)))+(((-1.00000000000000)*(gconst58)*(gconst62)*(gconst63)*(gconst73)))+(((-1.00000000000000)*(gconst55)*(gconst61)*(gconst67)*(gconst72)))+(((gconst55)*(gconst59)*(gconst65)*(gconst77)))+(((gconst56)*(gconst62)*(gconst64)*(gconst74)))+(((-1.00000000000000)*(gconst55)*(gconst65)*(gconst66)*(gconst68)))+(((gconst56)*(gconst62)*(gconst66)*(gconst71)))+(((-1.00000000000000)*(gconst60)*(gconst61)*(gconst64)*(gconst72)))+(((gconst57)*(gconst61)*(gconst63)*(gconst76)))+(((-1.00000000000000)*(gconst56)*(gconst61)*(gconst67)*(gconst71)))+(((-1.00000000000000)*(gconst57)*(gconst58)*(gconst63)*(gconst78)))+(((-1.00000000000000)*(gconst57)*(gconst62)*(gconst63)*(gconst74)))+(((gconst58)*(gconst62)*(gconst64)*(gconst72)))+(((-1.00000000000000)*(gconst57)*(gconst62)*(gconst66)*(gconst69)))+(((gconst55)*(gconst56)*(gconst66)*(gconst78)))); op[6]=((((-1.00000000000000)*(gconst56)*(gconst61)*(gconst64)*(gconst67)))+(((-1.00000000000000)*(gconst57)*(gconst58)*(gconst62)*(gconst69)))+(((-1.00000000000000)*(gconst55)*(gconst58)*(gconst65)*(gconst68)))+(((-1.00000000000000)*(gconst54)*(gconst57)*(gconst61)*(gconst75)))+(((gconst54)*(gconst57)*(gconst66)*(gconst68)))+(((gconst56)*(gconst58)*(gconst62)*(gconst71)))+(((gconst59)*(gconst61)*(gconst64)*(gconst65)))+(((gconst55)*(gconst56)*(gconst58)*(gconst78)))+(((-1.00000000000000)*(gconst55)*(gconst56)*(gconst61)*(gconst76)))+(((-1.00000000000000)*(gconst55)*(gconst60)*(gconst61)*(gconst72)))+(((gconst53)*(gconst60)*(gconst61)*(gconst73)))+(((-1.00000000000000)*(gconst53)*(gconst57)*(gconst58)*(gconst78)))+(((gconst57)*(gconst60)*(gconst61)*(gconst69)))+(((-1.00000000000000)*(gconst53)*(gconst57)*(gconst62)*(gconst74)))+(((gconst55)*(gconst56)*(gconst62)*(gconst74)))+(((gconst55)*(gconst58)*(gconst62)*(gconst72)))+(((-1.00000000000000)*(gconst57)*(gconst59)*(gconst61)*(gconst70)))+(((-1.00000000000000)*(gconst54)*(gconst57)*(gconst59)*(gconst77)))+(((gconst56)*(gconst62)*(gconst64)*(gconst66)))+(((gconst53)*(gconst57)*(gconst61)*(gconst76)))+(((-1.00000000000000)*(gconst54)*(gconst59)*(gconst61)*(gconst73)))+(((gconst57)*(gconst61)*(gconst63)*(gconst67)))+(((gconst53)*(gconst57)*(gconst60)*(gconst77)))+(((-1.00000000000000)*(gconst57)*(gconst62)*(gconst63)*(gconst66)))+(((-1.00000000000000)*(gconst53)*(gconst58)*(gconst62)*(gconst73)))+(((-1.00000000000000)*(gconst56)*(gconst60)*(gconst61)*(gconst71)))+(((-1.00000000000000)*(gconst55)*(gconst56)*(gconst60)*(gconst77)))); op[7]=((((gconst55)*(gconst59)*(gconst61)*(gconst65)))+(((-1.00000000000000)*(gconst57)*(gconst58)*(gconst62)*(gconst63)))+(((-1.00000000000000)*(gconst55)*(gconst56)*(gconst61)*(gconst67)))+(((gconst56)*(gconst58)*(gconst62)*(gconst64)))+(((gconst57)*(gconst60)*(gconst61)*(gconst63)))+(((gconst55)*(gconst56)*(gconst62)*(gconst66)))+(((-1.00000000000000)*(gconst53)*(gconst57)*(gconst62)*(gconst66)))+(((gconst53)*(gconst57)*(gconst61)*(gconst67)))+(((gconst54)*(gconst57)*(gconst58)*(gconst68)))+(((-1.00000000000000)*(gconst56)*(gconst60)*(gconst61)*(gconst64)))); op[8]=((((gconst55)*(gconst56)*(gconst58)*(gconst62)))+(((-1.00000000000000)*(gconst53)*(gconst57)*(gconst58)*(gconst62)))+(((-1.00000000000000)*(gconst54)*(gconst57)*(gconst59)*(gconst61)))+(((gconst53)*(gconst57)*(gconst60)*(gconst61)))+(((-1.00000000000000)*(gconst55)*(gconst56)*(gconst60)*(gconst61)))); polyroots8(op,zeror,numroots); IKReal j1array[8], cj1array[8], sj1array[8], tempj1array[1]; int numsolutions = 0; for(int ij1 = 0; ij1 < numroots; ++ij1) { IKReal htj1 = zeror[ij1]; tempj1array[0]=((2.00000000000000)*(atan(htj1))); for(int kj1 = 0; kj1 < 1; ++kj1) { j1array[numsolutions] = tempj1array[kj1]; if( j1array[numsolutions] > IKPI ) { j1array[numsolutions]-=IK2PI; } else if( j1array[numsolutions] < -IKPI ) { j1array[numsolutions]+=IK2PI; } sj1array[numsolutions] = IKsin(j1array[numsolutions]); cj1array[numsolutions] = IKcos(j1array[numsolutions]); numsolutions++; } } bool j1valid[8]={true,true,true,true,true,true,true,true}; _nj1 = 8; for(int ij1 = 0; ij1 < numsolutions; ++ij1) { if( !j1valid[ij1] ) { continue; } j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1]; htj1 = IKtan(j1/2); _ij1[0] = ij1; _ij1[1] = -1; for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1) { if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH ) { j1valid[iij1]=false; _ij1[1] = iij1; break; } } { IKReal dummyeval[1]; IKReal gconst95; gconst95=IKsign(((((-1569.00000000000)*((cj1)*(cj1))))+(((-1569.00000000000)*((sj1)*(sj1)))))); dummyeval[0]=((((-1.00000000000000)*((sj1)*(sj1))))+(((-1.00000000000000)*((cj1)*(cj1))))); if( IKabs(dummyeval[0]) < 0.0000010000000000 ) { continue; } else { { IKReal j2array[1], cj2array[1], sj2array[1]; bool j2valid[1]={false}; _nj2 = 1; if( IKabs(((gconst95)*(((((4000.00000000000)*(cj0)*(px)*(sj1)))+(((4000.00000000000)*(py)*(sj0)*(sj1)))+(((378.600000000000)*(r10)*(sj0)*(sj1)*(sj5)))+(((378.600000000000)*(cj0)*(r00)*(sj1)*(sj5)))+(((378.600000000000)*(cj0)*(cj5)*(r01)*(sj1)))+(((378.600000000000)*(cj1)*(cj5)*(r21)))+(((378.600000000000)*(cj5)*(r11)*(sj0)*(sj1)))+(((4000.00000000000)*(cj1)*(pz)))+(((378.600000000000)*(cj1)*(r20)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst95)*(((((-378.600000000000)*(cj1)*(r10)*(sj0)*(sj5)))+(((1700.00000000000)*((cj1)*(cj1))))+(((378.600000000000)*(r20)*(sj1)*(sj5)))+(((-4000.00000000000)*(cj0)*(cj1)*(px)))+(((4000.00000000000)*(pz)*(sj1)))+(((-378.600000000000)*(cj0)*(cj1)*(cj5)*(r01)))+(((-378.600000000000)*(cj1)*(cj5)*(r11)*(sj0)))+(((1700.00000000000)*((sj1)*(sj1))))+(((-4000.00000000000)*(cj1)*(py)*(sj0)))+(((-378.600000000000)*(cj0)*(cj1)*(r00)*(sj5)))+(((378.600000000000)*(cj5)*(r21)*(sj1))))))) < IKFAST_ATAN2_MAGTHRESH ) continue; j2array[0]=IKatan2(((gconst95)*(((((4000.00000000000)*(cj0)*(px)*(sj1)))+(((4000.00000000000)*(py)*(sj0)*(sj1)))+(((378.600000000000)*(r10)*(sj0)*(sj1)*(sj5)))+(((378.600000000000)*(cj0)*(r00)*(sj1)*(sj5)))+(((378.600000000000)*(cj0)*(cj5)*(r01)*(sj1)))+(((378.600000000000)*(cj1)*(cj5)*(r21)))+(((378.600000000000)*(cj5)*(r11)*(sj0)*(sj1)))+(((4000.00000000000)*(cj1)*(pz)))+(((378.600000000000)*(cj1)*(r20)*(sj5)))))), ((gconst95)*(((((-378.600000000000)*(cj1)*(r10)*(sj0)*(sj5)))+(((1700.00000000000)*((cj1)*(cj1))))+(((378.600000000000)*(r20)*(sj1)*(sj5)))+(((-4000.00000000000)*(cj0)*(cj1)*(px)))+(((4000.00000000000)*(pz)*(sj1)))+(((-378.600000000000)*(cj0)*(cj1)*(cj5)*(r01)))+(((-378.600000000000)*(cj1)*(cj5)*(r11)*(sj0)))+(((1700.00000000000)*((sj1)*(sj1))))+(((-4000.00000000000)*(cj1)*(py)*(sj0)))+(((-378.600000000000)*(cj0)*(cj1)*(r00)*(sj5)))+(((378.600000000000)*(cj5)*(r21)*(sj1))))))); sj2array[0]=IKsin(j2array[0]); cj2array[0]=IKcos(j2array[0]); if( j2array[0] > IKPI ) { j2array[0]-=IK2PI; } else if( j2array[0] < -IKPI ) { j2array[0]+=IK2PI; } j2valid[0] = true; for(int ij2 = 0; ij2 < 1; ++ij2) { if( !j2valid[ij2] ) { continue; } _ij2[0] = ij2; _ij2[1] = -1; for(int iij2 = ij2+1; iij2 < 1; ++iij2) { if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH ) { j2valid[iij2]=false; _ij2[1] = iij2; break; } } j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2]; { IKReal evalcond[2]; IKReal x65=IKcos(j2); IKReal x66=IKsin(j2); evalcond[0]=((((0.0946500000000000)*(cj5)*(r21)))+(((0.425000000000000)*(sj1)))+(((0.392250000000000)*(cj1)*(x66)))+(((0.0946500000000000)*(r20)*(sj5)))+(((0.392250000000000)*(sj1)*(x65)))+(pz)); evalcond[1]=((((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-0.392250000000000)*(sj1)*(x66)))+(((0.392250000000000)*(cj1)*(x65)))+(((0.425000000000000)*(cj1)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5)))); if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) { continue; } } { IKReal dummyeval[1]; IKReal gconst96; IKReal x67=(sj1)*(sj1); IKReal x68=(cj2)*(cj2); IKReal x69=(cj1)*(cj1); IKReal x70=(sj2)*(sj2); gconst96=IKsign(((((x69)*(x70)))+(((x67)*(x70)))+(((x68)*(x69)))+(((x67)*(x68))))); IKReal x71=(sj1)*(sj1); IKReal x72=(cj2)*(cj2); IKReal x73=(cj1)*(cj1); IKReal x74=(sj2)*(sj2); dummyeval[0]=((((x72)*(x73)))+(((x73)*(x74)))+(((x71)*(x72)))+(((x71)*(x74)))); if( IKabs(dummyeval[0]) < 0.0000010000000000 ) { continue; } else { { IKReal j3array[1], cj3array[1], sj3array[1]; bool j3valid[1]={false}; _nj3 = 1; if( IKabs(((gconst96)*(((((-1.00000000000000)*(cj1)*(cj5)*(r21)*(sj2)))+(((-1.00000000000000)*(cj2)*(r20)*(sj1)*(sj5)))+(((cj4)*(r21)*(sj1)*(sj2)*(sj5)))+(((cj1)*(cj2)*(cj4)*(cj5)*(r20)))+(((-1.00000000000000)*(cj4)*(cj5)*(r20)*(sj1)*(sj2)))+(((r22)*(sj1)*(sj2)*(sj4)))+(((-1.00000000000000)*(cj1)*(cj2)*(r22)*(sj4)))+(((-1.00000000000000)*(cj2)*(cj5)*(r21)*(sj1)))+(((-1.00000000000000)*(cj1)*(cj2)*(cj4)*(r21)*(sj5)))+(((-1.00000000000000)*(cj1)*(r20)*(sj2)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst96)*(((((-1.00000000000000)*(r20)*(sj1)*(sj2)*(sj5)))+(((cj1)*(cj2)*(r20)*(sj5)))+(((-1.00000000000000)*(cj1)*(cj4)*(r21)*(sj2)*(sj5)))+(((-1.00000000000000)*(cj1)*(r22)*(sj2)*(sj4)))+(((-1.00000000000000)*(cj2)*(r22)*(sj1)*(sj4)))+(((cj2)*(cj4)*(cj5)*(r20)*(sj1)))+(((cj1)*(cj2)*(cj5)*(r21)))+(((cj1)*(cj4)*(cj5)*(r20)*(sj2)))+(((-1.00000000000000)*(cj2)*(cj4)*(r21)*(sj1)*(sj5)))+(((-1.00000000000000)*(cj5)*(r21)*(sj1)*(sj2))))))) < IKFAST_ATAN2_MAGTHRESH ) continue; j3array[0]=IKatan2(((gconst96)*(((((-1.00000000000000)*(cj1)*(cj5)*(r21)*(sj2)))+(((-1.00000000000000)*(cj2)*(r20)*(sj1)*(sj5)))+(((cj4)*(r21)*(sj1)*(sj2)*(sj5)))+(((cj1)*(cj2)*(cj4)*(cj5)*(r20)))+(((-1.00000000000000)*(cj4)*(cj5)*(r20)*(sj1)*(sj2)))+(((r22)*(sj1)*(sj2)*(sj4)))+(((-1.00000000000000)*(cj1)*(cj2)*(r22)*(sj4)))+(((-1.00000000000000)*(cj2)*(cj5)*(r21)*(sj1)))+(((-1.00000000000000)*(cj1)*(cj2)*(cj4)*(r21)*(sj5)))+(((-1.00000000000000)*(cj1)*(r20)*(sj2)*(sj5)))))), ((gconst96)*(((((-1.00000000000000)*(r20)*(sj1)*(sj2)*(sj5)))+(((cj1)*(cj2)*(r20)*(sj5)))+(((-1.00000000000000)*(cj1)*(cj4)*(r21)*(sj2)*(sj5)))+(((-1.00000000000000)*(cj1)*(r22)*(sj2)*(sj4)))+(((-1.00000000000000)*(cj2)*(r22)*(sj1)*(sj4)))+(((cj2)*(cj4)*(cj5)*(r20)*(sj1)))+(((cj1)*(cj2)*(cj5)*(r21)))+(((cj1)*(cj4)*(cj5)*(r20)*(sj2)))+(((-1.00000000000000)*(cj2)*(cj4)*(r21)*(sj1)*(sj5)))+(((-1.00000000000000)*(cj5)*(r21)*(sj1)*(sj2))))))); sj3array[0]=IKsin(j3array[0]); cj3array[0]=IKcos(j3array[0]); if( j3array[0] > IKPI ) { j3array[0]-=IK2PI; } else if( j3array[0] < -IKPI ) { j3array[0]+=IK2PI; } j3valid[0] = true; for(int ij3 = 0; ij3 < 1; ++ij3) { if( !j3valid[ij3] ) { continue; } _ij3[0] = ij3; _ij3[1] = -1; for(int iij3 = ij3+1; iij3 < 1; ++iij3) { if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) { j3valid[iij3]=false; _ij3[1] = iij3; break; } } j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; { IKReal evalcond[4]; IKReal x75=IKsin(j3); IKReal x76=IKcos(j3); IKReal x77=((sj1)*(sj2)*(x75)); IKReal x78=((cj2)*(sj1)*(x76)); IKReal x79=((cj1)*(sj2)*(x76)); IKReal x80=((cj1)*(cj2)*(x75)); IKReal x81=((cj1)*(sj2)*(x75)); IKReal x82=((cj2)*(sj1)*(x75)); IKReal x83=((sj1)*(sj2)*(x76)); IKReal x84=((cj1)*(cj2)*(x76)); evalcond[0]=((((-1.00000000000000)*(r20)*(sj5)))+(x84)+(((-1.00000000000000)*(cj5)*(r21)))+(((-1.00000000000000)*(x81)))+(((-1.00000000000000)*(x83)))+(((-1.00000000000000)*(x82)))); evalcond[1]=((((-1.00000000000000)*(x78)))+(((-1.00000000000000)*(x79)))+(((-1.00000000000000)*(r22)*(sj4)))+(((-1.00000000000000)*(cj4)*(r21)*(sj5)))+(x77)+(((cj4)*(cj5)*(r20)))+(((-1.00000000000000)*(x80)))); evalcond[2]=((((cj0)*(cj5)*(r01)))+(((-1.00000000000000)*(x78)))+(((-1.00000000000000)*(x79)))+(((cj5)*(r11)*(sj0)))+(x77)+(((cj0)*(r00)*(sj5)))+(((r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(x80)))); evalcond[3]=((((-1.00000000000000)*(cj4)*(cj5)*(r10)*(sj0)))+(((-1.00000000000000)*(cj0)*(cj4)*(cj5)*(r00)))+(((r12)*(sj0)*(sj4)))+(((cj4)*(r11)*(sj0)*(sj5)))+(x82)+(x83)+(x81)+(((-1.00000000000000)*(x84)))+(((cj0)*(cj4)*(r01)*(sj5)))+(((cj0)*(r02)*(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(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); } } } } } } } } } } } } } } } } } } } } return vsolutions.size()>0; } static inline void polyroots8(IKReal rawcoeffs[8+1], IKReal rawroots[8], int& numroots) { using std::complex; //IKFAST_ASSERT(rawcoeffs[0] != 0); const IKReal tol = 128.0*std::numeric_limits<IKReal>::epsilon(); const IKReal tolsqrt = 8*sqrt(std::numeric_limits<IKReal>::epsilon()); complex<IKReal> coeffs[8]; const int maxsteps = 110; for(int i = 0; i < 8; ++i) { coeffs[i] = complex<IKReal>(rawcoeffs[i+1]/rawcoeffs[0]); } complex<IKReal> roots[8]; IKReal err[8]; roots[0] = complex<IKReal>(1,0); roots[1] = complex<IKReal>(0.4,0.9); // any complex number not a root of unity works err[0] = 1.0; err[1] = 1.0; for(int i = 2; i < 8; ++i) { roots[i] = roots[i-1]*roots[1]; err[i] = 1.0; } for(int step = 0; step < maxsteps; ++step) { bool changed = false; for(int i = 0; i < 8; ++i) { if ( err[i] >= tol ) { changed = true; // evaluate complex<IKReal> x = roots[i] + coeffs[0]; for(int j = 1; j < 8; ++j) { x = roots[i] * x + coeffs[j]; } for(int j = 0; j < 8; ++j) { if( i != j ) { if( roots[i] != roots[j] ) { x /= (roots[i] - roots[j]); } } } roots[i] -= x; err[i] = abs(x); } } if( !changed ) { break; } } numroots = 0; bool visited[8] = {false}; for(int i = 0; i < 8; ++i) { if( !visited[i] ) { // might be a multiple root, in which case it will have more error than the other roots // find any neighboring roots, and take the average complex<IKReal> newroot=roots[i]; int n = 1; for(int j = i+1; j < 8; ++j) { if( abs(roots[i]-roots[j]) < tolsqrt ) { newroot += roots[j]; n += 1; visited[j] = true; } } if( n > 1 ) { newroot /= n; } // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt if( IKabs(imag(newroot)) < sqrt(std::numeric_limits<IKReal>::epsilon()) ) { rawroots[numroots++] = real(newroot); } } } } }; /// 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 "b61b6309e37433286886547930660887"; } IKFAST_API const char* getIKFastVersion() { return "54"; } #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