Skip to content

Instantly share code, notes, and snippets.

@evenator

evenator/ifastdemo.cpp

Created January 29, 2013 07:51
Show Gist options
  • Save evenator/4662558 to your computer and use it in GitHub Desktop.
Save evenator/4662558 to your computer and use it in GitHub Desktop.
Download ZIP
ikfast code files
Raw
ifastdemo.cpp
/*
* IKFast Demo
*
* Shows how to calculate FK from joint angles.
* Calculates IK from rotation-translation matrix, or translation-quaternion pose.
* Performance timing tests.
*
* Run the program to view command line parameters.
*
*
* To compile, run:
* g++ -lstdc++ -llapack -o compute ikfastdemo.cpp -lrt
* (need to link with 'rt' for gettime(), it must come after the source file name)
*
*
* Tested with Ubuntu 11.10 (Oneiric)
* IKFast54 from OpenRAVE 0.6.0
* IKFast56/61 from OpenRave 0.8.2
*
* Author: David Butterworth, KAIST
* Based on code by Rosen Diankov
* Date: November 2012
*/
/*
* Copyright (c) 2012, David Butterworth, KAIST
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#define IKFAST_HAS_LIBRARY // Build IKFast with API functions
#define IKFAST_NO_MAIN // Don't include main() from IKFast
/*
Set which IKFast version you are using
The API calls are slightly different for versions > 54
*/
#define IK_VERSION 61
#include "output_ikfast61.cpp"
//#define IK_VERSION 56
//#include "ikfast56.Transform6D.0_1_2_3_4_5.cpp"
//#define IK_VERSION 54
//#include "output_ikfast54.cpp"
//----------------------------------------------------------------------------//
#include <stdio.h>
#include <stdlib.h>
#include <time.h> // for clock_gettime()
float SIGN(float x);
float NORM(float a, float b, float c, float d);
#if IK_VERSION > 54
#define IKREAL_TYPE IkReal // for IKFast 56,61
#else
#define IKREAL_TYPE IKReal // for IKFast 54
#endif
int main(int argc, char** argv)
{
IKREAL_TYPE eerot[9],eetrans[3];
#if IK_VERSION > 54
// for IKFast 56,61
unsigned int num_of_joints = GetNumJoints();
unsigned int num_free_parameters = GetNumFreeParameters();
#else
// for IKFast 54
unsigned int num_of_joints = getNumJoints();
unsigned int num_free_parameters = getNumFreeParameters();
#endif
std::string cmd;
if (argv[1]) cmd = argv[1];
//printf("command: %s \n\n", cmd.c_str() );
if (cmd.compare("ik") == 0) // ik mode
{
if( argc == 1+7+num_free_parameters+1 ) // ik, given translation vector and quaternion pose
{
#if IK_VERSION > 54
// for IKFast 56,61
IkSolutionList<IKREAL_TYPE> solutions;
#else
// for IKFast 54
std::vector<IKSolution> vsolutions;
#endif
std::vector<IKREAL_TYPE> vfree(num_free_parameters);
eetrans[0] = atof(argv[2]);
eetrans[1] = atof(argv[3]);
eetrans[2] = atof(argv[4]);
// Convert input effector pose, in w x y z quaternion notation, to rotation matrix.
// Must use doubles, else lose precision compared to directly inputting the rotation matrix.
double qw = atof(argv[5]);
double qx = atof(argv[6]);
double qy = atof(argv[7]);
double qz = atof(argv[8]);
const double n = 1.0f/sqrt(qx*qx+qy*qy+qz*qz+qw*qw);
qw *= n;
qx *= n;
qy *= n;
qz *= n;
eerot[0] = 1.0f - 2.0f*qy*qy - 2.0f*qz*qz; eerot[1] = 2.0f*qx*qy - 2.0f*qz*qw; eerot[2] = 2.0f*qx*qz + 2.0f*qy*qw;
eerot[3] = 2.0f*qx*qy + 2.0f*qz*qw; eerot[4] = 1.0f - 2.0f*qx*qx - 2.0f*qz*qz; eerot[5] = 2.0f*qy*qz - 2.0f*qx*qw;
eerot[6] = 2.0f*qx*qz - 2.0f*qy*qw; eerot[7] = 2.0f*qy*qz + 2.0f*qx*qw; eerot[8] = 1.0f - 2.0f*qx*qx - 2.0f*qy*qy;
// For debugging, output the matrix
/*
for (unsigned char i=0; i<=8; i++)
{ // detect -0.0 and replace with 0.0
if ( ((int&)(eerot[i]) & 0xFFFFFFFF) == 0) eerot[i] = 0.0;
}
printf(" Rotation %f %f %f \n", eerot[0], eerot[1], eerot[2] );
printf(" %f %f %f \n", eerot[3], eerot[4], eerot[5] );
printf(" %f %f %f \n", eerot[6], eerot[7], eerot[8] );
printf("\n");
*/
for(std::size_t i = 0; i < vfree.size(); ++i)
vfree[i] = atof(argv[13+i]);
#if IK_VERSION > 54
// for IKFast 56,61
bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
#else
// for IKFast 54
bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
#endif
if( !bSuccess ) {
fprintf(stderr,"Failed to get ik solution\n");
return -1;
}
#if IK_VERSION > 54
// for IKFast 56,61
unsigned int num_of_solutions = (int)solutions.GetNumSolutions();
#else
// for IKFast 54
unsigned int num_of_solutions = (int)vsolutions.size();
#endif
printf("Found %d ik solutions:\n", num_of_solutions );
std::vector<IKREAL_TYPE> solvalues(num_of_joints);
for(std::size_t i = 0; i < num_of_solutions; ++i) {
#if IK_VERSION > 54
// for IKFast 56,61
const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i);
int this_sol_free_params = (int)sol.GetFree().size();
#else
// for IKFast 54
int this_sol_free_params = (int)vsolutions[i].GetFree().size();
#endif
printf("sol%d (free=%d): ", (int)i, this_sol_free_params );
std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params);
#if IK_VERSION > 54
// for IKFast 56,61
sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
#else
// for IKFast 54
vsolutions[i].GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
#endif
for( std::size_t j = 0; j < solvalues.size(); ++j)
printf("%.15f, ", solvalues[j]);
printf("\n");
}
}
else if( argc == 1+12+num_free_parameters+1 ) // ik, given rotation-translation matrix
{
#if IK_VERSION > 54
// for IKFast 56,61
IkSolutionList<IKREAL_TYPE> solutions;
#else
// for IKFast 54
std::vector<IKSolution> vsolutions;
#endif
std::vector<IKREAL_TYPE> vfree(num_free_parameters);
eerot[0] = atof(argv[2]); eerot[1] = atof(argv[3]); eerot[2] = atof(argv[4]); eetrans[0] = atof(argv[5]);
eerot[3] = atof(argv[6]); eerot[4] = atof(argv[7]); eerot[5] = atof(argv[8]); eetrans[1] = atof(argv[9]);
eerot[6] = atof(argv[10]); eerot[7] = atof(argv[11]); eerot[8] = atof(argv[12]); eetrans[2] = atof(argv[13]);
for(std::size_t i = 0; i < vfree.size(); ++i)
vfree[i] = atof(argv[13+i]);
#if IK_VERSION > 54
// for IKFast 56,61
bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
#else
// for IKFast 54
bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
#endif
if( !bSuccess ) {
fprintf(stderr,"Failed to get ik solution\n");
return -1;
}
#if IK_VERSION > 54
// for IKFast 56,61
unsigned int num_of_solutions = (int)solutions.GetNumSolutions();
#else
// for IKFast 54
unsigned int num_of_solutions = (int)vsolutions.size();
#endif
printf("Found %d ik solutions:\n", num_of_solutions );
std::vector<IKREAL_TYPE> solvalues(num_of_joints);
for(std::size_t i = 0; i < num_of_solutions; ++i) {
#if IK_VERSION > 54
// for IKFast 56,61
const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i);
int this_sol_free_params = (int)sol.GetFree().size();
#else
// for IKFast 54
int this_sol_free_params = (int)vsolutions[i].GetFree().size();
#endif
printf("sol%d (free=%d): ", (int)i, this_sol_free_params );
std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params);
#if IK_VERSION > 54
// for IKFast 56,61
sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
#else
// for IKFast 54
vsolutions[i].GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
#endif
for( std::size_t j = 0; j < solvalues.size(); ++j)
printf("%.15f, ", solvalues[j]);
printf("\n");
}
}
else {
printf("\n "
"Usage: \n\n "
" ./compute ik t0 t1 t2 qw qi qj qk free0 ...\n\n "
" Returns the ik solutions given the transformation of the end effector specified by \n "
" a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k). \n "
" There are %d free parameters that have to be specified.\n\n", num_free_parameters );
printf(" \n "
" ./compute 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", num_free_parameters );
return 1;
}
} // endif ik mode
else if (cmd.compare("fk") == 0) // fk mode
{
if( argc != num_of_joints+2 ) {
printf("\n "
"Usage: \n\n "
" ./compute fk j0 j1 ... j%d \n\n"
" Returns the forward kinematic solution given the joint angles (in radians). \n\n", num_of_joints-1 );
return 1;
}
printf("\n\n");
// Put input joint values into array
IKREAL_TYPE joints[num_of_joints];
for (unsigned int i=0; i<num_of_joints; i++)
{
joints[i] = atof(argv[i+2]);
}
#if IK_VERSION > 54
// for IKFast 56,61
ComputeFk(joints, eetrans, eerot); // void return
#else
// for IKFast 54
fk(joints, eetrans, eerot); // void return
#endif
printf("Found fk solution for end frame: \n\n");
printf(" Translation: x: %f y: %f z: %f \n", eetrans[0], eetrans[1], eetrans[2] );
printf("\n");
printf(" Rotation %f %f %f \n", eerot[0], eerot[1], eerot[2] );
printf(" Matrix: %f %f %f \n", eerot[3], eerot[4], eerot[5] );
printf(" %f %f %f \n", eerot[6], eerot[7], eerot[8] );
printf("\n");
// Display equivalent Euler angles
float yaw;
float pitch;
float roll;
if ( eerot[5] > 0.998 || eerot[5] < -0.998 ) { // singularity
yaw = IKatan2( -eerot[6], eerot[0] );
pitch = 0;
} else {
yaw = IKatan2( eerot[2], eerot[8] );
pitch = IKatan2( eerot[3], eerot[4] );
}
roll = IKasin( eerot[5] );
printf(" Euler angles: \n");
printf(" Yaw: %f ", yaw ); printf("(1st: rotation around vertical blue Z-axis in ROS Rviz) \n");
printf(" Pitch: %f \n", pitch );
printf(" Roll: %f \n", roll );
printf("\n");
// Convert rotation matrix to quaternion (Daisuke Miyazaki)
float q0 = ( eerot[0] + eerot[4] + eerot[8] + 1.0f) / 4.0f;
float q1 = ( eerot[0] - eerot[4] - eerot[8] + 1.0f) / 4.0f;
float q2 = (-eerot[0] + eerot[4] - eerot[8] + 1.0f) / 4.0f;
float q3 = (-eerot[0] - eerot[4] + eerot[8] + 1.0f) / 4.0f;
if(q0 < 0.0f) q0 = 0.0f;
if(q1 < 0.0f) q1 = 0.0f;
if(q2 < 0.0f) q2 = 0.0f;
if(q3 < 0.0f) q3 = 0.0f;
q0 = sqrt(q0);
q1 = sqrt(q1);
q2 = sqrt(q2);
q3 = sqrt(q3);
if(q0 >= q1 && q0 >= q2 && q0 >= q3) {
q0 *= +1.0f;
q1 *= SIGN(eerot[7] - eerot[5]);
q2 *= SIGN(eerot[2] - eerot[6]);
q3 *= SIGN(eerot[3] - eerot[1]);
} else if(q1 >= q0 && q1 >= q2 && q1 >= q3) {
q0 *= SIGN(eerot[7] - eerot[5]);
q1 *= +1.0f;
q2 *= SIGN(eerot[3] + eerot[1]);
q3 *= SIGN(eerot[2] + eerot[6]);
} else if(q2 >= q0 && q2 >= q1 && q2 >= q3) {
q0 *= SIGN(eerot[2] - eerot[6]);
q1 *= SIGN(eerot[3] + eerot[1]);
q2 *= +1.0f;
q3 *= SIGN(eerot[7] + eerot[5]);
} else if(q3 >= q0 && q3 >= q1 && q3 >= q2) {
q0 *= SIGN(eerot[3] - eerot[1]);
q1 *= SIGN(eerot[6] + eerot[2]);
q2 *= SIGN(eerot[7] + eerot[5]);
q3 *= +1.0f;
} else {
printf("Error while converting to quaternion! \n");
}
float r = NORM(q0, q1, q2, q3);
q0 /= r;
q1 /= r;
q2 /= r;
q3 /= r;
printf(" Quaternion: %f %f %f %f \n", q0, q1, q2, q3 );
printf(" ");
// print quaternion with convention and +/- signs such that it can be copy-pasted into WolframAlpha.com
printf("%f ", q0);
if (q1 > 0) printf("+ %fi ", q1); else if (q1 < 0) printf("- %fi ", -q1); else printf("+ 0.00000i ");
if (q2 > 0) printf("+ %fj ", q2); else if (q2 < 0) printf("- %fj ", -q2); else printf("+ 0.00000j ");
if (q3 > 0) printf("+ %fk ", q3); else if (q3 < 0) printf("- %fk ", -q3); else printf("+ 0.00000k ");
printf(" (alternate convention) \n");
printf("\n\n");
}
else if (cmd.compare("iktiming") == 0) // generate random ik and check time performance
{
if( argc != 2 ) {
printf("\n "
"Usage: \n\n "
" ./compute iktiming \n\n"
" For fixed number of iterations, generates random joint angles, then \n"
" calculates fk, calculates ik, measures average time taken. \n\n", num_of_joints-1 );
return 1;
}
printf("\n\n");
#if IK_VERSION > 54
// for IKFast 56,61
IkSolutionList<IKREAL_TYPE> solutions;
#else
// for IKFast 54
std::vector<IKSolution> vsolutions;
#endif
std::vector<IKREAL_TYPE> vfree(num_free_parameters);
//for(std::size_t i = 0; i < vfree.size(); ++i)
// vfree[i] = atof(argv[13+i]);
srand( (unsigned)time(0) ); // seed random number generator
float min = -3.14;
float max = 3.14;
float rnd;
IKREAL_TYPE joints[num_of_joints];
timespec start_time, end_time;
unsigned int elapsed_time = 0;
unsigned int sum_time = 0;
#if IK_VERSION > 54
// for IKFast 56,61
unsigned int num_of_tests = 1000000;
#else
// for IKFast 54
unsigned int num_of_tests = 100000;
#endif
for (unsigned int i=0; i < num_of_tests; i++)
{
// Measure avg time for whole process
//clock_gettime(CLOCK_REALTIME, &start_time);
// Put random joint values into array
for (unsigned int i=0; i<num_of_joints; i++)
{
float rnd = (float)rand() / (float)RAND_MAX;
joints[i] = min + rnd * (max - min);
}
/*
printf("Joint angles: ");
for (unsigned int i=0; i<num_of_joints; i++)
{
printf("%f ", joints[i] );
}
printf("\n");
*/
#if IK_VERSION > 54
// for IKFast 56,61
ComputeFk(joints, eetrans, eerot); // void return
#else
// for IKFast 54
fk(joints, eetrans, eerot); // void return
#endif
// Measure avg time for IK
clock_gettime(CLOCK_REALTIME, &start_time);
#if IK_VERSION > 54
// for IKFast 56,61
ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
#else
// for IKFast 54
ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
#endif
/*
#if IK_VERSION > 54
// for IKFast 56,61
unsigned int num_of_solutions = (int)solutions.GetNumSolutions();
#else
// for IKFast 54
unsigned int num_of_solutions = (int)vsolutions.size();
#endif
printf("Found %d ik solutions:\n", num_of_solutions );
*/
clock_gettime(CLOCK_REALTIME, &end_time);
elapsed_time = (unsigned int)(end_time.tv_nsec - start_time.tv_nsec);
sum_time += elapsed_time;
} // endfor
unsigned int avg_time = (unsigned int)sum_time / (unsigned int)num_of_tests;
printf("avg time: %f ms over %d tests \n", (float)avg_time/1000.0, num_of_tests );
return 1;
}
else if (cmd.compare("iktiming2") == 0) // for fixed joint values, check time performance of ik
{
if( argc != 2 ) {
printf("\n "
"Usage: \n\n "
" ./compute iktiming2 \n\n"
" For fixed number of iterations, with one set of joint variables, this \n"
" finds the ik solutions and measures the average time taken. \n\n", num_of_joints-1 );
return 1;
}
printf("\n\n");
#if IK_VERSION > 54
// for IKFast 56,61
IkSolutionList<IKREAL_TYPE> solutions;
#else
// for IKFast 54
std::vector<IKSolution> vsolutions;
#endif
std::vector<IKREAL_TYPE> vfree(num_free_parameters);
//for(std::size_t i = 0; i < vfree.size(); ++i)
// vfree[i] = atof(argv[13+i]);
IKREAL_TYPE joints[num_of_joints];
timespec start_time, end_time;
unsigned int elapsed_time = 0;
unsigned int sum_time = 0;
#if IK_VERSION > 54
// for IKFast 56,61
unsigned int num_of_tests = 1000000;
#else
// for IKFast 54
unsigned int num_of_tests = 100000;
#endif
// fixed rotation-translation matrix corresponding to an unusual robot pose
eerot[0] = 0.002569; eerot[1] = -0.658044; eerot[2] = -0.752975; eetrans[0] = 0.121937;
eerot[3] = 0.001347; eerot[4] = -0.752975; eerot[5] = 0.658048; eetrans[1] = -0.276022;
eerot[6] = -0.999996; eerot[7] = -0.002705; eerot[8] = -0.001047; eetrans[2] = 0.005685;
for (unsigned int i=0; i < num_of_tests; i++)
{
clock_gettime(CLOCK_REALTIME, &start_time);
#if IK_VERSION > 54
// for IKFast 56,61
ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
#else
// for IKFast 54
ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
#endif
/*
#if IK_VERSION > 54
// for IKFast 56,61
unsigned int num_of_solutions = (int)solutions.GetNumSolutions();
#else
// for IKFast 54
unsigned int num_of_solutions = (int)vsolutions.size();
#endif
printf("Found %d ik solutions:\n", num_of_solutions );
*/
clock_gettime(CLOCK_REALTIME, &end_time);
elapsed_time = (unsigned int)(end_time.tv_nsec - start_time.tv_nsec);
sum_time += elapsed_time;
} // endfor
unsigned int avg_time = (unsigned int)sum_time / (unsigned int)num_of_tests;
printf("avg time: %f ms over %d tests \n", (float)avg_time/1000.0, num_of_tests );
return 1;
} else {
printf("\n"
"Usage: \n\n");
printf(" ./compute fk j0 j1 ... j%d \n\n"
" Returns the forward kinematic solution given the joint angles (in radians). \n\n", num_of_joints-1 );
printf("\n"
" ./compute ik t0 t1 t2 qw qi qj qk free0 ... \n\n"
" Returns the ik solutions given the transformation of the end effector specified by \n"
" a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k). \n"
" There are %d free parameters that have to be specified. \n\n", num_free_parameters );
printf(" \n"
" ./compute 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", num_free_parameters );
printf("\n"
" ./compute iktiming \n\n"
" For fixed number of iterations, generates random joint angles, then \n"
" calculates fk, calculates ik, measures average time taken. \n\n", num_of_joints-1 );
printf("\n"
" ./compute iktiming2 \n\n"
" For fixed number of iterations, with one set of joint variables, this \n"
" finds the ik solutions and measures the average time taken. \n\n", num_of_joints-1 );
return 1;
}
return 0;
}
float SIGN(float x) {
return (x >= 0.0f) ? +1.0f : -1.0f;
}
float NORM(float a, float b, float c, float d) {
return sqrt(a * a + b * b + c * c + d * d);
}
Raw
ikfast.h
// -*- coding: utf-8 -*-
// Copyright (C) 2012 Rosen Diankov <rosen.diankov@gmail.com>
//
// 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.
/** \brief Header file for all ikfast c++ files/shared objects.
The ikfast inverse kinematics compiler is part of OpenRAVE.
The file is divided into two sections:
- <b>Common</b> - the abstract classes section that all ikfast share regardless of their settings
- <b>Library Specific</b> - the library-specific definitions, which depends on the precision/settings that the library was compiled with
The defines are as follows, they are also used for the ikfast C++ class:
- IKFAST_HEADER_COMMON - common classes
- IKFAST_HAS_LIBRARY - if defined, will include library-specific functions. by default this is off
- IKFAST_CLIBRARY - Define this linking statically or dynamically to get correct visibility.
- IKFAST_NO_MAIN - Remove the ``main`` function, usually used with IKFAST_CLIBRARY
- IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and other invalid conditions are detected.
- IKFAST_REAL - Use to force a custom real number type for IkReal.
- IKFAST_NAMESPACE - Enclose all functions and classes in this namespace, the ``main`` function is excluded.
*/
#include <vector>
#include <list>
#include <stdexcept>
#ifndef IKFAST_HEADER_COMMON
#define IKFAST_HEADER_COMMON
/// should be the same as ikfast.__version__
#define IKFAST_VERSION 61
namespace ikfast {
/// \brief holds the solution for a single dof
template <typename T>
class IkSingleDOFSolutionBase
{
public:
IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) {
indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1;
}
T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset
signed char freeind; ///< if >= 0, mimics another joint
unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider
unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself
unsigned char indices[5]; ///< 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
};
/// \brief The discrete solutions are returned in this structure.
///
/// Sometimes the joint axes of the robot can align allowing an infinite number of solutions.
/// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its prototype is:
template <typename T>
class IkSolutionBase
{
public:
virtual ~IkSolutionBase() {
}
/// \brief gets a concrete solution
///
/// \param[out] solution the result
/// \param[in] freevalues values for the free parameters \se GetFree
virtual void GetSolution(T* solution, const T* freevalues) const = 0;
/// \brief std::vector version of \ref GetSolution
virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const {
solution.resize(GetDOF());
GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
}
/// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned
///
/// \return vector of indices indicating the free parameters
virtual const std::vector<int>& GetFree() const = 0;
/// \brief the dof of the solution
virtual const int GetDOF() const = 0;
};
/// \brief manages all the solutions
template <typename T>
class IkSolutionListBase
{
public:
virtual ~IkSolutionListBase() {
}
/// \brief add one solution and return its index for later retrieval
///
/// \param vinfos Solution data for each degree of freedom of the manipulator
/// \param vfree If the solution represents an infinite space, holds free parameters of the solution that users can freely set.
virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) = 0;
/// \brief returns the solution pointer
virtual const IkSolutionBase<T>& GetSolution(size_t index) const = 0;
/// \brief returns the number of solutions stored
virtual size_t GetNumSolutions() const = 0;
/// \brief clears all current solutions, note that any memory addresses returned from \ref GetSolution will be invalidated.
virtual void Clear() = 0;
};
/// \brief holds function pointers for all the exported functions of ikfast
template <typename T>
class IkFastFunctions
{
public:
IkFastFunctions() : _ComputeIk(NULL), _ComputeFk(NULL), _GetNumFreeParameters(NULL), _GetFreeParameters(NULL), _GetNumJoints(NULL), _GetIkRealSize(NULL), _GetIkFastVersion(NULL), _GetIkType(NULL), _GetKinematicsHash(NULL) {
}
virtual ~IkFastFunctions() {
}
typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase<T>&);
ComputeIkFn _ComputeIk;
typedef void (*ComputeFkFn)(const T*, T*, T*);
ComputeFkFn _ComputeFk;
typedef int (*GetNumFreeParametersFn)();
GetNumFreeParametersFn _GetNumFreeParameters;
typedef int* (*GetFreeParametersFn)();
GetFreeParametersFn _GetFreeParameters;
typedef int (*GetNumJointsFn)();
GetNumJointsFn _GetNumJoints;
typedef int (*GetIkRealSizeFn)();
GetIkRealSizeFn _GetIkRealSize;
typedef const char* (*GetIkFastVersionFn)();
GetIkFastVersionFn _GetIkFastVersion;
typedef int (*GetIkTypeFn)();
GetIkTypeFn _GetIkType;
typedef const char* (*GetKinematicsHashFn)();
GetKinematicsHashFn _GetKinematicsHash;
};
// Implementations of the abstract classes, user doesn't need to use them
/// \brief Default implementation of \ref IkSolutionBase
template <typename T>
class IkSolution : public IkSolutionBase<T>
{
public:
IkSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) {
_vbasesol = vinfos;
_vfree = vfree;
}
virtual void GetSolution(T* solution, const T* freevalues) const {
for(std::size_t i = 0; i < _vbasesol.size(); ++i) {
if( _vbasesol[i].freeind < 0 )
solution[i] = _vbasesol[i].foffset;
else {
solution[i] = freevalues[_vbasesol[i].freeind]*_vbasesol[i].fmul + _vbasesol[i].foffset;
if( solution[i] > T(3.14159265358979) ) {
solution[i] -= T(6.28318530717959);
}
else if( solution[i] < T(-3.14159265358979) ) {
solution[i] += T(6.28318530717959);
}
}
}
}
virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const {
solution.resize(GetDOF());
GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
}
virtual const std::vector<int>& GetFree() const {
return _vfree;
}
virtual const int GetDOF() const {
return static_cast<int>(_vbasesol.size());
}
virtual void Validate() const {
for(size_t i = 0; i < _vbasesol.size(); ++i) {
if( _vbasesol[i].maxsolutions == (unsigned char)-1) {
throw std::runtime_error("max solutions for joint not initialized");
}
if( _vbasesol[i].maxsolutions > 0 ) {
if( _vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions ) {
throw std::runtime_error("index >= max solutions for joint");
}
if( _vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions ) {
throw std::runtime_error("2nd index >= max solutions for joint");
}
}
}
}
virtual void GetSolutionIndices(std::vector<unsigned int>& v) const {
v.resize(0);
v.push_back(0);
for(int i = (int)_vbasesol.size()-1; i >= 0; --i) {
if( _vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1 ) {
for(size_t j = 0; j < v.size(); ++j) {
v[j] *= _vbasesol[i].maxsolutions;
}
size_t orgsize=v.size();
if( _vbasesol[i].indices[1] != (unsigned char)-1 ) {
for(size_t j = 0; j < orgsize; ++j) {
v.push_back(v[j]+_vbasesol[i].indices[1]);
}
}
if( _vbasesol[i].indices[0] != (unsigned char)-1 ) {
for(size_t j = 0; j < orgsize; ++j) {
v[j] += _vbasesol[i].indices[0];
}
}
}
}
}
std::vector< IkSingleDOFSolutionBase<T> > _vbasesol; ///< solution and their offsets if joints are mimiced
std::vector<int> _vfree;
};
/// \brief Default implementation of \ref IkSolutionListBase
template <typename T>
class IkSolutionList : public IkSolutionListBase<T>
{
public:
virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
{
size_t index = _listsolutions.size();
_listsolutions.push_back(IkSolution<T>(vinfos,vfree));
return index;
}
virtual const IkSolutionBase<T>& GetSolution(size_t index) const
{
if( index >= _listsolutions.size() ) {
throw std::runtime_error("GetSolution index is invalid");
}
typename std::list< IkSolution<T> >::const_iterator it = _listsolutions.begin();
std::advance(it,index);
return *it;
}
virtual size_t GetNumSolutions() const {
return _listsolutions.size();
}
virtual void Clear() {
_listsolutions.clear();
}
protected:
std::list< IkSolution<T> > _listsolutions;
};
}
#endif // OPENRAVE_IKFAST_HEADER
// The following code is dependent on the C++ library linking with.
#ifdef IKFAST_HAS_LIBRARY
// 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
#ifdef IKFAST_NAMESPACE
namespace IKFAST_NAMESPACE {
#endif
#ifdef IKFAST_REAL
typedef IKFAST_REAL IkReal;
#else
typedef double IkReal;
#endif
/** \brief Computes all IK solutions given a end effector coordinates and the free joints.
- ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is the orientation.
- ``eerot``
- For **Transform6D** it is 9 values for the 3x3 rotation matrix.
- For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction.
- For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** the first value represents the angle.
- For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end effector coordinate system.
*/
IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase<IkReal>& solutions);
/// \brief Computes the end effector coordinates given the joint values. This function is used to double check ik.
IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot);
/// \brief returns the number of free parameters users has to set apriori
IKFAST_API int GetNumFreeParameters();
/// \brief the indices of the free parameters indexed by the chain joints
IKFAST_API int* GetFreeParameters();
/// \brief the total number of indices of the chain
IKFAST_API int GetNumJoints();
/// \brief the size in bytes of the configured number type
IKFAST_API int GetIkRealSize();
/// \brief the ikfast version used to generate this file
IKFAST_API const char* GetIkFastVersion();
/// \brief the ik type ID
IKFAST_API int GetIkType();
/// \brief a hash of all the chain values used for double checking that the correct IK is used.
IKFAST_API const char* GetKinematicsHash();
#ifdef IKFAST_NAMESPACE
}
#endif
#endif // IKFAST_HAS_LIBRARY
Raw
output_ikfast61.cpp
/// 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 61 generated on 2013-01-29 02:24:10.307444
/// 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
#define IKFAST_HAS_LIBRARY
#include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
using namespace ikfast;
// check if the included ikfast version matches what this file was compiled with
#define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61);
#include <cmath>
#include <vector>
#include <limits>
#include <algorithm>
#include <complex>
#define IKFAST_STRINGIZE2(s) #s
#define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
#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
// 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
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 ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46,x47,x48,x49,x50,x51,x52,x53,x54,x55,x56,x57,x58;
x0=IKcos(j[0]);
x1=IKcos(j[1]);
x2=IKcos(j[2]);
x3=IKsin(j[1]);
x4=IKsin(j[2]);
x5=IKsin(j[0]);
x6=IKsin(j[3]);
x7=IKcos(j[3]);
x8=IKsin(j[5]);
x9=IKsin(j[4]);
x10=IKcos(j[4]);
x11=IKcos(j[5]);
x12=((IkReal(0.00151567000000000))*(x6));
x13=((IkReal(0.0720009000000000))*(x2));
x14=((IkReal(1.00000000000000))*(x10));
x15=((IkReal(1.00000000000000))*(x0));
x16=((IkReal(0.00151567000000000))*(x4));
x17=((IkReal(1.00000000000000))*(x9));
x18=((IkReal(1.00000000000000))*(x6));
x19=((IkReal(0.270000000000000))*(x3));
x20=((IkReal(0.302000000000000))*(x0));
x21=((IkReal(0.0701403000000000))*(x4));
x22=((IkReal(0.0720009000000000))*(x7));
x23=((IkReal(0.302000000000000))*(x5));
x24=((IkReal(0.000315200000000000))*(x4));
x25=((IkReal(1.00000000000000))*(x5));
x26=((IkReal(1.00000000000000))*(x7));
x27=((IkReal(1.00000000000000))*(x4));
x28=((IkReal(0.0720009000000000))*(x6));
x29=((IkReal(0.000315200000000000))*(x7));
x30=((x1)*(x2));
x31=((x0)*(x1));
x32=((x3)*(x4));
x33=((x1)*(x5));
x34=((x2)*(x3));
x35=((x7)*(x9));
x36=((x1)*(x4));
x37=((x18)*(x5));
x38=((x27)*(x3));
x39=((IkReal(1.00000000000000))*(x30));
x40=((x0)*(x34));
x41=((x25)*(x32));
x42=((x25)*(x30));
x43=((x39)+(((IkReal(-1.00000000000000))*(x38))));
x44=((((IkReal(-1.00000000000000))*(x1)*(x27)))+(((IkReal(-1.00000000000000))*(x34))));
x45=((x43)*(x6));
x46=((x44)*(x9));
x47=((x15)*(((((IkReal(-1.00000000000000))*(x32)))+(x30))));
x48=((x42)+(((IkReal(-1.00000000000000))*(x41))));
x49=((x10)*(x43)*(x7));
x50=((x15)*(((x36)+(x34))));
x51=((IkReal(-1.00000000000000))*(x50));
x52=((x25)*(((x36)+(x34))));
x53=((IkReal(-1.00000000000000))*(x52));
x54=((x51)*(x7));
x55=((((IkReal(-1.00000000000000))*(x26)*(x5)))+(((x50)*(x6))));
x56=((x37)+(((IkReal(-1.00000000000000))*(x26)*(x51))));
x57=((((IkReal(-1.00000000000000))*(x26)*(x53)))+(((IkReal(-1.00000000000000))*(x0)*(x18))));
x58=((((IkReal(-1.00000000000000))*(x18)*(x52)))+(((IkReal(-1.00000000000000))*(x0)*(x26))));
eerot[0]=((((x55)*(x8)))+(((x11)*(((((IkReal(-1.00000000000000))*(x17)*(x47)))+(((IkReal(-1.00000000000000))*(x14)*(x56))))))));
eerot[1]=((((x8)*(((((x47)*(x9)))+(((x10)*(x56)))))))+(((x11)*(x55))));
eerot[2]=((((x9)*(((x54)+(((IkReal(-1.00000000000000))*(x37)))))))+(((x10)*(x47))));
IkReal x59=((IkReal(1.00000000000000))*(x5));
eetrans[0]=((IkReal(0.0420395000000000))+(((x21)*(x31)))+(((IkReal(0.0701403000000000))*(x40)))+(((IkReal(0.0100933000000000))*(x5)))+(((x12)*(x5)))+(((IkReal(-1.00000000000000))*(x20)*(x32)))+(((x9)*(((((IkReal(-1.00000000000000))*(x28)*(x59)))+(((x22)*(x51)))))))+(((x6)*(((((x24)*(x31)))+(((IkReal(0.000315200000000000))*(x40)))))))+(((x20)*(x30)))+(((IkReal(-1.00000000000000))*(x29)*(x59)))+(((x10)*(((((x13)*(x31)))+(((IkReal(-0.0720009000000000))*(x0)*(x32)))))))+(((x7)*(((((x16)*(x31)))+(((IkReal(0.00151567000000000))*(x40)))))))+(((x0)*(x19))));
eerot[3]=((((x45)*(x8)))+(((x11)*(((((IkReal(-1.00000000000000))*(x49)))+(((IkReal(-1.00000000000000))*(x46))))))));
eerot[4]=((((x11)*(x45)))+(((x8)*(((x49)+(x46))))));
eerot[5]=((((x35)*(((x38)+(((IkReal(-1.00000000000000))*(x39)))))))+(((x10)*(x44))));
IkReal x60=((IkReal(1.00000000000000))*(x3));
eetrans[1]=((IkReal(0.210360400000000))+(((IkReal(0.270000000000000))*(x1)))+(((IkReal(-1.00000000000000))*(x21)*(x60)))+(((x35)*(((((IkReal(0.0720009000000000))*(x32)))+(((IkReal(-1.00000000000000))*(x1)*(x13)))))))+(((IkReal(-0.302000000000000))*(x36)))+(((x10)*(((((IkReal(-1.00000000000000))*(x13)*(x60)))+(((IkReal(-0.0720009000000000))*(x36)))))))+(((x6)*(((((IkReal(0.000315200000000000))*(x30)))+(((IkReal(-1.00000000000000))*(x24)*(x60)))))))+(((IkReal(0.0701403000000000))*(x30)))+(((IkReal(-0.302000000000000))*(x34)))+(((x7)*(((((IkReal(0.00151567000000000))*(x30)))+(((IkReal(-1.00000000000000))*(x16)*(x60))))))));
eerot[6]=((((x11)*(((((x10)*(x57)))+(((x48)*(x9)))))))+(((x58)*(x8))));
eerot[7]=((((x8)*(((((IkReal(-1.00000000000000))*(x14)*(x57)))+(((IkReal(-1.00000000000000))*(x17)*(x48)))))))+(((x11)*(x58))));
eerot[8]=((((x10)*(((x41)+(((IkReal(-1.00000000000000))*(x42)))))))+(((x57)*(x9))));
IkReal x61=((IkReal(1.00000000000000))*(x0));
IkReal x62=((x34)*(x5));
IkReal x63=((IkReal(1.00000000000000))*(x33));
eetrans[2]=((((IkReal(-1.00000000000000))*(x19)*(x5)))+(((x7)*(((((IkReal(-0.00151567000000000))*(x62)))+(((IkReal(-1.00000000000000))*(x16)*(x63)))))))+(((IkReal(-1.00000000000000))*(x21)*(x63)))+(((x9)*(((((IkReal(-1.00000000000000))*(x22)*(x53)))+(((IkReal(-1.00000000000000))*(x28)*(x61)))))))+(((IkReal(-1.00000000000000))*(x29)*(x61)))+(((x10)*(((((IkReal(0.0720009000000000))*(x32)*(x5)))+(((IkReal(-1.00000000000000))*(x13)*(x63)))))))+(((x23)*(x32)))+(((IkReal(-0.0701403000000000))*(x62)))+(((IkReal(-1.00000000000000))*(x23)*(x30)))+(((x0)*(x12)))+(((IkReal(0.0100933000000000))*(x0)))+(((x6)*(((((IkReal(-0.000315200000000000))*(x62)))+(((IkReal(-1.00000000000000))*(x24)*(x63))))))));
}
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 j6,cj6,sj6,htj6,j7,cj7,sj7,htj7,j8,cj8,sj8,htj8,j9,cj9,sj9,htj9,j10,cj10,sj10,htj10,j11,cj11,sj11,htj11,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 _ij6[2], _nj6,_ij7[2], _nj7,_ij8[2], _nj8,_ij9[2], _nj9,_ij10[2], _nj10,_ij11[2], _nj11;
bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
j6=numeric_limits<IkReal>::quiet_NaN(); _ij6[0] = -1; _ij6[1] = -1; _nj6 = -1; j7=numeric_limits<IkReal>::quiet_NaN(); _ij7[0] = -1; _ij7[1] = -1; _nj7 = -1; j8=numeric_limits<IkReal>::quiet_NaN(); _ij8[0] = -1; _ij8[1] = -1; _nj8 = -1; j9=numeric_limits<IkReal>::quiet_NaN(); _ij9[0] = -1; _ij9[1] = -1; _nj9 = -1; j10=numeric_limits<IkReal>::quiet_NaN(); _ij10[0] = -1; _ij10[1] = -1; _nj10 = -1; j11=numeric_limits<IkReal>::quiet_NaN(); _ij11[0] = -1; _ij11[1] = -1; _nj11 = -1;
for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
solutions.Clear();
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=r01;
new_r01=((IkReal(-1.00000000000000))*(r00));
new_r02=r02;
new_px=((IkReal(-0.0420395000000000))+(((IkReal(-0.0720009000000000))*(r02)))+(px));
new_r10=((IkReal(-1.00000000000000))*(r21));
new_r11=r20;
new_r12=((IkReal(-1.00000000000000))*(r22));
new_py=((((IkReal(-1.00000000000000))*(pz)))+(((IkReal(0.0720009000000000))*(r22))));
new_r20=r11;
new_r21=((IkReal(-1.00000000000000))*(r10));
new_r22=r12;
new_pz=((IkReal(-0.210360400000000))+(((IkReal(-0.0720009000000000))*(r12)))+(py));
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))+((pz)*(pz))+((py)*(py)));
npx=((((py)*(r10)))+(((pz)*(r20)))+(((px)*(r00))));
npy=((((px)*(r01)))+(((pz)*(r21)))+(((py)*(r11))));
npz=((((py)*(r12)))+(((pz)*(r22)))+(((px)*(r02))));
rxp0_0=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10))));
rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00))));
rxp0_2=((((py)*(r00)))+(((IkReal(-1.00000000000000))*(px)*(r10))));
rxp1_0=((((pz)*(r11)))+(((IkReal(-1.00000000000000))*(py)*(r21))));
rxp1_1=((((IkReal(-1.00000000000000))*(pz)*(r01)))+(((px)*(r21))));
rxp1_2=((((py)*(r01)))+(((IkReal(-1.00000000000000))*(px)*(r11))));
rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12))));
rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02))));
rxp2_2=((((py)*(r02)))+(((IkReal(-1.00000000000000))*(px)*(r12))));
IkReal op[162], zeror[48];
int numroots;
IkReal x64=((IkReal(0.000315200000000000))*(r10));
IkReal x65=((IkReal(0.00501877483443709))*(rxp0_1));
IkReal x66=((IkReal(0.00135506920529801))*(r00));
IkReal x67=((IkReal(0.00271013841059603))*(r01));
IkReal x68=((IkReal(0.000630400000000000))*(r11));
IkReal x69=((IkReal(0.0100375496688742))*(rxp1_1));
IkReal x70=((IkReal(0.270000000000000))*(r20));
IkReal x71=((IkReal(0.232252649006623))*(rxp0_1));
IkReal x72=((IkReal(0.0627082152317881))*(r00));
IkReal x73=((IkReal(0.0100933000000000))*(r10));
IkReal x74=((IkReal(0.540000000000000))*(r00));
IkReal x75=((IkReal(0.125416430463576))*(r20));
IkReal x76=((IkReal(0.464505298013245))*(rxp1_1));
IkReal x77=((IkReal(2.00000000000000))*(npy));
IkReal x78=((IkReal(0.540000000000000))*(r21));
IkReal x79=((IkReal(0.125416430463576))*(r01));
IkReal x80=((IkReal(0.0201866000000000))*(r11));
IkReal x81=((IkReal(1.00000000000000))*(npx));
IkReal x82=((IkReal(0.0201866000000000))*(npz));
IkReal x83=((IkReal(0.000630400000000000))*(rxp1_1));
IkReal x84=((IkReal(0.000170208000000000))*(r01));
IkReal x85=((IkReal(0.0233231397823911))*(r12));
IkReal x86=((IkReal(0.000915464680000000))*(r11));
IkReal x87=((IkReal(0.540000000000000))*(rxp2_0));
IkReal x88=((IkReal(0.00545038200000000))*(r22));
IkReal x89=((IkReal(0.0109007640000000))*(r02));
IkReal x90=((IkReal(0.000340416000000000))*(r21));
IkReal x91=((IkReal(1.08000000000000))*(rxp2_2));
IkReal x92=((IkReal(0.000340416000000000))*(r00));
IkReal x93=((IkReal(0.00126080000000000))*(rxp0_1));
IkReal x94=((IkReal(0.00183092936000000))*(r10));
IkReal x95=((IkReal(0.101000800000000))*(npx));
IkReal x96=((IkReal(0.0221436384351489))*(r10));
IkReal x97=((IkReal(0.00743227067472318))*(r00));
IkReal x98=((IkReal(0.0272702160000000))*(r20));
IkReal x99=((IkReal(0.540000000000000))*(rxp0_0));
IkReal x100=((IkReal(0.0275269284249007))*(rxp0_1));
IkReal x101=((IkReal(0.124000400000000))*(py));
IkReal x102=((IkReal(0.0545404320000000))*(r00));
IkReal x103=((IkReal(1.08000000000000))*(rxp1_0));
IkReal x104=((IkReal(0.0148645413494464))*(r01));
IkReal x105=((IkReal(0.0545404320000000))*(r21));
IkReal x106=((IkReal(0.202001600000000))*(npy));
IkReal x107=((IkReal(0.0442872768702978))*(r11));
IkReal x108=((IkReal(0.0550538568498013))*(rxp1_1));
IkReal x109=((pp)*(r10));
IkReal x110=((IkReal(0.540000000000000))*(rxp1_0));
IkReal x111=((IkReal(0.0201866000000000))*(npy));
IkReal x112=((IkReal(0.0233231397823911))*(r11));
IkReal x113=((IkReal(0.000630400000000000))*(rxp2_1));
IkReal x114=((IkReal(0.000170208000000000))*(r02));
IkReal x115=((IkReal(0.00545038200000000))*(r21));
IkReal x116=((IkReal(0.000915464680000000))*(r12));
IkReal x117=((IkReal(0.000340416000000000))*(r22));
IkReal x118=((IkReal(1.08000000000000))*(rxp1_2));
IkReal x119=((IkReal(0.0109007640000000))*(r01));
IkReal x120=((pp)*(r11));
IkReal x121=((IkReal(1.08000000000000))*(rxp0_0));
IkReal x122=((IkReal(0.0466462795647822))*(r10));
IkReal x123=((IkReal(0.0109007640000000))*(r20));
IkReal x124=((IkReal(0.0403732000000000))*(npx));
IkReal x125=((IkReal(0.0334801080000000))*(r20));
IkReal x126=((IkReal(0.0280447970345762))*(rxp0_1));
IkReal x127=((IkReal(0.540000000000000))*(pz));
IkReal x128=((IkReal(0.00757209519933556))*(r00));
IkReal x129=((IkReal(0.101000800000000))*(py));
IkReal x130=((IkReal(1.00000000000000))*(pp));
IkReal x131=((IkReal(0.124000400000000))*(npx));
IkReal x132=((IkReal(0.00142599406584200))*(r10));
IkReal x133=((IkReal(1.08000000000000))*(px));
IkReal x134=((IkReal(0.0669602160000000))*(r00));
IkReal x135=((IkReal(0.0151441903986711))*(r20));
IkReal x136=((IkReal(0.0560895940691523))*(rxp1_1));
IkReal x137=((IkReal(0.0151441903986711))*(r01));
IkReal x138=((IkReal(0.00285198813168400))*(r11));
IkReal x139=((IkReal(0.248000800000000))*(npy));
IkReal x140=((IkReal(0.0669602160000000))*(r21));
IkReal x141=((IkReal(0.0100375496688742))*(rxp0_0));
IkReal x142=((IkReal(0.000630400000000000))*(r00));
IkReal x143=((IkReal(2.00000000000000))*(px));
IkReal x144=((IkReal(0.00271013841059603))*(r10));
IkReal x145=((IkReal(0.0200750993377483))*(rxp1_0));
IkReal x146=((IkReal(0.00542027682119205))*(r11));
IkReal x147=((IkReal(0.00126080000000000))*(r01));
IkReal x148=((IkReal(0.464505298013245))*(rxp0_0));
IkReal x149=((IkReal(0.125416430463576))*(r10));
IkReal x150=((IkReal(0.0201866000000000))*(r00));
IkReal x151=((IkReal(0.0403732000000000))*(r01));
IkReal x152=((IkReal(0.250832860927152))*(r11));
IkReal x153=((IkReal(0.929010596026490))*(rxp1_0));
IkReal x154=((IkReal(1.08000000000000))*(rxp2_1));
IkReal x155=((IkReal(0.000340416000000000))*(r11));
IkReal x156=((IkReal(0.0466462795647822))*(r02));
IkReal x157=((IkReal(0.00126080000000000))*(rxp1_0));
IkReal x158=((IkReal(0.00183092936000000))*(r01));
IkReal x159=((IkReal(0.00252160000000000))*(rxp0_0));
IkReal x160=((IkReal(0.000680832000000000))*(r10));
IkReal x161=((IkReal(0.00366185872000000))*(r00));
IkReal x162=((IkReal(0.0550538568498013))*(rxp0_0));
IkReal x163=((IkReal(0.248000800000000))*(px));
IkReal x164=((IkReal(0.0442872768702978))*(r00));
IkReal x165=((IkReal(1.08000000000000))*(rxp0_1));
IkReal x166=((IkReal(0.0148645413494464))*(r10));
IkReal x167=((IkReal(0.0885745537405956))*(r01));
IkReal x168=((IkReal(0.0297290826988927))*(r11));
IkReal x169=((IkReal(0.110107713699603))*(rxp1_0));
IkReal x170=((IkReal(2.16000000000000))*(rxp1_1));
IkReal x171=((IkReal(0.00126080000000000))*(rxp2_0));
IkReal x172=((IkReal(1.08000000000000))*(rxp1_1));
IkReal x173=((IkReal(0.00183092936000000))*(r02));
IkReal x174=((IkReal(0.0466462795647822))*(r01));
IkReal x175=((IkReal(0.000340416000000000))*(r12));
IkReal x176=((IkReal(0.0932925591295644))*(r00));
IkReal x177=((IkReal(2.16000000000000))*(rxp0_1));
IkReal x178=((IkReal(0.00285198813168400))*(r00));
IkReal x179=((IkReal(0.0151441903986711))*(r10));
IkReal x180=((IkReal(0.202001600000000))*(px));
IkReal x181=((IkReal(0.0560895940691523))*(rxp0_0));
IkReal x182=((IkReal(2.16000000000000))*(py));
IkReal x183=((IkReal(0.133920432000000))*(r10));
IkReal x184=((IkReal(0.00570397626336800))*(r01));
IkReal x185=((IkReal(0.0302883807973423))*(r11));
IkReal x186=((IkReal(0.112179188138305))*(rxp1_0));
IkReal x187=((IkReal(1.00000000000000))*(py));
IkReal x188=((IkReal(0.00542027682119205))*(r21));
IkReal x189=((IkReal(0.00271013841059603))*(r20));
IkReal x190=((IkReal(1.08000000000000))*(r01));
IkReal x191=((IkReal(0.250832860927152))*(r21));
IkReal x192=((pp)*(r12));
IkReal x193=((IkReal(0.000680832000000000))*(r20));
IkReal x194=((IkReal(1.08000000000000))*(rxp0_2));
IkReal x195=((IkReal(0.0148645413494464))*(r20));
IkReal x196=((IkReal(2.16000000000000))*(rxp1_2));
IkReal x197=((IkReal(0.0297290826988927))*(r21));
IkReal x198=((IkReal(0.109080864000000))*(r01));
IkReal x199=((IkReal(2.16000000000000))*(rxp0_2));
IkReal x200=((IkReal(0.0218015280000000))*(r00));
IkReal x201=((IkReal(0.133920432000000))*(r01));
IkReal x202=((IkReal(0.0302883807973423))*(r21));
IkReal x203=((IkReal(4.00000000000000))*(px));
IkReal x204=((IkReal(2.00000000000000))*(pp));
IkReal x205=((IkReal(2.00000000000000))*(py));
IkReal x206=((IkReal(4.00000000000000))*(py));
IkReal x207=((IkReal(4.00000000000000))*(pp));
IkReal x208=((IkReal(8.00000000000000))*(px));
IkReal x209=((npz)*(x205));
IkReal x210=((r12)*(x130));
IkReal x211=((npx)*(x205));
IkReal x212=((IkReal(1.00000000000000))*(x109));
IkReal x213=((IkReal(2.00000000000000))*(x120));
IkReal x214=((npy)*(x206));
IkReal x215=((py)*(x77));
IkReal x216=((IkReal(2.00000000000000))*(x109));
IkReal x217=((npx)*(x206));
IkReal x218=((IkReal(1.00000000000000))*(x120));
IkReal x219=((r02)*(x204));
IkReal x220=((npz)*(x203));
IkReal x221=((IkReal(-0.0218015280000000))*(r12));
IkReal x222=((npx)*(x203));
IkReal x223=((r00)*(x204));
IkReal x224=((npy)*(x208));
IkReal x225=((r01)*(x207));
IkReal x226=((r01)*(x204));
IkReal x227=((npy)*(x203));
IkReal x228=((npx)*(x208));
IkReal x229=((r00)*(x207));
IkReal x230=((IkReal(-0.00271013841059603))*(r20));
IkReal x231=((IkReal(-0.00100075587183800))+(x95));
IkReal x232=((IkReal(-0.00100075587183800))+(x98));
IkReal x233=((IkReal(0.0100933000000000))+(x64));
IkReal x234=((IkReal(0.0100933000000000))+(x65));
IkReal x235=((npx)+(x73));
IkReal x236=((IkReal(0.0243082794441289))+(x129));
IkReal x237=((IkReal(-0.000315200000000000))+(x71));
IkReal x238=((x66)+(py));
IkReal x239=((x195)+(x194));
IkReal x240=((x150)+(x149));
IkReal x241=((x148)+(x149));
IkReal x242=((x67)+(x69));
IkReal x243=((x67)+(x68));
IkReal x244=((x75)+(x74));
IkReal x245=((x93)+(x94));
IkReal x246=((x178)+(x180));
IkReal x247=((x166)+(x165));
IkReal x248=((x111)+(x110));
IkReal x249=((x113)+(x116));
IkReal x250=((x128)+(x131));
IkReal x251=((x192)+(x82));
IkReal x252=((x109)+(x101));
IkReal x253=((x99)+(x97));
IkReal x254=((x73)+(x81));
IkReal x255=((x137)+(x140));
IkReal x256=((x104)+(x103));
IkReal x257=((x139)+(x138));
IkReal x258=((x135)+(x133));
IkReal x259=((x120)+(x115));
IkReal x260=((x128)+(x130));
IkReal x261=((x126)+(x125));
IkReal x262=((x127)+(x132));
IkReal x263=((x76)+(x78));
IkReal x264=((x83)+(x86));
IkReal x265=((x170)+(x168));
IkReal x266=((x171)+(x173));
IkReal x267=((x89)+(x90));
IkReal x268=((x142)+(x143));
IkReal x269=((x115)+(x114));
IkReal x270=((x101)+(x95));
IkReal x271=((x65)+(x187));
IkReal x272=((x119)+(x118));
IkReal x273=((x71)+(x70));
IkReal x274=((x141)+(x144));
IkReal x275=((x101)+(x98));
IkReal x276=((x120)+(x114));
IkReal x277=((x131)+(x130));
IkReal x278=((x64)+(x66));
IkReal x279=((x159)+(x161));
IkReal x280=((x82)+(x87));
IkReal x281=((x157)+(x158));
IkReal x282=((x179)+(x181));
IkReal x283=((x197)+(x196));
IkReal x284=((x88)+(x87));
IkReal x285=((x126)+(x127));
IkReal x286=((x88)+(x82));
IkReal x287=((x73)+(x70));
IkReal x288=((x77)+(x80));
IkReal x289=((x73)+(x72));
IkReal x290=((x136)+(x139));
IkReal x291=((x156)+(x220));
IkReal x292=((x210)+(x88));
IkReal x293=((x163)+(x223));
IkReal x294=((x215)+(x112));
IkReal x295=((x216)+(x124));
IkReal x296=((IkReal(-0.000315200000000000))+(x72)+(x70));
IkReal x297=((x212)+(x98));
IkReal x298=((x210)+(x87));
IkReal x299=((x174)+(x227));
IkReal x300=((x213)+(x106));
IkReal x301=((x209)+(x85));
IkReal x302=((x155)+(x219));
IkReal x303=((x217)+(x122));
IkReal x304=((x175)+(x226));
IkReal x305=((x176)+(x228));
IkReal x306=((x245)+(x92));
IkReal x307=((x239)+(x102));
IkReal x308=((x125)+(x129)+(x132));
IkReal x309=((x284)+(x84));
IkReal x310=((x303)+(x121));
IkReal x311=((x162)+(x164)+(x222));
IkReal x312=((x169)+(x167)+(x224));
IkReal x313=((x214)+(x108)+(x107));
IkReal x314=((x211)+(x100)+(x96));
IkReal x315=((x313)+(x105));
IkReal x316=((x253)+(x314));
op[0]=((x238)+(x234)+(((IkReal(-1.00000000000000))*(x64))));
op[1]=((x68)+(((IkReal(-1.00000000000000))*(x242))));
op[2]=((x233)+(((IkReal(-1.00000000000000))*(x66)))+(py)+(((IkReal(-1.00000000000000))*(x65))));
op[3]=x230;
op[4]=x188;
op[5]=x189;
op[6]=((x234)+(((IkReal(-1.00000000000000))*(x278)))+(py));
op[7]=((x243)+(((IkReal(-1.00000000000000))*(x69))));
op[8]=((x238)+(x233)+(((IkReal(-1.00000000000000))*(x65))));
op[9]=((IkReal(-0.000315200000000000))+(x235)+(((IkReal(-1.00000000000000))*(x72)))+(((IkReal(-1.00000000000000))*(x273))));
op[10]=((((IkReal(-1.00000000000000))*(x288)))+(x79)+(x263));
op[11]=((x237)+(((IkReal(-1.00000000000000))*(x254)))+(x72)+(x70));
op[12]=((((IkReal(-1.00000000000000))*(x74)))+(x75));
op[13]=((((IkReal(-1.00000000000000))*(x191)))+(x190));
op[14]=((((IkReal(-1.00000000000000))*(x75)))+(x74));
op[15]=((x235)+(x296)+(((IkReal(-1.00000000000000))*(x71))));
op[16]=((((IkReal(-1.00000000000000))*(x288)))+(((IkReal(-1.00000000000000))*(x79)))+(x76)+(((IkReal(-1.00000000000000))*(x78))));
op[17]=((((IkReal(-1.00000000000000))*(x70)))+(x237)+(((IkReal(-1.00000000000000))*(x254)))+(((IkReal(-1.00000000000000))*(x72))));
op[18]=((x280)+(((IkReal(-1.00000000000000))*(x292)))+(x301)+(((IkReal(-1.00000000000000))*(x84)))+(((IkReal(-1.00000000000000))*(x264))));
op[19]=((IkReal(-1.00000000000000))*(x306));
op[20]=((x280)+(((IkReal(-1.00000000000000))*(x292)))+(x301)+(x264)+(x84));
op[21]=((((IkReal(-1.00000000000000))*(x89)))+(((IkReal(-1.00000000000000))*(x91)))+(x90));
op[22]=x193;
op[23]=((((IkReal(-1.00000000000000))*(x267)))+(((IkReal(-1.00000000000000))*(x91))));
op[24]=((x286)+(x301)+(((IkReal(-1.00000000000000))*(x298)))+(((IkReal(-1.00000000000000))*(x264)))+(x84));
op[25]=((((IkReal(-1.00000000000000))*(x245)))+(x92));
op[26]=((x286)+(x301)+(((IkReal(-1.00000000000000))*(x298)))+(((IkReal(-1.00000000000000))*(x84)))+(x264));
op[27]=((((IkReal(-1.00000000000000))*(x212)))+(x232)+(x316)+(((IkReal(-1.00000000000000))*(x270))));
op[28]=((((IkReal(-1.00000000000000))*(x256)))+(x300)+(((IkReal(-1.00000000000000))*(x315))));
op[29]=((x231)+(((IkReal(-1.00000000000000))*(x316)))+(((IkReal(-1.00000000000000))*(x275)))+(x109));
op[30]=((((IkReal(-1.00000000000000))*(x239)))+(x102));
op[31]=((x283)+(((IkReal(-1.00000000000000))*(x198))));
op[32]=((((IkReal(-1.00000000000000))*(x102)))+(x239));
op[33]=((IkReal(-0.00100075587183800))+(x314)+(((IkReal(-1.00000000000000))*(x297)))+(((IkReal(-1.00000000000000))*(x270)))+(((IkReal(-1.00000000000000))*(x253))));
op[34]=((x300)+(x256)+(x105)+(((IkReal(-1.00000000000000))*(x313))));
op[35]=((((IkReal(-1.00000000000000))*(x101)))+(x231)+(x253)+(((IkReal(-1.00000000000000))*(x314)))+(x109)+(x98));
op[36]=((((IkReal(-1.00000000000000))*(x114)))+(((IkReal(-1.00000000000000))*(x249)))+(x259)+(((IkReal(-1.00000000000000))*(x248)))+(((IkReal(-1.00000000000000))*(x294))));
op[37]=((x216)+(x123)+(((IkReal(-1.00000000000000))*(x124)))+(((IkReal(-1.00000000000000))*(x310))));
op[38]=((((IkReal(-1.00000000000000))*(x249)))+(((IkReal(-1.00000000000000))*(x218)))+(((IkReal(-1.00000000000000))*(x269)))+(x294)+(x248));
op[39]=((x272)+(x117));
op[40]=((x199)+(x200));
op[41]=((((IkReal(-1.00000000000000))*(x272)))+(x117));
op[42]=((((IkReal(-1.00000000000000))*(x249)))+(((IkReal(-1.00000000000000))*(x111)))+(x276)+(((IkReal(-1.00000000000000))*(x115)))+(x110)+(((IkReal(-1.00000000000000))*(x294))));
op[43]=((x216)+(x121)+(((IkReal(-1.00000000000000))*(x124)))+(((IkReal(-1.00000000000000))*(x303)))+(((IkReal(-1.00000000000000))*(x123))));
op[44]=((((IkReal(-1.00000000000000))*(x249)))+(((IkReal(-1.00000000000000))*(x218)))+(x294)+(((IkReal(-1.00000000000000))*(x110)))+(x111)+(x269));
op[45]=((((IkReal(-1.00000000000000))*(x260)))+(x236)+(((IkReal(-1.00000000000000))*(x261)))+(x131)+(x262));
op[46]=((x255)+(((IkReal(-1.00000000000000))*(x257)))+(x136));
op[47]=((x236)+(x127)+(x128)+(((IkReal(-1.00000000000000))*(x277)))+(((IkReal(-1.00000000000000))*(x132)))+(x261));
op[48]=((x258)+(((IkReal(-1.00000000000000))*(x134))));
op[49]=((((IkReal(-1.00000000000000))*(x202)))+(x201));
op[50]=((((IkReal(-1.00000000000000))*(x135)))+(x134)+(x133));
op[51]=((x236)+(x250)+(x125)+(((IkReal(-1.00000000000000))*(x285)))+(x132)+(((IkReal(-1.00000000000000))*(x130))));
op[52]=((((IkReal(-1.00000000000000))*(x255)))+(((IkReal(-1.00000000000000))*(x257)))+(x136));
op[53]=((x236)+(x126)+(((IkReal(-1.00000000000000))*(x125)))+(((IkReal(-1.00000000000000))*(x262)))+(((IkReal(-1.00000000000000))*(x250)))+(((IkReal(-1.00000000000000))*(x130))));
op[54]=((((IkReal(-1.00000000000000))*(x141)))+(x142)+(x144)+(((IkReal(-1.00000000000000))*(x143))));
op[55]=((((IkReal(-1.00000000000000))*(x146)))+(((IkReal(-1.00000000000000))*(x147)))+(x145));
op[56]=((((IkReal(-1.00000000000000))*(x268)))+(x141)+(((IkReal(-1.00000000000000))*(x144))));
op[57]=IkReal(0);
op[58]=IkReal(0);
op[59]=IkReal(0);
op[60]=((((IkReal(-1.00000000000000))*(x274)))+(x142)+(((IkReal(-1.00000000000000))*(x143))));
op[61]=((((IkReal(-1.00000000000000))*(x147)))+(x145)+(x146));
op[62]=((((IkReal(-1.00000000000000))*(x268)))+(x274));
op[63]=((((IkReal(-1.00000000000000))*(x240)))+(x148));
op[64]=((x152)+(x151)+(((IkReal(-1.00000000000000))*(x153))));
op[65]=((((IkReal(-1.00000000000000))*(x148)))+(x240));
op[66]=((IkReal(-1.08000000000000))*(r10));
op[67]=((IkReal(2.16000000000000))*(r11));
op[68]=((IkReal(1.08000000000000))*(r10));
op[69]=((x241)+(((IkReal(-1.00000000000000))*(x150))));
op[70]=((x151)+(((IkReal(-1.00000000000000))*(x153)))+(((IkReal(-1.00000000000000))*(x152))));
op[71]=((x150)+(((IkReal(-1.00000000000000))*(x241))));
op[72]=((((IkReal(-1.00000000000000))*(x155)))+(x154)+(x281)+(((IkReal(-1.00000000000000))*(x291)))+(x219));
op[73]=((((IkReal(-1.00000000000000))*(x160)))+(x279));
op[74]=((x154)+(((IkReal(-1.00000000000000))*(x291)))+(x302)+(((IkReal(-1.00000000000000))*(x281))));
op[75]=x221;
op[76]=IkReal(0);
op[77]=x221;
op[78]=((((IkReal(-1.00000000000000))*(x154)))+(x281)+(((IkReal(-1.00000000000000))*(x291)))+(x302));
op[79]=((x160)+(x279));
op[80]=((((IkReal(-1.00000000000000))*(x155)))+(((IkReal(-1.00000000000000))*(x154)))+(((IkReal(-1.00000000000000))*(x291)))+(x219)+(((IkReal(-1.00000000000000))*(x281))));
op[81]=((x293)+(x247)+(((IkReal(-1.00000000000000))*(x311))));
op[82]=((((IkReal(-1.00000000000000))*(x225)))+(x312)+(((IkReal(-1.00000000000000))*(x265))));
op[83]=((((IkReal(-1.00000000000000))*(x247)))+(((IkReal(-1.00000000000000))*(x223)))+(x311)+(x163));
op[84]=((IkReal(0.109080864000000))*(r10));
op[85]=((IkReal(-0.218161728000000))*(r11));
op[86]=((IkReal(-0.109080864000000))*(r10));
op[87]=((((IkReal(-1.00000000000000))*(x247)))+(x293)+(((IkReal(-1.00000000000000))*(x311))));
op[88]=((((IkReal(-1.00000000000000))*(x225)))+(x312)+(x265));
op[89]=((((IkReal(-1.00000000000000))*(x223)))+(x311)+(x163)+(x247));
op[90]=((((IkReal(-1.00000000000000))*(x304)))+(x299)+(((IkReal(-1.00000000000000))*(x172)))+(x266));
op[91]=((x305)+(((IkReal(-1.00000000000000))*(x229)))+(((IkReal(-1.00000000000000))*(x177))));
op[92]=((((IkReal(-1.00000000000000))*(x175)))+(x172)+(x226)+(((IkReal(-1.00000000000000))*(x299)))+(x266));
op[93]=((IkReal(0.0218015280000000))*(r11));
op[94]=((IkReal(0.0436030560000000))*(r10));
op[95]=((IkReal(-0.0218015280000000))*(r11));
op[96]=((x172)+(x175)+(x299)+(x266)+(((IkReal(-1.00000000000000))*(x226))));
op[97]=((x177)+(x305)+(((IkReal(-1.00000000000000))*(x229))));
op[98]=((x304)+(((IkReal(-1.00000000000000))*(x172)))+(((IkReal(-1.00000000000000))*(x299)))+(x266));
op[99]=((((IkReal(-1.00000000000000))*(x179)))+(x181)+(((IkReal(-1.00000000000000))*(x246))));
op[100]=((x184)+(x185)+(((IkReal(-1.00000000000000))*(x186))));
op[101]=((x179)+(x178)+(((IkReal(-1.00000000000000))*(x181)))+(((IkReal(-1.00000000000000))*(x180))));
op[102]=((((IkReal(-1.00000000000000))*(x183)))+(x182));
op[103]=((IkReal(0.267840864000000))*(r11));
op[104]=((x182)+(x183));
op[105]=((x282)+(((IkReal(-1.00000000000000))*(x246))));
op[106]=((((IkReal(-1.00000000000000))*(x185)))+(x184)+(((IkReal(-1.00000000000000))*(x186))));
op[107]=((x178)+(((IkReal(-1.00000000000000))*(x282)))+(((IkReal(-1.00000000000000))*(x180))));
op[108]=((x233)+(((IkReal(-1.00000000000000))*(x66)))+(((IkReal(-1.00000000000000))*(x271))));
op[109]=((x242)+(((IkReal(-1.00000000000000))*(x68))));
op[110]=((x66)+(x234)+(((IkReal(-1.00000000000000))*(x187)))+(((IkReal(-1.00000000000000))*(x64))));
op[111]=x230;
op[112]=x188;
op[113]=x189;
op[114]=((x66)+(x233)+(((IkReal(-1.00000000000000))*(x271))));
op[115]=((x69)+(((IkReal(-1.00000000000000))*(x243))));
op[116]=((x234)+(((IkReal(-1.00000000000000))*(x278)))+(((IkReal(-1.00000000000000))*(x187))));
op[117]=((x237)+(((IkReal(-1.00000000000000))*(x287)))+(npx)+(x72));
op[118]=((((IkReal(-1.00000000000000))*(x79)))+(((IkReal(-1.00000000000000))*(x77)))+(((IkReal(-1.00000000000000))*(x76)))+(x78)+(x80));
op[119]=((IkReal(-0.000315200000000000))+(x287)+(((IkReal(-1.00000000000000))*(x81)))+(((IkReal(-1.00000000000000))*(x72)))+(((IkReal(-1.00000000000000))*(x71))));
op[120]=x244;
op[121]=((((IkReal(-1.00000000000000))*(x191)))+(((IkReal(-1.00000000000000))*(x190))));
op[122]=((IkReal(-1.00000000000000))*(x244));
op[123]=((((IkReal(-1.00000000000000))*(x289)))+(x237)+(npx)+(x70));
op[124]=((((IkReal(-1.00000000000000))*(x263)))+(((IkReal(-1.00000000000000))*(x77)))+(x79)+(x80));
op[125]=((IkReal(-0.000315200000000000))+(x289)+(((IkReal(-1.00000000000000))*(x81)))+(((IkReal(-1.00000000000000))*(x273))));
op[126]=((((IkReal(-1.00000000000000))*(x301)))+(x251)+(((IkReal(-1.00000000000000))*(x284)))+(x264)+(x84));
op[127]=x306;
op[128]=((((IkReal(-1.00000000000000))*(x301)))+(x251)+(((IkReal(-1.00000000000000))*(x309)))+(((IkReal(-1.00000000000000))*(x264))));
op[129]=((((IkReal(-1.00000000000000))*(x91)))+(x267));
op[130]=x193;
op[131]=((((IkReal(-1.00000000000000))*(x90)))+(((IkReal(-1.00000000000000))*(x91)))+(x89));
op[132]=((((IkReal(-1.00000000000000))*(x301)))+(x284)+(x251)+(((IkReal(-1.00000000000000))*(x84)))+(x264));
op[133]=((((IkReal(-1.00000000000000))*(x92)))+(x245));
op[134]=((((IkReal(-1.00000000000000))*(x301)))+(x309)+(x251)+(((IkReal(-1.00000000000000))*(x264))));
op[135]=((x232)+(x252)+(((IkReal(-1.00000000000000))*(x316)))+(((IkReal(-1.00000000000000))*(x95))));
op[136]=((x256)+(x313)+(((IkReal(-1.00000000000000))*(x213)))+(((IkReal(-1.00000000000000))*(x105)))+(x106));
op[137]=((x231)+(x316)+(((IkReal(-1.00000000000000))*(x297)))+(x101));
op[138]=((IkReal(-1.00000000000000))*(x307));
op[139]=((x283)+(x198));
op[140]=x307;
op[141]=((IkReal(-0.00100075587183800))+(x252)+(x253)+(((IkReal(-1.00000000000000))*(x314)))+(((IkReal(-1.00000000000000))*(x95)))+(((IkReal(-1.00000000000000))*(x98))));
op[142]=((((IkReal(-1.00000000000000))*(x256)))+(x315)+(((IkReal(-1.00000000000000))*(x213)))+(x106));
op[143]=((((IkReal(-1.00000000000000))*(x212)))+(x231)+(x314)+(x275)+(((IkReal(-1.00000000000000))*(x253))));
op[144]=((((IkReal(-1.00000000000000))*(x218)))+(x294)+(((IkReal(-1.00000000000000))*(x111)))+(x249)+(x110)+(x269));
op[145]=((x123)+(x310)+(((IkReal(-1.00000000000000))*(x295))));
op[146]=((x276)+(x249)+(((IkReal(-1.00000000000000))*(x110)))+(((IkReal(-1.00000000000000))*(x115)))+(x111)+(((IkReal(-1.00000000000000))*(x294))));
op[147]=((x117)+(x118)+(((IkReal(-1.00000000000000))*(x119))));
op[148]=((((IkReal(-1.00000000000000))*(x200)))+(x199));
op[149]=((((IkReal(-1.00000000000000))*(x118)))+(x117)+(x119));
op[150]=((((IkReal(-1.00000000000000))*(x218)))+(((IkReal(-1.00000000000000))*(x269)))+(x294)+(((IkReal(-1.00000000000000))*(x248)))+(x249));
op[151]=((x303)+(((IkReal(-1.00000000000000))*(x295)))+(((IkReal(-1.00000000000000))*(x123)))+(((IkReal(-1.00000000000000))*(x121))));
op[152]=((((IkReal(-1.00000000000000))*(x114)))+(x259)+(x249)+(x248)+(((IkReal(-1.00000000000000))*(x294))));
op[153]=((IkReal(0.0243082794441289))+(x285)+(x250)+(((IkReal(-1.00000000000000))*(x308)))+(((IkReal(-1.00000000000000))*(x130))));
op[154]=((((IkReal(-1.00000000000000))*(x290)))+(((IkReal(-1.00000000000000))*(x137)))+(x138)+(x140));
op[155]=((IkReal(0.0243082794441289))+(((IkReal(-1.00000000000000))*(x129)))+(((IkReal(-1.00000000000000))*(x126)))+(x125)+(((IkReal(-1.00000000000000))*(x250)))+(x262)+(((IkReal(-1.00000000000000))*(x130))));
op[156]=((x135)+(x134)+(((IkReal(-1.00000000000000))*(x133))));
op[157]=((((IkReal(-1.00000000000000))*(x202)))+(((IkReal(-1.00000000000000))*(x201))));
op[158]=((((IkReal(-1.00000000000000))*(x258)))+(((IkReal(-1.00000000000000))*(x134))));
op[159]=((IkReal(0.0243082794441289))+(((IkReal(-1.00000000000000))*(x260)))+(((IkReal(-1.00000000000000))*(x129)))+(((IkReal(-1.00000000000000))*(x262)))+(x131)+(x261));
op[160]=((((IkReal(-1.00000000000000))*(x290)))+(((IkReal(-1.00000000000000))*(x140)))+(x137)+(x138));
op[161]=((IkReal(0.0243082794441289))+(((IkReal(-1.00000000000000))*(x129)))+(((IkReal(-1.00000000000000))*(x127)))+(x128)+(((IkReal(-1.00000000000000))*(x277)))+(((IkReal(-1.00000000000000))*(x261)))+(x132));
solvedialyticpoly12qep(op,zeror,numroots);
IkReal j6array[16], cj6array[16], sj6array[16], j7array[16], cj7array[16], sj7array[16], j11array[16], cj11array[16], sj11array[16];
int numsolutions = 0;
for(int ij6 = 0; ij6 < numroots; ij6 += 3)
{
IkReal htj6 = zeror[ij6+0], htj7 = zeror[ij6+1], htj11 = zeror[ij6+2];
j6array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj6)));
j7array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj7)));
j11array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj11)));
IkReal x317=(htj6)*(htj6);
IkReal x318=(htj7)*(htj7);
IkReal x319=(htj11)*(htj11);
cj6array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x317))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x317)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x317))))));
cj7array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x318))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x318)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x318))))));
cj11array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x319))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x319)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x319))))));
sj6array[numsolutions]=((IkReal(2.00000000000000))*(htj6)*(((IKabs(((IkReal(1.00000000000000))+((htj6)*(htj6)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj6)*(htj6))))):(IkReal)1.0e30)));
sj7array[numsolutions]=((IkReal(2.00000000000000))*(htj7)*(((IKabs(((IkReal(1.00000000000000))+((htj7)*(htj7)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj7)*(htj7))))):(IkReal)1.0e30)));
sj11array[numsolutions]=((IkReal(2.00000000000000))*(htj11)*(((IKabs(((IkReal(1.00000000000000))+((htj11)*(htj11)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj11)*(htj11))))):(IkReal)1.0e30)));
if( j6array[numsolutions] > IKPI )
{
j6array[numsolutions]-=IK2PI;
}
else if( j6array[numsolutions] < -IKPI )
{
j6array[numsolutions]+=IK2PI;
}
if( j7array[numsolutions] > IKPI )
{
j7array[numsolutions]-=IK2PI;
}
else if( j7array[numsolutions] < -IKPI )
{
j7array[numsolutions]+=IK2PI;
}
if( j11array[numsolutions] > IKPI )
{
j11array[numsolutions]-=IK2PI;
}
else if( j11array[numsolutions] < -IKPI )
{
j11array[numsolutions]+=IK2PI;
}
numsolutions++;
}
bool j6valid[16]={true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true};
_nj6 = 16;
_nj7 = 1;
_nj11 = 1;
for(int ij6 = 0; ij6 < numsolutions; ++ij6)
{
if( !j6valid[ij6] )
{
continue;
}
_ij6[0] = ij6; _ij6[1] = -1;
_ij7[0] = 0; _ij7[1] = -1;
_ij11[0] = 0; _ij11[1] = -1;
for(int iij6 = ij6+1; iij6 < numsolutions; ++iij6)
{
if( !j6valid[iij6] ) { continue; }
if( IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(cj7array[ij6]-cj7array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij6]-sj7array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(cj11array[ij6]-cj11array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij6]-sj11array[iij6]) < IKFAST_SOLUTION_THRESH && 1 )
{
j6valid[iij6]=false; _ij6[1] = iij6; _ij7[1] = 0; _ij11[1] = 0; break;
}
}
j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
j7 = j7array[ij6]; cj7 = cj7array[ij6]; sj7 = sj7array[ij6];
j11 = j11array[ij6]; cj11 = cj11array[ij6]; sj11 = sj11array[ij6];
{
IkReal j9array[1], cj9array[1], sj9array[1];
bool j9valid[1]={false};
_nj9 = 1;
IkReal x320=((cj11)*(r00)*(sj6));
IkReal x321=((r01)*(sj11)*(sj6));
IkReal x322=((cj11)*(cj6)*(r10));
IkReal x323=((cj6)*(r11)*(sj11));
if( IKabs(((IkReal(-6.65929918781793))+(((IkReal(0.207960835801989))*(x321)))+(((IkReal(0.207960835801989))*(x322)))+(((IkReal(-659.774225260116))*(cj6)*(py)))+(((IkReal(-0.207960835801989))*(x320)))+(((IkReal(-0.207960835801989))*(x323)))+(((IkReal(659.774225260116))*(px)*(sj6))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(x323)))+(x321)+(x322)+(((IkReal(-1.00000000000000))*(x320))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-6.65929918781793))+(((IkReal(0.207960835801989))*(x321)))+(((IkReal(0.207960835801989))*(x322)))+(((IkReal(-659.774225260116))*(cj6)*(py)))+(((IkReal(-0.207960835801989))*(x320)))+(((IkReal(-0.207960835801989))*(x323)))+(((IkReal(659.774225260116))*(px)*(sj6)))))+IKsqr(((((IkReal(-1.00000000000000))*(x323)))+(x321)+(x322)+(((IkReal(-1.00000000000000))*(x320)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j9array[0]=IKatan2(((IkReal(-6.65929918781793))+(((IkReal(0.207960835801989))*(x321)))+(((IkReal(0.207960835801989))*(x322)))+(((IkReal(-659.774225260116))*(cj6)*(py)))+(((IkReal(-0.207960835801989))*(x320)))+(((IkReal(-0.207960835801989))*(x323)))+(((IkReal(659.774225260116))*(px)*(sj6)))), ((((IkReal(-1.00000000000000))*(x323)))+(x321)+(x322)+(((IkReal(-1.00000000000000))*(x320)))));
sj9array[0]=IKsin(j9array[0]);
cj9array[0]=IKcos(j9array[0]);
if( j9array[0] > IKPI )
{
j9array[0]-=IK2PI;
}
else if( j9array[0] < -IKPI )
{ j9array[0]+=IK2PI;
}
j9valid[0] = true;
for(int ij9 = 0; ij9 < 1; ++ij9)
{
if( !j9valid[ij9] )
{
continue;
}
_ij9[0] = ij9; _ij9[1] = -1;
for(int iij9 = ij9+1; iij9 < 1; ++iij9)
{
if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
{
j9valid[iij9]=false; _ij9[1] = iij9; break;
}
}
j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
{
IkReal evalcond[2];
IkReal x324=IKcos(j9);
IkReal x325=((IkReal(0.0620002000000000))*(sj11));
IkReal x326=((r01)*(sj6));
IkReal x327=((cj6)*(r11));
IkReal x328=((cj11)*(cj6)*(r10));
IkReal x329=((cj11)*(r00)*(sj6));
evalcond[0]=((((IkReal(-1.00000000000000))*(sj11)*(x326)))+(((IkReal(-1.00000000000000))*(x328)))+(x324)+(x329)+(((sj11)*(x327))));
evalcond[1]=((IkReal(-0.0100933000000000))+(((IkReal(-0.0620002000000000))*(x329)))+(((px)*(sj6)))+(((IkReal(0.0620002000000000))*(x328)))+(((IkReal(-0.0616850000000000))*(x324)))+(((x325)*(x326)))+(((IkReal(-1.00000000000000))*(cj6)*(py)))+(((IkReal(-1.00000000000000))*(x325)*(x327)))+(((IkReal(-0.00151567000000000))*(IKsin(j9)))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
{
continue;
}
}
{
IkReal dummyeval[1];
IkReal gconst1;
gconst1=IKsign(sj9);
dummyeval[0]=sj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
IkReal gconst0;
gconst0=IKsign(sj9);
dummyeval[0]=sj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[2];
dummyeval[0]=((((IkReal(-46.2767620920121))*(sj9)))+(((IkReal(40.6981730851703))*((sj9)*(sj9))))+(((IkReal(-1.00000000000000))*(cj9)*(sj9))));
dummyeval[1]=sj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 || IKabs(dummyeval[1]) < 0.0000010000000000 )
{
{
IkReal evalcond[7];
IkReal x330=((IkReal(1.00000000000000))*(cj11));
IkReal x331=((r01)*(sj6));
IkReal x332=((cj6)*(sj7));
IkReal x333=((r01)*(sj11));
IkReal x334=((IkReal(1.00000000000000))*(cj6));
IkReal x335=((IkReal(0.0620002000000000))*(sj11));
IkReal x336=((cj11)*(r10));
IkReal x337=((r11)*(sj11));
IkReal x338=((sj6)*(sj7));
IkReal x339=((r21)*(sj11));
IkReal x340=((r00)*(sj6));
IkReal x341=((cj7)*(sj6));
IkReal x342=((cj6)*(r10));
IkReal x343=((IkReal(1.00000000000000))*(sj11));
IkReal x344=((cj11)*(cj6));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j9)), IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x334))));
evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x330)*(x342)))+(((cj11)*(x340)))+(((cj6)*(x337)))+(((IkReal(-1.00000000000000))*(x331)*(x343))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x330)*(x331)))+(((IkReal(-1.00000000000000))*(x340)*(x343)))+(((sj11)*(x342)))+(((r11)*(x344))));
evalcond[4]=((IkReal(-0.0717783000000000))+(((px)*(sj6)))+(((x331)*(x335)))+(((IkReal(-1.00000000000000))*(py)*(x334)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x335)))+(((IkReal(0.0620002000000000))*(cj6)*(x336)))+(((IkReal(-0.0620002000000000))*(cj11)*(x340))));
evalcond[5]=((((x337)*(x338)))+(((IkReal(-1.00000000000000))*(r10)*(x330)*(x338)))+(((cj7)*(x339)))+(((x332)*(x333)))+(((IkReal(-1.00000000000000))*(r00)*(x330)*(x332)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x330))));
evalcond[6]=((((IkReal(-1.00000000000000))*(r20)*(sj7)*(x330)))+(((x336)*(x341)))+(((IkReal(-1.00000000000000))*(cj7)*(x333)*(x334)))+(((IkReal(-1.00000000000000))*(x337)*(x341)))+(((sj7)*(x339)))+(((cj7)*(r00)*(x344))));
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 j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x345=((IkReal(0.0461153212112420))*(cj11));
IkReal x346=((cj6)*(sj7));
IkReal x347=((cj7)*(r20));
IkReal x348=((IkReal(3.13477742929877))*(px));
IkReal x349=((IkReal(0.194356827572010))*(cj11));
IkReal x350=((IkReal(3.13477742929877))*(pz));
IkReal x351=((py)*(sj6));
IkReal x352=((IkReal(0.743793104074535))*(sj7));
IkReal x353=((r11)*(sj6));
IkReal x354=((r20)*(sj7));
IkReal x355=((cj6)*(r01));
IkReal x356=((cj6)*(cj7));
IkReal x357=((IkReal(0.743793104074535))*(px));
IkReal x358=((IkReal(0.743793104074535))*(cj7));
IkReal x359=((r10)*(sj6));
IkReal x360=((IkReal(0.194356827572010))*(sj11));
IkReal x361=((IkReal(0.0461153212112420))*(sj11));
IkReal x362=((r00)*(x356));
IkReal x363=((cj7)*(x360));
IkReal x364=((cj7)*(x361));
IkReal x365=((r21)*(sj11)*(sj7));
if( IKabs(((IkReal(0.846389905910668))+(((IkReal(-1.00000000000000))*(x346)*(x348)))+(((IkReal(-3.13477742929877))*(sj7)*(x351)))+(((x356)*(x357)))+(((r00)*(x346)*(x349)))+(((IkReal(-1.00000000000000))*(cj7)*(x350)))+(((IkReal(-1.00000000000000))*(r01)*(x346)*(x360)))+(((IkReal(-1.00000000000000))*(pz)*(x352)))+(((x355)*(x364)))+(((IkReal(-1.00000000000000))*(cj7)*(x345)*(x359)))+(((IkReal(-1.00000000000000))*(sj7)*(x353)*(x360)))+(((x347)*(x349)))+(((x351)*(x358)))+(((sj7)*(x349)*(x359)))+(((IkReal(-1.00000000000000))*(x345)*(x362)))+(((x353)*(x364)))+(((IkReal(-1.00000000000000))*(r21)*(x363)))+(((IkReal(-1.00000000000000))*(r21)*(sj7)*(x361)))+(((x345)*(x354))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.200824138100125))+(((x353)*(x363)))+(((pz)*(x358)))+(((x348)*(x356)))+(((x349)*(x354)))+(((x355)*(x363)))+(((IkReal(-1.00000000000000))*(sj7)*(x350)))+(((r21)*(x364)))+(((IkReal(-1.00000000000000))*(r21)*(sj7)*(x360)))+(((IkReal(-1.00000000000000))*(sj7)*(x345)*(x359)))+(((x351)*(x352)))+(((IkReal(-1.00000000000000))*(r00)*(x345)*(x346)))+(((sj7)*(x353)*(x361)))+(((IkReal(3.13477742929877))*(cj7)*(x351)))+(((r01)*(x346)*(x361)))+(((x346)*(x357)))+(((IkReal(-1.00000000000000))*(x345)*(x347)))+(((IkReal(-1.00000000000000))*(cj7)*(x349)*(x359)))+(((IkReal(-1.00000000000000))*(x349)*(x362))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.846389905910668))+(((IkReal(-1.00000000000000))*(x346)*(x348)))+(((IkReal(-3.13477742929877))*(sj7)*(x351)))+(((x356)*(x357)))+(((r00)*(x346)*(x349)))+(((IkReal(-1.00000000000000))*(cj7)*(x350)))+(((IkReal(-1.00000000000000))*(r01)*(x346)*(x360)))+(((IkReal(-1.00000000000000))*(pz)*(x352)))+(((x355)*(x364)))+(((IkReal(-1.00000000000000))*(cj7)*(x345)*(x359)))+(((IkReal(-1.00000000000000))*(sj7)*(x353)*(x360)))+(((x347)*(x349)))+(((x351)*(x358)))+(((sj7)*(x349)*(x359)))+(((IkReal(-1.00000000000000))*(x345)*(x362)))+(((x353)*(x364)))+(((IkReal(-1.00000000000000))*(r21)*(x363)))+(((IkReal(-1.00000000000000))*(r21)*(sj7)*(x361)))+(((x345)*(x354)))))+IKsqr(((IkReal(-0.200824138100125))+(((x353)*(x363)))+(((pz)*(x358)))+(((x348)*(x356)))+(((x349)*(x354)))+(((x355)*(x363)))+(((IkReal(-1.00000000000000))*(sj7)*(x350)))+(((r21)*(x364)))+(((IkReal(-1.00000000000000))*(r21)*(sj7)*(x360)))+(((IkReal(-1.00000000000000))*(sj7)*(x345)*(x359)))+(((x351)*(x352)))+(((IkReal(-1.00000000000000))*(r00)*(x345)*(x346)))+(((sj7)*(x353)*(x361)))+(((IkReal(3.13477742929877))*(cj7)*(x351)))+(((r01)*(x346)*(x361)))+(((x346)*(x357)))+(((IkReal(-1.00000000000000))*(x345)*(x347)))+(((IkReal(-1.00000000000000))*(cj7)*(x349)*(x359)))+(((IkReal(-1.00000000000000))*(x349)*(x362)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((IkReal(0.846389905910668))+(((IkReal(-1.00000000000000))*(x346)*(x348)))+(((IkReal(-3.13477742929877))*(sj7)*(x351)))+(((x356)*(x357)))+(((r00)*(x346)*(x349)))+(((IkReal(-1.00000000000000))*(cj7)*(x350)))+(((IkReal(-1.00000000000000))*(r01)*(x346)*(x360)))+(((IkReal(-1.00000000000000))*(pz)*(x352)))+(((x355)*(x364)))+(((IkReal(-1.00000000000000))*(cj7)*(x345)*(x359)))+(((IkReal(-1.00000000000000))*(sj7)*(x353)*(x360)))+(((x347)*(x349)))+(((x351)*(x358)))+(((sj7)*(x349)*(x359)))+(((IkReal(-1.00000000000000))*(x345)*(x362)))+(((x353)*(x364)))+(((IkReal(-1.00000000000000))*(r21)*(x363)))+(((IkReal(-1.00000000000000))*(r21)*(sj7)*(x361)))+(((x345)*(x354)))), ((IkReal(-0.200824138100125))+(((x353)*(x363)))+(((pz)*(x358)))+(((x348)*(x356)))+(((x349)*(x354)))+(((x355)*(x363)))+(((IkReal(-1.00000000000000))*(sj7)*(x350)))+(((r21)*(x364)))+(((IkReal(-1.00000000000000))*(r21)*(sj7)*(x360)))+(((IkReal(-1.00000000000000))*(sj7)*(x345)*(x359)))+(((x351)*(x352)))+(((IkReal(-1.00000000000000))*(r00)*(x345)*(x346)))+(((sj7)*(x353)*(x361)))+(((IkReal(3.13477742929877))*(cj7)*(x351)))+(((r01)*(x346)*(x361)))+(((x346)*(x357)))+(((IkReal(-1.00000000000000))*(x345)*(x347)))+(((IkReal(-1.00000000000000))*(cj7)*(x349)*(x359)))+(((IkReal(-1.00000000000000))*(x349)*(x362)))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[2];
IkReal x366=IKcos(j8);
IkReal x367=IKsin(j8);
IkReal x368=((IkReal(0.0620002000000000))*(sj11));
IkReal x369=((cj6)*(sj7));
IkReal x370=((IkReal(0.0620002000000000))*(cj11));
IkReal x371=((cj7)*(sj6));
IkReal x372=((IkReal(1.00000000000000))*(pz));
IkReal x373=((sj6)*(sj7));
IkReal x374=((cj6)*(cj7));
evalcond[0]=((((px)*(x374)))+(((IkReal(-0.0716559700000000))*(x367)))+(((IkReal(-1.00000000000000))*(r21)*(sj7)*(x368)))+(((r20)*(sj7)*(x370)))+(((py)*(x371)))+(((IkReal(-1.00000000000000))*(r10)*(x370)*(x371)))+(((IkReal(-1.00000000000000))*(sj7)*(x372)))+(((r11)*(x368)*(x371)))+(((IkReal(-1.00000000000000))*(r00)*(x370)*(x374)))+(((r01)*(x368)*(x374)))+(((IkReal(-0.302000000000000))*(x366))));
evalcond[1]=((IkReal(0.270000000000000))+(((r00)*(x369)*(x370)))+(((IkReal(0.0716559700000000))*(x366)))+(((cj7)*(r20)*(x370)))+(((IkReal(-1.00000000000000))*(px)*(x369)))+(((IkReal(-0.302000000000000))*(x367)))+(((IkReal(-1.00000000000000))*(cj7)*(x372)))+(((IkReal(-1.00000000000000))*(cj7)*(r21)*(x368)))+(((r10)*(x370)*(x373)))+(((IkReal(-1.00000000000000))*(r11)*(x368)*(x373)))+(((IkReal(-1.00000000000000))*(py)*(x373)))+(((IkReal(-1.00000000000000))*(r01)*(x368)*(x369))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
{
continue;
}
}
{
IkReal dummyeval[1];
IkReal gconst18;
gconst18=IKsign((((cj8)*(cj8))+((sj8)*(sj8))));
dummyeval[0]=(((cj8)*(cj8))+((sj8)*(sj8)));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
IkReal gconst19;
gconst19=IKsign(((((IkReal(-1.00000000000000))*((sj8)*(sj8))))+(((IkReal(-1.00000000000000))*((cj8)*(cj8))))));
dummyeval[0]=((((IkReal(-1.00000000000000))*((sj8)*(sj8))))+(((IkReal(-1.00000000000000))*((cj8)*(cj8)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
continue;
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x375=((sj7)*(sj8));
IkReal x376=((cj11)*(r21));
IkReal x377=((cj6)*(r00));
IkReal x378=((cj8)*(sj7));
IkReal x379=((r10)*(sj6));
IkReal x380=((IkReal(1.00000000000000))*(cj8));
IkReal x381=((cj7)*(sj11));
IkReal x382=((r20)*(sj11));
IkReal x383=((cj6)*(cj7));
IkReal x384=((cj11)*(r01));
IkReal x385=((IkReal(1.00000000000000))*(sj8));
IkReal x386=((cj7)*(r12)*(sj6));
IkReal x387=((x381)*(x385));
IkReal x388=((cj11)*(cj7)*(r11)*(sj6));
if( IKabs(((gconst19)*(((((r02)*(sj8)*(x383)))+(((IkReal(-1.00000000000000))*(r22)*(x375)))+(((IkReal(-1.00000000000000))*(x380)*(x383)*(x384)))+(((sj8)*(x386)))+(((IkReal(-1.00000000000000))*(x379)*(x380)*(x381)))+(((IkReal(-1.00000000000000))*(x377)*(x380)*(x381)))+(((x376)*(x378)))+(((x378)*(x382)))+(((IkReal(-1.00000000000000))*(x380)*(x388))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst19)*(((((IkReal(-1.00000000000000))*(x379)*(x387)))+(((x375)*(x382)))+(((IkReal(-1.00000000000000))*(x385)*(x388)))+(((IkReal(-1.00000000000000))*(x380)*(x386)))+(((r22)*(x378)))+(((x375)*(x376)))+(((IkReal(-1.00000000000000))*(x377)*(x387)))+(((IkReal(-1.00000000000000))*(x383)*(x384)*(x385)))+(((IkReal(-1.00000000000000))*(r02)*(x380)*(x383))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst19)*(((((r02)*(sj8)*(x383)))+(((IkReal(-1.00000000000000))*(r22)*(x375)))+(((IkReal(-1.00000000000000))*(x380)*(x383)*(x384)))+(((sj8)*(x386)))+(((IkReal(-1.00000000000000))*(x379)*(x380)*(x381)))+(((IkReal(-1.00000000000000))*(x377)*(x380)*(x381)))+(((x376)*(x378)))+(((x378)*(x382)))+(((IkReal(-1.00000000000000))*(x380)*(x388)))))), ((gconst19)*(((((IkReal(-1.00000000000000))*(x379)*(x387)))+(((x375)*(x382)))+(((IkReal(-1.00000000000000))*(x385)*(x388)))+(((IkReal(-1.00000000000000))*(x380)*(x386)))+(((r22)*(x378)))+(((x375)*(x376)))+(((IkReal(-1.00000000000000))*(x377)*(x387)))+(((IkReal(-1.00000000000000))*(x383)*(x384)*(x385)))+(((IkReal(-1.00000000000000))*(r02)*(x380)*(x383)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x389=IKcos(j10);
IkReal x390=IKsin(j10);
IkReal x391=((IkReal(1.00000000000000))*(cj7));
IkReal x392=((cj6)*(r02));
IkReal x393=((IkReal(1.00000000000000))*(sj7));
IkReal x394=((r20)*(sj11));
IkReal x395=((cj11)*(r21));
IkReal x396=((r12)*(sj6));
IkReal x397=((sj8)*(x390));
IkReal x398=((cj8)*(x390));
IkReal x399=((r10)*(sj11)*(sj6));
IkReal x400=((IkReal(1.00000000000000))*(x389));
IkReal x401=((cj11)*(cj6)*(r01));
IkReal x402=((cj11)*(r11)*(sj6));
IkReal x403=((cj6)*(r00)*(sj11));
IkReal x404=((cj8)*(x400));
evalcond[0]=((((IkReal(-1.00000000000000))*(x404)))+(((cj7)*(x392)))+(((cj7)*(x396)))+(((IkReal(-1.00000000000000))*(r22)*(x393)))+(x397));
evalcond[1]=((((IkReal(-1.00000000000000))*(sj8)*(x400)))+(((IkReal(-1.00000000000000))*(x392)*(x393)))+(((IkReal(-1.00000000000000))*(x393)*(x396)))+(((IkReal(-1.00000000000000))*(r22)*(x391)))+(((IkReal(-1.00000000000000))*(x398))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x404)))+(((cj7)*(x394)))+(((sj7)*(x399)))+(((sj7)*(x403)))+(((sj7)*(x401)))+(((cj7)*(x395)))+(x397)+(((sj7)*(x402))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x391)*(x399)))+(((IkReal(-1.00000000000000))*(x391)*(x401)))+(((IkReal(-1.00000000000000))*(x391)*(x402)))+(((sj7)*(x395)))+(((sj7)*(x394)))+(((sj8)*(x389)))+(x398)+(((IkReal(-1.00000000000000))*(x391)*(x403))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x405=((cj7)*(cj8));
IkReal x406=((r12)*(sj6));
IkReal x407=((cj6)*(r02));
IkReal x408=((IkReal(1.00000000000000))*(r22));
IkReal x409=((sj7)*(sj8));
IkReal x410=((cj7)*(sj8));
IkReal x411=((IkReal(1.00000000000000))*(cj8)*(sj7));
if( IKabs(((gconst18)*(((((IkReal(-1.00000000000000))*(x407)*(x411)))+(((IkReal(-1.00000000000000))*(x406)*(x410)))+(((IkReal(-1.00000000000000))*(x407)*(x410)))+(((IkReal(-1.00000000000000))*(x405)*(x408)))+(((IkReal(-1.00000000000000))*(x406)*(x411)))+(((r22)*(x409))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst18)*(((((IkReal(-1.00000000000000))*(x408)*(x410)))+(((x405)*(x406)))+(((IkReal(-1.00000000000000))*(x406)*(x409)))+(((x405)*(x407)))+(((IkReal(-1.00000000000000))*(x407)*(x409)))+(((IkReal(-1.00000000000000))*(cj8)*(sj7)*(x408))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst18)*(((((IkReal(-1.00000000000000))*(x407)*(x411)))+(((IkReal(-1.00000000000000))*(x406)*(x410)))+(((IkReal(-1.00000000000000))*(x407)*(x410)))+(((IkReal(-1.00000000000000))*(x405)*(x408)))+(((IkReal(-1.00000000000000))*(x406)*(x411)))+(((r22)*(x409)))))), ((gconst18)*(((((IkReal(-1.00000000000000))*(x408)*(x410)))+(((x405)*(x406)))+(((IkReal(-1.00000000000000))*(x406)*(x409)))+(((x405)*(x407)))+(((IkReal(-1.00000000000000))*(x407)*(x409)))+(((IkReal(-1.00000000000000))*(cj8)*(sj7)*(x408)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x412=IKcos(j10);
IkReal x413=IKsin(j10);
IkReal x414=((IkReal(1.00000000000000))*(cj7));
IkReal x415=((cj6)*(r02));
IkReal x416=((IkReal(1.00000000000000))*(sj7));
IkReal x417=((r20)*(sj11));
IkReal x418=((cj11)*(r21));
IkReal x419=((r12)*(sj6));
IkReal x420=((sj8)*(x413));
IkReal x421=((cj8)*(x413));
IkReal x422=((r10)*(sj11)*(sj6));
IkReal x423=((IkReal(1.00000000000000))*(x412));
IkReal x424=((cj11)*(cj6)*(r01));
IkReal x425=((cj11)*(r11)*(sj6));
IkReal x426=((cj6)*(r00)*(sj11));
IkReal x427=((cj8)*(x423));
evalcond[0]=((x420)+(((cj7)*(x415)))+(((IkReal(-1.00000000000000))*(x427)))+(((cj7)*(x419)))+(((IkReal(-1.00000000000000))*(r22)*(x416))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x415)*(x416)))+(((IkReal(-1.00000000000000))*(sj8)*(x423)))+(((IkReal(-1.00000000000000))*(r22)*(x414)))+(((IkReal(-1.00000000000000))*(x416)*(x419)))+(((IkReal(-1.00000000000000))*(x421))));
evalcond[2]=((((sj7)*(x424)))+(x420)+(((sj7)*(x426)))+(((cj7)*(x418)))+(((sj7)*(x422)))+(((IkReal(-1.00000000000000))*(x427)))+(((sj7)*(x425)))+(((cj7)*(x417))));
evalcond[3]=((((sj7)*(x418)))+(((IkReal(-1.00000000000000))*(x414)*(x426)))+(((IkReal(-1.00000000000000))*(x414)*(x422)))+(x421)+(((sj8)*(x412)))+(((sj7)*(x417)))+(((IkReal(-1.00000000000000))*(x414)*(x424)))+(((IkReal(-1.00000000000000))*(x414)*(x425))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
}
}
} else
{
IkReal x428=((IkReal(1.00000000000000))*(cj11));
IkReal x429=((r01)*(sj6));
IkReal x430=((cj6)*(sj7));
IkReal x431=((r01)*(sj11));
IkReal x432=((IkReal(1.00000000000000))*(cj6));
IkReal x433=((IkReal(0.0620002000000000))*(sj11));
IkReal x434=((cj11)*(r10));
IkReal x435=((r11)*(sj11));
IkReal x436=((sj6)*(sj7));
IkReal x437=((r21)*(sj11));
IkReal x438=((r00)*(sj6));
IkReal x439=((cj7)*(sj6));
IkReal x440=((cj6)*(r10));
IkReal x441=((IkReal(1.00000000000000))*(sj11));
IkReal x442=((cj11)*(cj6));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j9, IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x432))));
evalcond[2]=((IkReal(-1.00000000000000))+(((cj11)*(x438)))+(((cj6)*(x435)))+(((IkReal(-1.00000000000000))*(x429)*(x441)))+(((IkReal(-1.00000000000000))*(x428)*(x440))));
evalcond[3]=((((sj11)*(x440)))+(((IkReal(-1.00000000000000))*(x438)*(x441)))+(((IkReal(-1.00000000000000))*(x428)*(x429)))+(((r11)*(x442))));
evalcond[4]=((IkReal(0.0515917000000000))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x433)))+(((IkReal(-0.0620002000000000))*(cj11)*(x438)))+(((IkReal(0.0620002000000000))*(cj6)*(x434)))+(((IkReal(-1.00000000000000))*(py)*(x432)))+(((x429)*(x433))));
evalcond[5]=((((cj7)*(x437)))+(((x430)*(x431)))+(((IkReal(-1.00000000000000))*(r10)*(x428)*(x436)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x428)))+(((x435)*(x436)))+(((IkReal(-1.00000000000000))*(r00)*(x428)*(x430))));
evalcond[6]=((((IkReal(-1.00000000000000))*(cj7)*(x431)*(x432)))+(((cj7)*(r00)*(x442)))+(((IkReal(-1.00000000000000))*(x435)*(x439)))+(((IkReal(-1.00000000000000))*(r20)*(sj7)*(x428)))+(((x434)*(x439)))+(((sj7)*(x437))));
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 j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x443=((cj6)*(sj7));
IkReal x444=((cj11)*(cj7));
IkReal x445=((IkReal(0.0443602609595982))*(r20));
IkReal x446=((IkReal(0.715485771974900))*(px));
IkReal x447=((cj6)*(cj7));
IkReal x448=((pz)*(sj7));
IkReal x449=((IkReal(3.14867567426476))*(cj7));
IkReal x450=((py)*(sj6));
IkReal x451=((IkReal(0.195218521539550))*(sj11));
IkReal x452=((r11)*(sj6));
IkReal x453=((IkReal(3.14867567426476))*(px));
IkReal x454=((IkReal(0.195218521539550))*(r00));
IkReal x455=((IkReal(0.715485771974900))*(cj7));
IkReal x456=((IkReal(0.0443602609595982))*(sj7));
IkReal x457=((r21)*(sj11));
IkReal x458=((r10)*(sj6));
IkReal x459=((IkReal(0.0443602609595982))*(r00));
IkReal x460=((IkReal(0.0443602609595982))*(cj7));
IkReal x461=((IkReal(0.0443602609595982))*(r01)*(sj11));
IkReal x462=((IkReal(0.195218521539550))*(cj11)*(sj7));
if( IKabs(((IkReal(0.850142432051486))+(((IkReal(-1.00000000000000))*(r01)*(x443)*(x451)))+(((IkReal(-0.0443602609595982))*(x444)*(x458)))+(((cj11)*(x443)*(x454)))+(((x447)*(x461)))+(((IkReal(-1.00000000000000))*(cj6)*(x444)*(x459)))+(((IkReal(-1.00000000000000))*(pz)*(x449)))+(((x450)*(x455)))+(((x446)*(x447)))+(((IkReal(-1.00000000000000))*(sj7)*(x451)*(x452)))+(((sj11)*(x452)*(x460)))+(((IkReal(-1.00000000000000))*(x456)*(x457)))+(((IkReal(-0.715485771974900))*(x448)))+(((IkReal(0.195218521539550))*(r20)*(x444)))+(((cj11)*(sj7)*(x445)))+(((IkReal(-1.00000000000000))*(x443)*(x453)))+(((IkReal(-3.14867567426476))*(sj7)*(x450)))+(((x458)*(x462)))+(((IkReal(-1.00000000000000))*(cj7)*(r21)*(x451))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.193181158433223))+(((r20)*(x462)))+(((IkReal(-1.00000000000000))*(cj11)*(x456)*(x458)))+(((IkReal(0.715485771974900))*(sj7)*(x450)))+(((IkReal(-3.14867567426476))*(x448)))+(((x443)*(x446)))+(((IkReal(-0.195218521539550))*(x444)*(x458)))+(((IkReal(-1.00000000000000))*(r21)*(sj7)*(x451)))+(((x443)*(x461)))+(((x449)*(x450)))+(((pz)*(x455)))+(((IkReal(-1.00000000000000))*(cj11)*(x443)*(x459)))+(((IkReal(-1.00000000000000))*(x444)*(x445)))+(((x457)*(x460)))+(((x447)*(x453)))+(((cj7)*(x451)*(x452)))+(((IkReal(-1.00000000000000))*(cj6)*(x444)*(x454)))+(((r01)*(x447)*(x451)))+(((sj11)*(x452)*(x456))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.850142432051486))+(((IkReal(-1.00000000000000))*(r01)*(x443)*(x451)))+(((IkReal(-0.0443602609595982))*(x444)*(x458)))+(((cj11)*(x443)*(x454)))+(((x447)*(x461)))+(((IkReal(-1.00000000000000))*(cj6)*(x444)*(x459)))+(((IkReal(-1.00000000000000))*(pz)*(x449)))+(((x450)*(x455)))+(((x446)*(x447)))+(((IkReal(-1.00000000000000))*(sj7)*(x451)*(x452)))+(((sj11)*(x452)*(x460)))+(((IkReal(-1.00000000000000))*(x456)*(x457)))+(((IkReal(-0.715485771974900))*(x448)))+(((IkReal(0.195218521539550))*(r20)*(x444)))+(((cj11)*(sj7)*(x445)))+(((IkReal(-1.00000000000000))*(x443)*(x453)))+(((IkReal(-3.14867567426476))*(sj7)*(x450)))+(((x458)*(x462)))+(((IkReal(-1.00000000000000))*(cj7)*(r21)*(x451)))))+IKsqr(((IkReal(-0.193181158433223))+(((r20)*(x462)))+(((IkReal(-1.00000000000000))*(cj11)*(x456)*(x458)))+(((IkReal(0.715485771974900))*(sj7)*(x450)))+(((IkReal(-3.14867567426476))*(x448)))+(((x443)*(x446)))+(((IkReal(-0.195218521539550))*(x444)*(x458)))+(((IkReal(-1.00000000000000))*(r21)*(sj7)*(x451)))+(((x443)*(x461)))+(((x449)*(x450)))+(((pz)*(x455)))+(((IkReal(-1.00000000000000))*(cj11)*(x443)*(x459)))+(((IkReal(-1.00000000000000))*(x444)*(x445)))+(((x457)*(x460)))+(((x447)*(x453)))+(((cj7)*(x451)*(x452)))+(((IkReal(-1.00000000000000))*(cj6)*(x444)*(x454)))+(((r01)*(x447)*(x451)))+(((sj11)*(x452)*(x456)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((IkReal(0.850142432051486))+(((IkReal(-1.00000000000000))*(r01)*(x443)*(x451)))+(((IkReal(-0.0443602609595982))*(x444)*(x458)))+(((cj11)*(x443)*(x454)))+(((x447)*(x461)))+(((IkReal(-1.00000000000000))*(cj6)*(x444)*(x459)))+(((IkReal(-1.00000000000000))*(pz)*(x449)))+(((x450)*(x455)))+(((x446)*(x447)))+(((IkReal(-1.00000000000000))*(sj7)*(x451)*(x452)))+(((sj11)*(x452)*(x460)))+(((IkReal(-1.00000000000000))*(x456)*(x457)))+(((IkReal(-0.715485771974900))*(x448)))+(((IkReal(0.195218521539550))*(r20)*(x444)))+(((cj11)*(sj7)*(x445)))+(((IkReal(-1.00000000000000))*(x443)*(x453)))+(((IkReal(-3.14867567426476))*(sj7)*(x450)))+(((x458)*(x462)))+(((IkReal(-1.00000000000000))*(cj7)*(r21)*(x451)))), ((IkReal(-0.193181158433223))+(((r20)*(x462)))+(((IkReal(-1.00000000000000))*(cj11)*(x456)*(x458)))+(((IkReal(0.715485771974900))*(sj7)*(x450)))+(((IkReal(-3.14867567426476))*(x448)))+(((x443)*(x446)))+(((IkReal(-0.195218521539550))*(x444)*(x458)))+(((IkReal(-1.00000000000000))*(r21)*(sj7)*(x451)))+(((x443)*(x461)))+(((x449)*(x450)))+(((pz)*(x455)))+(((IkReal(-1.00000000000000))*(cj11)*(x443)*(x459)))+(((IkReal(-1.00000000000000))*(x444)*(x445)))+(((x457)*(x460)))+(((x447)*(x453)))+(((cj7)*(x451)*(x452)))+(((IkReal(-1.00000000000000))*(cj6)*(x444)*(x454)))+(((r01)*(x447)*(x451)))+(((sj11)*(x452)*(x456)))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[2];
IkReal x463=IKcos(j8);
IkReal x464=IKsin(j8);
IkReal x465=((IkReal(0.0620002000000000))*(sj11));
IkReal x466=((cj6)*(sj7));
IkReal x467=((IkReal(0.0620002000000000))*(cj11));
IkReal x468=((cj7)*(sj6));
IkReal x469=((IkReal(1.00000000000000))*(pz));
IkReal x470=((sj6)*(sj7));
IkReal x471=((cj6)*(cj7));
evalcond[0]=((((px)*(x471)))+(((r11)*(x465)*(x468)))+(((IkReal(-1.00000000000000))*(sj7)*(x469)))+(((IkReal(-0.0686246300000000))*(x464)))+(((IkReal(-1.00000000000000))*(r00)*(x467)*(x471)))+(((IkReal(-0.302000000000000))*(x463)))+(((r20)*(sj7)*(x467)))+(((py)*(x468)))+(((IkReal(-1.00000000000000))*(r21)*(sj7)*(x465)))+(((IkReal(-1.00000000000000))*(r10)*(x467)*(x468)))+(((r01)*(x465)*(x471))));
evalcond[1]=((IkReal(0.270000000000000))+(((r00)*(x466)*(x467)))+(((IkReal(0.0686246300000000))*(x463)))+(((r10)*(x467)*(x470)))+(((IkReal(-1.00000000000000))*(px)*(x466)))+(((IkReal(-1.00000000000000))*(cj7)*(r21)*(x465)))+(((IkReal(-1.00000000000000))*(r11)*(x465)*(x470)))+(((IkReal(-1.00000000000000))*(r01)*(x465)*(x466)))+(((IkReal(-1.00000000000000))*(cj7)*(x469)))+(((IkReal(-1.00000000000000))*(py)*(x470)))+(((IkReal(-0.302000000000000))*(x464)))+(((cj7)*(r20)*(x467))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
{
continue;
}
}
{
IkReal dummyeval[1];
IkReal gconst20;
gconst20=IKsign((((cj8)*(cj8))+((sj8)*(sj8))));
dummyeval[0]=(((cj8)*(cj8))+((sj8)*(sj8)));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
IkReal gconst21;
gconst21=IKsign(((((IkReal(-1.00000000000000))*((sj8)*(sj8))))+(((IkReal(-1.00000000000000))*((cj8)*(cj8))))));
dummyeval[0]=((((IkReal(-1.00000000000000))*((sj8)*(sj8))))+(((IkReal(-1.00000000000000))*((cj8)*(cj8)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
continue;
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x472=((IkReal(1.00000000000000))*(cj8));
IkReal x473=((cj6)*(sj7));
IkReal x474=((r10)*(sj11));
IkReal x475=((cj7)*(r22));
IkReal x476=((cj8)*(sj11));
IkReal x477=((cj11)*(r01));
IkReal x478=((cj7)*(r20));
IkReal x479=((cj11)*(r11));
IkReal x480=((sj6)*(sj7));
IkReal x481=((sj11)*(sj8));
IkReal x482=((cj11)*(cj7)*(r21));
IkReal x483=((sj8)*(x480));
if( IKabs(((gconst21)*(((((sj8)*(x473)*(x477)))+(((x474)*(x483)))+(((r00)*(x473)*(x481)))+(((IkReal(-1.00000000000000))*(r12)*(x472)*(x480)))+(((IkReal(-1.00000000000000))*(r02)*(x472)*(x473)))+(((IkReal(-1.00000000000000))*(x472)*(x475)))+(((x479)*(x483)))+(((sj8)*(x482)))+(((x478)*(x481))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst21)*(((((r02)*(sj8)*(x473)))+(((r12)*(x483)))+(((cj8)*(x479)*(x480)))+(((cj8)*(x473)*(x477)))+(((cj8)*(x482)))+(((cj8)*(x474)*(x480)))+(((x476)*(x478)))+(((sj8)*(x475)))+(((r00)*(x473)*(x476))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst21)*(((((sj8)*(x473)*(x477)))+(((x474)*(x483)))+(((r00)*(x473)*(x481)))+(((IkReal(-1.00000000000000))*(r12)*(x472)*(x480)))+(((IkReal(-1.00000000000000))*(r02)*(x472)*(x473)))+(((IkReal(-1.00000000000000))*(x472)*(x475)))+(((x479)*(x483)))+(((sj8)*(x482)))+(((x478)*(x481)))))), ((gconst21)*(((((r02)*(sj8)*(x473)))+(((r12)*(x483)))+(((cj8)*(x479)*(x480)))+(((cj8)*(x473)*(x477)))+(((cj8)*(x482)))+(((cj8)*(x474)*(x480)))+(((x476)*(x478)))+(((sj8)*(x475)))+(((r00)*(x473)*(x476)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x484=IKsin(j10);
IkReal x485=IKcos(j10);
IkReal x486=((IkReal(1.00000000000000))*(cj7));
IkReal x487=((cj6)*(r02));
IkReal x488=((IkReal(1.00000000000000))*(sj7));
IkReal x489=((r20)*(sj11));
IkReal x490=((cj11)*(r21));
IkReal x491=((r12)*(sj6));
IkReal x492=((cj8)*(x484));
IkReal x493=((r10)*(sj11)*(sj6));
IkReal x494=((IkReal(1.00000000000000))*(x485));
IkReal x495=((cj11)*(cj6)*(r01));
IkReal x496=((sj8)*(x484));
IkReal x497=((cj11)*(r11)*(sj6));
IkReal x498=((cj6)*(r00)*(sj11));
IkReal x499=((sj8)*(x494));
evalcond[0]=((((IkReal(-1.00000000000000))*(cj8)*(x494)))+(((cj7)*(x487)))+(((cj7)*(x491)))+(((IkReal(-1.00000000000000))*(x496)))+(((IkReal(-1.00000000000000))*(r22)*(x488))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x487)*(x488)))+(((IkReal(-1.00000000000000))*(r22)*(x486)))+(x492)+(((IkReal(-1.00000000000000))*(x488)*(x491)))+(((IkReal(-1.00000000000000))*(x499))));
evalcond[2]=((((cj7)*(x489)))+(((sj7)*(x493)))+(((cj7)*(x490)))+(((sj7)*(x495)))+(((cj8)*(x485)))+(x496)+(((sj7)*(x497)))+(((sj7)*(x498))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x486)*(x495)))+(((sj7)*(x490)))+(((IkReal(-1.00000000000000))*(x486)*(x498)))+(((IkReal(-1.00000000000000))*(x486)*(x497)))+(x492)+(((IkReal(-1.00000000000000))*(x486)*(x493)))+(((sj7)*(x489)))+(((IkReal(-1.00000000000000))*(x499))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x500=((r12)*(sj6));
IkReal x501=((cj7)*(cj8));
IkReal x502=((cj6)*(r02));
IkReal x503=((cj8)*(sj7));
IkReal x504=((cj7)*(sj8));
IkReal x505=((IkReal(1.00000000000000))*(r22)*(sj7));
IkReal x506=((IkReal(1.00000000000000))*(sj7)*(sj8));
if( IKabs(((gconst20)*(((((x502)*(x504)))+(((r22)*(x501)))+(((x500)*(x503)))+(((x500)*(x504)))+(((IkReal(-1.00000000000000))*(sj8)*(x505)))+(((x502)*(x503))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst20)*(((((IkReal(-1.00000000000000))*(r22)*(x504)))+(((x500)*(x501)))+(((IkReal(-1.00000000000000))*(x500)*(x506)))+(((x501)*(x502)))+(((IkReal(-1.00000000000000))*(x502)*(x506)))+(((IkReal(-1.00000000000000))*(r22)*(x503))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst20)*(((((x502)*(x504)))+(((r22)*(x501)))+(((x500)*(x503)))+(((x500)*(x504)))+(((IkReal(-1.00000000000000))*(sj8)*(x505)))+(((x502)*(x503)))))), ((gconst20)*(((((IkReal(-1.00000000000000))*(r22)*(x504)))+(((x500)*(x501)))+(((IkReal(-1.00000000000000))*(x500)*(x506)))+(((x501)*(x502)))+(((IkReal(-1.00000000000000))*(x502)*(x506)))+(((IkReal(-1.00000000000000))*(r22)*(x503)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x507=IKsin(j10);
IkReal x508=IKcos(j10);
IkReal x509=((IkReal(1.00000000000000))*(cj7));
IkReal x510=((cj6)*(r02));
IkReal x511=((IkReal(1.00000000000000))*(sj7));
IkReal x512=((r20)*(sj11));
IkReal x513=((cj11)*(r21));
IkReal x514=((r12)*(sj6));
IkReal x515=((cj8)*(x507));
IkReal x516=((r10)*(sj11)*(sj6));
IkReal x517=((IkReal(1.00000000000000))*(x508));
IkReal x518=((cj11)*(cj6)*(r01));
IkReal x519=((sj8)*(x507));
IkReal x520=((cj11)*(r11)*(sj6));
IkReal x521=((cj6)*(r00)*(sj11));
IkReal x522=((sj8)*(x517));
evalcond[0]=((((cj7)*(x514)))+(((IkReal(-1.00000000000000))*(r22)*(x511)))+(((cj7)*(x510)))+(((IkReal(-1.00000000000000))*(x519)))+(((IkReal(-1.00000000000000))*(cj8)*(x517))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x511)*(x514)))+(x515)+(((IkReal(-1.00000000000000))*(x510)*(x511)))+(((IkReal(-1.00000000000000))*(r22)*(x509)))+(((IkReal(-1.00000000000000))*(x522))));
evalcond[2]=((((cj7)*(x513)))+(x519)+(((sj7)*(x521)))+(((cj7)*(x512)))+(((sj7)*(x518)))+(((sj7)*(x516)))+(((cj8)*(x508)))+(((sj7)*(x520))));
evalcond[3]=((x515)+(((IkReal(-1.00000000000000))*(x509)*(x520)))+(((IkReal(-1.00000000000000))*(x509)*(x518)))+(((IkReal(-1.00000000000000))*(x522)))+(((IkReal(-1.00000000000000))*(x509)*(x516)))+(((IkReal(-1.00000000000000))*(x509)*(x521)))+(((sj7)*(x513)))+(((sj7)*(x512))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
}
}
} else
{
if( 1 )
{
continue;
} else
{
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x523=((IkReal(500.000000000000))*(sj9));
IkReal x524=((cj11)*(cj7));
IkReal x525=((r10)*(sj6));
IkReal x526=((cj11)*(sj7));
IkReal x527=((IkReal(31.0001000000000))*(sj9));
IkReal x528=((cj6)*(r00));
IkReal x529=((sj11)*(sj7));
IkReal x530=((cj6)*(r01));
IkReal x531=((cj7)*(sj6));
IkReal x532=((cj7)*(sj11));
IkReal x533=((r11)*(sj6));
if( IKabs(((((IKabs(((((IkReal(30.8425000000000))*((sj9)*(sj9))))+(((IkReal(-35.0701500000000))*(sj9)))+(((IkReal(-0.757835000000000))*(cj9)*(sj9))))) != 0)?((IkReal)1/(((((IkReal(30.8425000000000))*((sj9)*(sj9))))+(((IkReal(-35.0701500000000))*(sj9)))+(((IkReal(-0.757835000000000))*(cj9)*(sj9)))))):(IkReal)1.0e30))*(((((x524)*(x527)*(x528)))+(((IkReal(-151.000000000000))*(x529)*(x530)))+(((IkReal(-1.00000000000000))*(x527)*(x530)*(x532)))+(((IkReal(-1.00000000000000))*(py)*(x523)*(x531)))+(((IkReal(-1.00000000000000))*(r20)*(x526)*(x527)))+(((x524)*(x525)*(x527)))+(((IkReal(-151.000000000000))*(r21)*(x532)))+(((IkReal(151.000000000000))*(x526)*(x528)))+(((IkReal(151.000000000000))*(x525)*(x526)))+(((IkReal(-151.000000000000))*(x529)*(x533)))+(((r21)*(x527)*(x529)))+(((IkReal(-1.00000000000000))*(cj6)*(cj7)*(px)*(x523)))+(((IkReal(-1.00000000000000))*(r11)*(sj11)*(x527)*(x531)))+(((pz)*(sj7)*(x523)))+(((IkReal(151.000000000000))*(r20)*(x524))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((r20)*(x524)))+(((IkReal(-1.00000000000000))*(x529)*(x530)))+(((x526)*(x528)))+(((x525)*(x526)))+(((IkReal(-1.00000000000000))*(x529)*(x533)))+(((IkReal(-1.00000000000000))*(r21)*(x532))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(((((IkReal(30.8425000000000))*((sj9)*(sj9))))+(((IkReal(-35.0701500000000))*(sj9)))+(((IkReal(-0.757835000000000))*(cj9)*(sj9))))) != 0)?((IkReal)1/(((((IkReal(30.8425000000000))*((sj9)*(sj9))))+(((IkReal(-35.0701500000000))*(sj9)))+(((IkReal(-0.757835000000000))*(cj9)*(sj9)))))):(IkReal)1.0e30))*(((((x524)*(x527)*(x528)))+(((IkReal(-151.000000000000))*(x529)*(x530)))+(((IkReal(-1.00000000000000))*(x527)*(x530)*(x532)))+(((IkReal(-1.00000000000000))*(py)*(x523)*(x531)))+(((IkReal(-1.00000000000000))*(r20)*(x526)*(x527)))+(((x524)*(x525)*(x527)))+(((IkReal(-151.000000000000))*(r21)*(x532)))+(((IkReal(151.000000000000))*(x526)*(x528)))+(((IkReal(151.000000000000))*(x525)*(x526)))+(((IkReal(-151.000000000000))*(x529)*(x533)))+(((r21)*(x527)*(x529)))+(((IkReal(-1.00000000000000))*(cj6)*(cj7)*(px)*(x523)))+(((IkReal(-1.00000000000000))*(r11)*(sj11)*(x527)*(x531)))+(((pz)*(sj7)*(x523)))+(((IkReal(151.000000000000))*(r20)*(x524)))))))+IKsqr(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((r20)*(x524)))+(((IkReal(-1.00000000000000))*(x529)*(x530)))+(((x526)*(x528)))+(((x525)*(x526)))+(((IkReal(-1.00000000000000))*(x529)*(x533)))+(((IkReal(-1.00000000000000))*(r21)*(x532)))))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((((IKabs(((((IkReal(30.8425000000000))*((sj9)*(sj9))))+(((IkReal(-35.0701500000000))*(sj9)))+(((IkReal(-0.757835000000000))*(cj9)*(sj9))))) != 0)?((IkReal)1/(((((IkReal(30.8425000000000))*((sj9)*(sj9))))+(((IkReal(-35.0701500000000))*(sj9)))+(((IkReal(-0.757835000000000))*(cj9)*(sj9)))))):(IkReal)1.0e30))*(((((x524)*(x527)*(x528)))+(((IkReal(-151.000000000000))*(x529)*(x530)))+(((IkReal(-1.00000000000000))*(x527)*(x530)*(x532)))+(((IkReal(-1.00000000000000))*(py)*(x523)*(x531)))+(((IkReal(-1.00000000000000))*(r20)*(x526)*(x527)))+(((x524)*(x525)*(x527)))+(((IkReal(-151.000000000000))*(r21)*(x532)))+(((IkReal(151.000000000000))*(x526)*(x528)))+(((IkReal(151.000000000000))*(x525)*(x526)))+(((IkReal(-151.000000000000))*(x529)*(x533)))+(((r21)*(x527)*(x529)))+(((IkReal(-1.00000000000000))*(cj6)*(cj7)*(px)*(x523)))+(((IkReal(-1.00000000000000))*(r11)*(sj11)*(x527)*(x531)))+(((pz)*(sj7)*(x523)))+(((IkReal(151.000000000000))*(r20)*(x524)))))), ((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((r20)*(x524)))+(((IkReal(-1.00000000000000))*(x529)*(x530)))+(((x526)*(x528)))+(((x525)*(x526)))+(((IkReal(-1.00000000000000))*(x529)*(x533)))+(((IkReal(-1.00000000000000))*(r21)*(x532)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[4];
IkReal x534=IKsin(j8);
IkReal x535=IKcos(j8);
IkReal x536=((IkReal(0.0620002000000000))*(sj11));
IkReal x537=((cj7)*(r21));
IkReal x538=((IkReal(1.00000000000000))*(cj11));
IkReal x539=((cj7)*(r20));
IkReal x540=((IkReal(0.00151567000000000))*(cj9));
IkReal x541=((r21)*(sj7));
IkReal x542=((IkReal(1.00000000000000))*(cj7));
IkReal x543=((cj7)*(sj6));
IkReal x544=((IkReal(0.0620002000000000))*(cj11));
IkReal x545=((IkReal(1.00000000000000))*(sj7));
IkReal x546=((cj6)*(px));
IkReal x547=((cj6)*(r01));
IkReal x548=((cj11)*(r20)*(sj7));
IkReal x549=((sj7)*(x547));
IkReal x550=((sj9)*(x535));
IkReal x551=((cj6)*(r00)*(sj7));
IkReal x552=((r11)*(sj6)*(sj7));
IkReal x553=((sj9)*(x534));
IkReal x554=((r10)*(sj6)*(sj7));
IkReal x555=((cj6)*(cj7)*(r00));
evalcond[0]=((((IkReal(-1.00000000000000))*(x538)*(x554)))+(((sj11)*(x549)))+(((IkReal(-1.00000000000000))*(x538)*(x551)))+(((IkReal(-1.00000000000000))*(x538)*(x539)))+(((sj11)*(x537)))+(((sj11)*(x552)))+(x550));
evalcond[1]=((((cj11)*(x555)))+(((IkReal(-1.00000000000000))*(x553)))+(((cj11)*(r10)*(x543)))+(((sj11)*(x541)))+(((IkReal(-1.00000000000000))*(r20)*(sj7)*(x538)))+(((IkReal(-1.00000000000000))*(r11)*(sj11)*(sj6)*(x542)))+(((IkReal(-1.00000000000000))*(sj11)*(x542)*(x547))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x544)*(x555)))+(((py)*(x543)))+(((IkReal(-1.00000000000000))*(pz)*(x545)))+(((IkReal(-0.302000000000000))*(x535)))+(((IkReal(-1.00000000000000))*(r10)*(x543)*(x544)))+(((cj7)*(x536)*(x547)))+(((IkReal(-0.0701403000000000))*(x534)))+(((r11)*(x536)*(x543)))+(((cj7)*(x546)))+(((IkReal(0.0616850000000000))*(x553)))+(((IkReal(-1.00000000000000))*(x536)*(x541)))+(((IkReal(-1.00000000000000))*(x534)*(x540)))+(((r20)*(sj7)*(x544))));
evalcond[3]=((IkReal(0.270000000000000))+(((IkReal(-1.00000000000000))*(pz)*(x542)))+(((x544)*(x551)))+(((IkReal(-0.302000000000000))*(x534)))+(((IkReal(-0.0616850000000000))*(x550)))+(((IkReal(-1.00000000000000))*(x545)*(x546)))+(((IkReal(0.0701403000000000))*(x535)))+(((IkReal(-1.00000000000000))*(x536)*(x537)))+(((x544)*(x554)))+(((x535)*(x540)))+(((IkReal(-1.00000000000000))*(x536)*(x549)))+(((x539)*(x544)))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x545)))+(((IkReal(-1.00000000000000))*(x536)*(x552))));
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 gconst2;
gconst2=IKsign(sj9);
dummyeval[0]=sj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[2];
dummyeval[0]=sj9;
dummyeval[1]=cj8;
if( IKabs(dummyeval[0]) < 0.0000010000000000 || IKabs(dummyeval[1]) < 0.0000010000000000 )
{
{
IkReal evalcond[9];
IkReal x556=((IkReal(0.0620002000000000))*(cj7));
IkReal x557=((r21)*(sj11));
IkReal x558=((IkReal(1.00000000000000))*(cj11));
IkReal x559=((r01)*(sj6));
IkReal x560=((r20)*(sj7));
IkReal x561=((IkReal(1.00000000000000))*(cj6));
IkReal x562=((IkReal(0.0620002000000000))*(cj11));
IkReal x563=((IkReal(1.00000000000000))*(sj7));
IkReal x564=((IkReal(0.0620002000000000))*(sj11));
IkReal x565=((cj6)*(r10));
IkReal x566=((IkReal(1.00000000000000))*(cj7));
IkReal x567=((cj7)*(sj6));
IkReal x568=((r00)*(sj6));
IkReal x569=((r11)*(sj11));
IkReal x570=((IkReal(0.0620002000000000))*(sj7));
IkReal x571=((cj11)*(r10));
IkReal x572=((IkReal(1.00000000000000))*(sj11));
IkReal x573=((cj6)*(r11));
IkReal x574=((r01)*(sj11));
IkReal x575=((cj11)*(cj6)*(r00));
IkReal x576=((r11)*(sj6)*(sj7));
IkReal x577=((r10)*(sj6)*(sj7));
IkReal x578=((cj6)*(sj7)*(x574));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j9)), IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x561))));
evalcond[2]=((IkReal(1.00000000000000))+(((cj6)*(x569)))+(((IkReal(-1.00000000000000))*(x559)*(x572)))+(((cj11)*(x568)))+(((IkReal(-1.00000000000000))*(x558)*(x565))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x558)*(x559)))+(((IkReal(-1.00000000000000))*(x568)*(x572)))+(((sj11)*(x565)))+(((cj11)*(x573))));
evalcond[4]=((IkReal(-0.0717783000000000))+(((IkReal(-1.00000000000000))*(x562)*(x568)))+(((x559)*(x564)))+(((x562)*(x565)))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(x564)*(x573)))+(((IkReal(-1.00000000000000))*(py)*(x561))));
evalcond[5]=((x578)+(((cj7)*(x557)))+(((sj6)*(sj7)*(x569)))+(((IkReal(-1.00000000000000))*(x558)*(x577)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj7)*(x558)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x558))));
evalcond[6]=((((IkReal(-1.00000000000000))*(cj7)*(x561)*(x574)))+(((cj7)*(x575)))+(((IkReal(-1.00000000000000))*(sj6)*(x566)*(x569)))+(((x567)*(x571)))+(((IkReal(-1.00000000000000))*(x558)*(x560)))+(((sj7)*(x557))));
evalcond[7]=((((IkReal(-0.302000000000000))*(cj8)))+(((IkReal(-1.00000000000000))*(pz)*(x563)))+(((IkReal(-0.0716559700000000))*(sj8)))+(((IkReal(-1.00000000000000))*(x556)*(x575)))+(((cj6)*(x556)*(x574)))+(((cj6)*(cj7)*(px)))+(((IkReal(-1.00000000000000))*(x557)*(x570)))+(((py)*(x567)))+(((x560)*(x562)))+(((IkReal(-1.00000000000000))*(sj6)*(x556)*(x571)))+(((sj6)*(x556)*(x569))));
evalcond[8]=((IkReal(0.270000000000000))+(((IkReal(-1.00000000000000))*(x556)*(x557)))+(((IkReal(0.0716559700000000))*(cj8)))+(((IkReal(-1.00000000000000))*(x564)*(x576)))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x563)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj7)*(x564)))+(((IkReal(-1.00000000000000))*(px)*(sj7)*(x561)))+(((x562)*(x577)))+(((cj11)*(r20)*(x556)))+(((cj6)*(r00)*(sj7)*(x562)))+(((IkReal(-0.302000000000000))*(sj8)))+(((IkReal(-1.00000000000000))*(pz)*(x566))));
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 )
{
{
IkReal dummyeval[1];
IkReal gconst3;
gconst3=IKsign((((cj8)*(cj8))+((sj8)*(sj8))));
dummyeval[0]=(((cj8)*(cj8))+((sj8)*(sj8)));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
IkReal gconst4;
gconst4=IKsign(((((IkReal(-1.00000000000000))*((sj8)*(sj8))))+(((IkReal(-1.00000000000000))*((cj8)*(cj8))))));
dummyeval[0]=((((IkReal(-1.00000000000000))*((sj8)*(sj8))))+(((IkReal(-1.00000000000000))*((cj8)*(cj8)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
continue;
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x579=((sj7)*(sj8));
IkReal x580=((cj11)*(r21));
IkReal x581=((r20)*(sj11));
IkReal x582=((cj8)*(sj7));
IkReal x583=((IkReal(1.00000000000000))*(sj11));
IkReal x584=((IkReal(1.00000000000000))*(cj11));
IkReal x585=((cj7)*(sj6)*(sj8));
IkReal x586=((cj6)*(cj7)*(sj8));
IkReal x587=((cj7)*(cj8)*(sj6));
IkReal x588=((cj6)*(cj7)*(cj8));
if( IKabs(((gconst4)*(((((r02)*(x586)))+(((r12)*(x585)))+(((IkReal(-1.00000000000000))*(r22)*(x579)))+(((IkReal(-1.00000000000000))*(r10)*(x583)*(x587)))+(((x581)*(x582)))+(((IkReal(-1.00000000000000))*(r11)*(x584)*(x587)))+(((IkReal(-1.00000000000000))*(r01)*(x584)*(x588)))+(((IkReal(-1.00000000000000))*(r00)*(x583)*(x588)))+(((x580)*(x582))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(r10)*(x583)*(x585)))+(((x579)*(x580)))+(((x579)*(x581)))+(((IkReal(-1.00000000000000))*(r01)*(x584)*(x586)))+(((IkReal(-1.00000000000000))*(r11)*(x584)*(x585)))+(((IkReal(-1.00000000000000))*(r00)*(x583)*(x586)))+(((r22)*(x582)))+(((IkReal(-1.00000000000000))*(r02)*(x588)))+(((IkReal(-1.00000000000000))*(r12)*(x587))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst4)*(((((r02)*(x586)))+(((r12)*(x585)))+(((IkReal(-1.00000000000000))*(r22)*(x579)))+(((IkReal(-1.00000000000000))*(r10)*(x583)*(x587)))+(((x581)*(x582)))+(((IkReal(-1.00000000000000))*(r11)*(x584)*(x587)))+(((IkReal(-1.00000000000000))*(r01)*(x584)*(x588)))+(((IkReal(-1.00000000000000))*(r00)*(x583)*(x588)))+(((x580)*(x582)))))), ((gconst4)*(((((IkReal(-1.00000000000000))*(r10)*(x583)*(x585)))+(((x579)*(x580)))+(((x579)*(x581)))+(((IkReal(-1.00000000000000))*(r01)*(x584)*(x586)))+(((IkReal(-1.00000000000000))*(r11)*(x584)*(x585)))+(((IkReal(-1.00000000000000))*(r00)*(x583)*(x586)))+(((r22)*(x582)))+(((IkReal(-1.00000000000000))*(r02)*(x588)))+(((IkReal(-1.00000000000000))*(r12)*(x587)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x589=IKcos(j10);
IkReal x590=IKsin(j10);
IkReal x591=((IkReal(1.00000000000000))*(cj7));
IkReal x592=((cj6)*(r02));
IkReal x593=((IkReal(1.00000000000000))*(sj7));
IkReal x594=((r20)*(sj11));
IkReal x595=((cj11)*(r21));
IkReal x596=((r12)*(sj6));
IkReal x597=((sj8)*(x590));
IkReal x598=((cj8)*(x590));
IkReal x599=((r10)*(sj11)*(sj6));
IkReal x600=((IkReal(1.00000000000000))*(x589));
IkReal x601=((cj11)*(cj6)*(r01));
IkReal x602=((cj11)*(r11)*(sj6));
IkReal x603=((cj6)*(r00)*(sj11));
IkReal x604=((cj8)*(x600));
evalcond[0]=((((cj7)*(x592)))+(((IkReal(-1.00000000000000))*(r22)*(x593)))+(((cj7)*(x596)))+(((IkReal(-1.00000000000000))*(x604)))+(x597));
evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(x591)))+(((IkReal(-1.00000000000000))*(x592)*(x593)))+(((IkReal(-1.00000000000000))*(x593)*(x596)))+(((IkReal(-1.00000000000000))*(sj8)*(x600)))+(((IkReal(-1.00000000000000))*(x598))));
evalcond[2]=((((cj7)*(x594)))+(((sj7)*(x603)))+(((IkReal(-1.00000000000000))*(x604)))+(((sj7)*(x599)))+(((cj7)*(x595)))+(((sj7)*(x602)))+(x597)+(((sj7)*(x601))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x591)*(x599)))+(((sj7)*(x594)))+(((sj7)*(x595)))+(((IkReal(-1.00000000000000))*(x591)*(x602)))+(((IkReal(-1.00000000000000))*(x591)*(x603)))+(((IkReal(-1.00000000000000))*(x591)*(x601)))+(x598)+(((sj8)*(x589))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x605=((cj7)*(cj8));
IkReal x606=((r12)*(sj6));
IkReal x607=((IkReal(1.00000000000000))*(sj7));
IkReal x608=((cj6)*(r02));
IkReal x609=((IkReal(1.00000000000000))*(r22));
IkReal x610=((cj7)*(sj8));
if( IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(x606)*(x610)))+(((IkReal(-1.00000000000000))*(cj8)*(x607)*(x608)))+(((IkReal(-1.00000000000000))*(x605)*(x609)))+(((r22)*(sj7)*(sj8)))+(((IkReal(-1.00000000000000))*(x608)*(x610)))+(((IkReal(-1.00000000000000))*(cj8)*(x606)*(x607))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((x605)*(x608)))+(((IkReal(-1.00000000000000))*(sj8)*(x606)*(x607)))+(((IkReal(-1.00000000000000))*(x609)*(x610)))+(((IkReal(-1.00000000000000))*(sj8)*(x607)*(x608)))+(((IkReal(-1.00000000000000))*(cj8)*(r22)*(x607)))+(((x605)*(x606))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst3)*(((((IkReal(-1.00000000000000))*(x606)*(x610)))+(((IkReal(-1.00000000000000))*(cj8)*(x607)*(x608)))+(((IkReal(-1.00000000000000))*(x605)*(x609)))+(((r22)*(sj7)*(sj8)))+(((IkReal(-1.00000000000000))*(x608)*(x610)))+(((IkReal(-1.00000000000000))*(cj8)*(x606)*(x607)))))), ((gconst3)*(((((x605)*(x608)))+(((IkReal(-1.00000000000000))*(sj8)*(x606)*(x607)))+(((IkReal(-1.00000000000000))*(x609)*(x610)))+(((IkReal(-1.00000000000000))*(sj8)*(x607)*(x608)))+(((IkReal(-1.00000000000000))*(cj8)*(r22)*(x607)))+(((x605)*(x606)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x611=IKcos(j10);
IkReal x612=IKsin(j10);
IkReal x613=((IkReal(1.00000000000000))*(cj7));
IkReal x614=((cj6)*(r02));
IkReal x615=((IkReal(1.00000000000000))*(sj7));
IkReal x616=((r20)*(sj11));
IkReal x617=((cj11)*(r21));
IkReal x618=((r12)*(sj6));
IkReal x619=((sj8)*(x612));
IkReal x620=((cj8)*(x612));
IkReal x621=((r10)*(sj11)*(sj6));
IkReal x622=((IkReal(1.00000000000000))*(x611));
IkReal x623=((cj11)*(cj6)*(r01));
IkReal x624=((cj11)*(r11)*(sj6));
IkReal x625=((cj6)*(r00)*(sj11));
IkReal x626=((cj8)*(x622));
evalcond[0]=((((cj7)*(x614)))+(((IkReal(-1.00000000000000))*(x626)))+(((cj7)*(x618)))+(((IkReal(-1.00000000000000))*(r22)*(x615)))+(x619));
evalcond[1]=((((IkReal(-1.00000000000000))*(sj8)*(x622)))+(((IkReal(-1.00000000000000))*(r22)*(x613)))+(((IkReal(-1.00000000000000))*(x614)*(x615)))+(((IkReal(-1.00000000000000))*(x615)*(x618)))+(((IkReal(-1.00000000000000))*(x620))));
evalcond[2]=((((sj7)*(x621)))+(((sj7)*(x625)))+(((cj7)*(x616)))+(((IkReal(-1.00000000000000))*(x626)))+(((cj7)*(x617)))+(((sj7)*(x624)))+(((sj7)*(x623)))+(x619));
evalcond[3]=((((sj7)*(x617)))+(((IkReal(-1.00000000000000))*(x613)*(x621)))+(((IkReal(-1.00000000000000))*(x613)*(x624)))+(((IkReal(-1.00000000000000))*(x613)*(x623)))+(((IkReal(-1.00000000000000))*(x613)*(x625)))+(((sj8)*(x611)))+(x620)+(((sj7)*(x616))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
IkReal x627=((IkReal(0.0620002000000000))*(cj7));
IkReal x628=((r21)*(sj11));
IkReal x629=((IkReal(1.00000000000000))*(cj11));
IkReal x630=((r01)*(sj6));
IkReal x631=((r20)*(sj7));
IkReal x632=((IkReal(1.00000000000000))*(cj6));
IkReal x633=((IkReal(0.0620002000000000))*(cj11));
IkReal x634=((IkReal(1.00000000000000))*(sj7));
IkReal x635=((IkReal(0.0620002000000000))*(sj11));
IkReal x636=((cj6)*(r10));
IkReal x637=((IkReal(1.00000000000000))*(cj7));
IkReal x638=((cj7)*(sj6));
IkReal x639=((r00)*(sj6));
IkReal x640=((r11)*(sj11));
IkReal x641=((IkReal(0.0620002000000000))*(sj7));
IkReal x642=((cj11)*(r10));
IkReal x643=((IkReal(1.00000000000000))*(sj11));
IkReal x644=((cj6)*(r11));
IkReal x645=((r01)*(sj11));
IkReal x646=((cj11)*(cj6)*(r00));
IkReal x647=((r11)*(sj6)*(sj7));
IkReal x648=((r10)*(sj6)*(sj7));
IkReal x649=((cj6)*(sj7)*(x645));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j9, IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x632))));
evalcond[2]=((IkReal(-1.00000000000000))+(((cj6)*(x640)))+(((IkReal(-1.00000000000000))*(x630)*(x643)))+(((cj11)*(x639)))+(((IkReal(-1.00000000000000))*(x629)*(x636))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x629)*(x630)))+(((sj11)*(x636)))+(((cj11)*(x644)))+(((IkReal(-1.00000000000000))*(x639)*(x643))));
evalcond[4]=((IkReal(0.0515917000000000))+(((IkReal(-1.00000000000000))*(x635)*(x644)))+(((x633)*(x636)))+(((px)*(sj6)))+(((x630)*(x635)))+(((IkReal(-1.00000000000000))*(x633)*(x639)))+(((IkReal(-1.00000000000000))*(py)*(x632))));
evalcond[5]=((x649)+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj7)*(x629)))+(((cj7)*(x628)))+(((IkReal(-1.00000000000000))*(x629)*(x648)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x629)))+(((sj6)*(sj7)*(x640))));
evalcond[6]=((((IkReal(-1.00000000000000))*(cj7)*(x632)*(x645)))+(((x638)*(x642)))+(((cj7)*(x646)))+(((sj7)*(x628)))+(((IkReal(-1.00000000000000))*(x629)*(x631)))+(((IkReal(-1.00000000000000))*(sj6)*(x637)*(x640))));
evalcond[7]=((((IkReal(-0.302000000000000))*(cj8)))+(((IkReal(-1.00000000000000))*(sj6)*(x627)*(x642)))+(((IkReal(-0.0686246300000000))*(sj8)))+(((py)*(x638)))+(((IkReal(-1.00000000000000))*(x628)*(x641)))+(((cj6)*(cj7)*(px)))+(((IkReal(-1.00000000000000))*(x627)*(x646)))+(((IkReal(-1.00000000000000))*(pz)*(x634)))+(((sj6)*(x627)*(x640)))+(((x631)*(x633)))+(((cj6)*(x627)*(x645))));
evalcond[8]=((IkReal(0.270000000000000))+(((IkReal(-1.00000000000000))*(x627)*(x628)))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x634)))+(((cj6)*(r00)*(sj7)*(x633)))+(((IkReal(-1.00000000000000))*(x635)*(x647)))+(((IkReal(0.0686246300000000))*(cj8)))+(((cj11)*(r20)*(x627)))+(((IkReal(-0.302000000000000))*(sj8)))+(((IkReal(-1.00000000000000))*(px)*(sj7)*(x632)))+(((x633)*(x648)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj7)*(x635)))+(((IkReal(-1.00000000000000))*(pz)*(x637))));
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 )
{
{
IkReal dummyeval[1];
IkReal gconst5;
gconst5=IKsign((((cj8)*(cj8))+((sj8)*(sj8))));
dummyeval[0]=(((cj8)*(cj8))+((sj8)*(sj8)));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
IkReal gconst6;
gconst6=IKsign(((((IkReal(-1.00000000000000))*((sj8)*(sj8))))+(((IkReal(-1.00000000000000))*((cj8)*(cj8))))));
dummyeval[0]=((((IkReal(-1.00000000000000))*((sj8)*(sj8))))+(((IkReal(-1.00000000000000))*((cj8)*(cj8)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
continue;
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x650=((IkReal(1.00000000000000))*(cj8));
IkReal x651=((cj6)*(sj7));
IkReal x652=((cj8)*(sj11));
IkReal x653=((cj7)*(r20));
IkReal x654=((sj6)*(sj7));
IkReal x655=((sj11)*(sj8));
IkReal x656=((cj7)*(r22));
IkReal x657=((cj11)*(sj8));
IkReal x658=((cj11)*(cj8));
IkReal x659=((cj11)*(cj7)*(r21));
if( IKabs(((gconst6)*(((((cj7)*(r21)*(x657)))+(((r11)*(x654)*(x657)))+(((r10)*(x654)*(x655)))+(((IkReal(-1.00000000000000))*(r12)*(x650)*(x654)))+(((r01)*(x651)*(x657)))+(((IkReal(-1.00000000000000))*(x650)*(x656)))+(((x653)*(x655)))+(((IkReal(-1.00000000000000))*(r02)*(x650)*(x651)))+(((r00)*(x651)*(x655))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((sj8)*(x656)))+(((r01)*(x651)*(x658)))+(((r02)*(sj8)*(x651)))+(((r10)*(x652)*(x654)))+(((x652)*(x653)))+(((r12)*(sj8)*(x654)))+(((r00)*(x651)*(x652)))+(((cj7)*(r21)*(x658)))+(((r11)*(x654)*(x658))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst6)*(((((cj7)*(r21)*(x657)))+(((r11)*(x654)*(x657)))+(((r10)*(x654)*(x655)))+(((IkReal(-1.00000000000000))*(r12)*(x650)*(x654)))+(((r01)*(x651)*(x657)))+(((IkReal(-1.00000000000000))*(x650)*(x656)))+(((x653)*(x655)))+(((IkReal(-1.00000000000000))*(r02)*(x650)*(x651)))+(((r00)*(x651)*(x655)))))), ((gconst6)*(((((sj8)*(x656)))+(((r01)*(x651)*(x658)))+(((r02)*(sj8)*(x651)))+(((r10)*(x652)*(x654)))+(((x652)*(x653)))+(((r12)*(sj8)*(x654)))+(((r00)*(x651)*(x652)))+(((cj7)*(r21)*(x658)))+(((r11)*(x654)*(x658)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x660=IKsin(j10);
IkReal x661=IKcos(j10);
IkReal x662=((IkReal(1.00000000000000))*(cj7));
IkReal x663=((cj6)*(r02));
IkReal x664=((IkReal(1.00000000000000))*(sj7));
IkReal x665=((r20)*(sj11));
IkReal x666=((cj11)*(r21));
IkReal x667=((r12)*(sj6));
IkReal x668=((cj8)*(x660));
IkReal x669=((r10)*(sj11)*(sj6));
IkReal x670=((IkReal(1.00000000000000))*(x661));
IkReal x671=((cj11)*(cj6)*(r01));
IkReal x672=((sj8)*(x660));
IkReal x673=((cj11)*(r11)*(sj6));
IkReal x674=((cj6)*(r00)*(sj11));
IkReal x675=((sj8)*(x670));
evalcond[0]=((((IkReal(-1.00000000000000))*(cj8)*(x670)))+(((cj7)*(x667)))+(((IkReal(-1.00000000000000))*(r22)*(x664)))+(((IkReal(-1.00000000000000))*(x672)))+(((cj7)*(x663))));
evalcond[1]=((x668)+(((IkReal(-1.00000000000000))*(x663)*(x664)))+(((IkReal(-1.00000000000000))*(x675)))+(((IkReal(-1.00000000000000))*(r22)*(x662)))+(((IkReal(-1.00000000000000))*(x664)*(x667))));
evalcond[2]=((((sj7)*(x671)))+(((sj7)*(x674)))+(((cj8)*(x661)))+(((sj7)*(x669)))+(((cj7)*(x665)))+(((cj7)*(x666)))+(((sj7)*(x673)))+(x672));
evalcond[3]=((x668)+(((sj7)*(x665)))+(((IkReal(-1.00000000000000))*(x662)*(x671)))+(((sj7)*(x666)))+(((IkReal(-1.00000000000000))*(x662)*(x673)))+(((IkReal(-1.00000000000000))*(x662)*(x674)))+(((IkReal(-1.00000000000000))*(x675)))+(((IkReal(-1.00000000000000))*(x662)*(x669))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x676=((cj6)*(r02));
IkReal x677=((cj7)*(cj8));
IkReal x678=((r12)*(sj6));
IkReal x679=((cj8)*(sj7));
IkReal x680=((cj7)*(sj8));
IkReal x681=((cj7)*(x678));
IkReal x682=((IkReal(1.00000000000000))*(r22)*(sj7));
IkReal x683=((IkReal(1.00000000000000))*(sj7)*(sj8));
if( IKabs(((gconst5)*(((((x678)*(x680)))+(((x676)*(x680)))+(((IkReal(-1.00000000000000))*(sj8)*(x682)))+(((x676)*(x679)))+(((x678)*(x679)))+(((r22)*(x677))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((((x676)*(x677)))+(((IkReal(-1.00000000000000))*(x676)*(x683)))+(((IkReal(-1.00000000000000))*(r22)*(x680)))+(((x677)*(x678)))+(((IkReal(-1.00000000000000))*(r22)*(x679)))+(((IkReal(-1.00000000000000))*(x678)*(x683))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst5)*(((((x678)*(x680)))+(((x676)*(x680)))+(((IkReal(-1.00000000000000))*(sj8)*(x682)))+(((x676)*(x679)))+(((x678)*(x679)))+(((r22)*(x677)))))), ((gconst5)*(((((x676)*(x677)))+(((IkReal(-1.00000000000000))*(x676)*(x683)))+(((IkReal(-1.00000000000000))*(r22)*(x680)))+(((x677)*(x678)))+(((IkReal(-1.00000000000000))*(r22)*(x679)))+(((IkReal(-1.00000000000000))*(x678)*(x683)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x684=IKsin(j10);
IkReal x685=IKcos(j10);
IkReal x686=((IkReal(1.00000000000000))*(cj7));
IkReal x687=((cj6)*(r02));
IkReal x688=((IkReal(1.00000000000000))*(sj7));
IkReal x689=((r20)*(sj11));
IkReal x690=((cj11)*(r21));
IkReal x691=((r12)*(sj6));
IkReal x692=((cj8)*(x684));
IkReal x693=((r10)*(sj11)*(sj6));
IkReal x694=((IkReal(1.00000000000000))*(x685));
IkReal x695=((cj11)*(cj6)*(r01));
IkReal x696=((sj8)*(x684));
IkReal x697=((cj11)*(r11)*(sj6));
IkReal x698=((cj6)*(r00)*(sj11));
IkReal x699=((sj8)*(x694));
evalcond[0]=((((cj7)*(x691)))+(((IkReal(-1.00000000000000))*(r22)*(x688)))+(((cj7)*(x687)))+(((IkReal(-1.00000000000000))*(cj8)*(x694)))+(((IkReal(-1.00000000000000))*(x696))));
evalcond[1]=((x692)+(((IkReal(-1.00000000000000))*(x688)*(x691)))+(((IkReal(-1.00000000000000))*(r22)*(x686)))+(((IkReal(-1.00000000000000))*(x687)*(x688)))+(((IkReal(-1.00000000000000))*(x699))));
evalcond[2]=((x696)+(((sj7)*(x695)))+(((sj7)*(x693)))+(((cj7)*(x690)))+(((sj7)*(x698)))+(((cj8)*(x685)))+(((cj7)*(x689)))+(((sj7)*(x697))));
evalcond[3]=((((sj7)*(x689)))+(x692)+(((sj7)*(x690)))+(((IkReal(-1.00000000000000))*(x686)*(x698)))+(((IkReal(-1.00000000000000))*(x686)*(x693)))+(((IkReal(-1.00000000000000))*(x686)*(x697)))+(((IkReal(-1.00000000000000))*(x686)*(x695)))+(((IkReal(-1.00000000000000))*(x699))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
IkReal x700=((IkReal(0.0620002000000000))*(sj11));
IkReal x701=((cj7)*(r21));
IkReal x702=((IkReal(1.00000000000000))*(cj11));
IkReal x703=((cj7)*(r20));
IkReal x704=((cj6)*(sj7));
IkReal x705=((r01)*(sj6));
IkReal x706=((IkReal(0.0620002000000000))*(cj11));
IkReal x707=((cj6)*(r10));
IkReal x708=((IkReal(1.00000000000000))*(cj7));
IkReal x709=((cj7)*(sj6));
IkReal x710=((r00)*(sj6));
IkReal x711=((r11)*(sj11));
IkReal x712=((IkReal(1.00000000000000))*(py));
IkReal x713=((sj6)*(sj7));
IkReal x714=((r21)*(sj7));
IkReal x715=((cj6)*(cj7));
IkReal x716=((r01)*(x704));
IkReal x717=((cj11)*(r20)*(sj7));
IkReal x718=((r11)*(x713));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j8)), IkReal(6.28318530717959))));
evalcond[1]=((((IkReal(-1.00000000000000))*(sj11)*(x705)))+(((IkReal(-1.00000000000000))*(x702)*(x707)))+(cj9)+(((cj11)*(x710)))+(((cj6)*(x711))));
evalcond[2]=((IkReal(-0.0100933000000000))+(((IkReal(-1.00000000000000))*(x706)*(x710)))+(((IkReal(-0.00151567000000000))*(sj9)))+(((IkReal(-0.0616850000000000))*(cj9)))+(((IkReal(-1.00000000000000))*(cj6)*(x712)))+(((x700)*(x705)))+(((px)*(sj6)))+(((x706)*(x707)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x700))));
evalcond[3]=((((sj11)*(x716)))+(((IkReal(-1.00000000000000))*(x702)*(x703)))+(((x711)*(x713)))+(((IkReal(-1.00000000000000))*(r10)*(x702)*(x713)))+(((sj11)*(x701)))+(((IkReal(-1.00000000000000))*(r00)*(x702)*(x704))));
evalcond[4]=((((IkReal(-1.00000000000000))*(r20)*(sj7)*(x702)))+(((cj11)*(r00)*(x715)))+(((IkReal(-1.00000000000000))*(sj6)*(x708)*(x711)))+(((IkReal(-1.00000000000000))*(sj9)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj11)*(x708)))+(((sj11)*(x714)))+(((cj11)*(r10)*(x709))));
evalcond[5]=((IkReal(-0.0701403000000000))+(((IkReal(-1.00000000000000))*(r00)*(x706)*(x715)))+(((IkReal(-1.00000000000000))*(pz)*(sj7)))+(((py)*(x709)))+(((IkReal(0.0616850000000000))*(sj9)))+(((IkReal(-1.00000000000000))*(r10)*(x706)*(x709)))+(((r20)*(sj7)*(x706)))+(((IkReal(-1.00000000000000))*(x700)*(x714)))+(((r11)*(x700)*(x709)))+(((px)*(x715)))+(((IkReal(-0.00151567000000000))*(cj9)))+(((r01)*(x700)*(x715))));
evalcond[6]=((IkReal(-0.0320000000000000))+(((IkReal(-1.00000000000000))*(px)*(x704)))+(((IkReal(-1.00000000000000))*(x700)*(x701)))+(((x703)*(x706)))+(((IkReal(-1.00000000000000))*(x700)*(x718)))+(((r10)*(x706)*(x713)))+(((IkReal(-1.00000000000000))*(x712)*(x713)))+(((IkReal(-1.00000000000000))*(pz)*(x708)))+(((r00)*(x704)*(x706)))+(((IkReal(-1.00000000000000))*(x700)*(x716))));
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 dummyeval[1];
IkReal gconst7;
gconst7=IKsign(sj9);
dummyeval[0]=sj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
dummyeval[0]=sj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal evalcond[9];
IkReal x719=((IkReal(0.0620002000000000))*(cj7));
IkReal x720=((r21)*(sj11));
IkReal x721=((IkReal(1.00000000000000))*(cj11));
IkReal x722=((r01)*(sj6));
IkReal x723=((r20)*(sj7));
IkReal x724=((IkReal(1.00000000000000))*(cj6));
IkReal x725=((IkReal(0.0620002000000000))*(cj11));
IkReal x726=((IkReal(1.00000000000000))*(sj7));
IkReal x727=((IkReal(0.0620002000000000))*(sj11));
IkReal x728=((cj6)*(r10));
IkReal x729=((IkReal(1.00000000000000))*(cj7));
IkReal x730=((cj7)*(sj6));
IkReal x731=((r00)*(sj6));
IkReal x732=((r11)*(sj11));
IkReal x733=((IkReal(0.0620002000000000))*(sj7));
IkReal x734=((cj11)*(r10));
IkReal x735=((IkReal(1.00000000000000))*(sj11));
IkReal x736=((cj6)*(r11));
IkReal x737=((r01)*(sj11));
IkReal x738=((cj11)*(cj6)*(r00));
IkReal x739=((r11)*(sj6)*(sj7));
IkReal x740=((r10)*(sj6)*(sj7));
IkReal x741=((cj6)*(sj7)*(x737));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j9)), IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x724))));
evalcond[2]=((IkReal(1.00000000000000))+(((cj6)*(x732)))+(((IkReal(-1.00000000000000))*(x721)*(x728)))+(((cj11)*(x731)))+(((IkReal(-1.00000000000000))*(x722)*(x735))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x731)*(x735)))+(((cj11)*(x736)))+(((IkReal(-1.00000000000000))*(x721)*(x722)))+(((sj11)*(x728))));
evalcond[4]=((IkReal(-0.0717783000000000))+(((IkReal(-1.00000000000000))*(py)*(x724)))+(((x722)*(x727)))+(((px)*(sj6)))+(((x725)*(x728)))+(((IkReal(-1.00000000000000))*(x725)*(x731)))+(((IkReal(-1.00000000000000))*(x727)*(x736))));
evalcond[5]=((((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj7)*(x721)))+(((IkReal(-1.00000000000000))*(x721)*(x740)))+(((sj6)*(sj7)*(x732)))+(((cj7)*(x720)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x721)))+(x741));
evalcond[6]=((((IkReal(-1.00000000000000))*(cj7)*(x724)*(x737)))+(((IkReal(-1.00000000000000))*(sj6)*(x729)*(x732)))+(((cj7)*(x738)))+(((x730)*(x734)))+(((IkReal(-1.00000000000000))*(x721)*(x723)))+(((sj7)*(x720))));
evalcond[7]=((IkReal(-0.0716559700000000))+(((sj6)*(x719)*(x732)))+(((IkReal(-1.00000000000000))*(sj6)*(x719)*(x734)))+(((cj6)*(cj7)*(px)))+(((cj6)*(x719)*(x737)))+(((IkReal(-1.00000000000000))*(x720)*(x733)))+(((py)*(x730)))+(((IkReal(-1.00000000000000))*(x719)*(x738)))+(((x723)*(x725)))+(((IkReal(-1.00000000000000))*(pz)*(x726))));
evalcond[8]=((IkReal(-0.0320000000000000))+(((IkReal(-1.00000000000000))*(px)*(sj7)*(x724)))+(((IkReal(-1.00000000000000))*(x727)*(x739)))+(((cj6)*(r00)*(sj7)*(x725)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj7)*(x727)))+(((IkReal(-1.00000000000000))*(pz)*(x729)))+(((x725)*(x740)))+(((cj11)*(r20)*(x719)))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x726)))+(((IkReal(-1.00000000000000))*(x719)*(x720))));
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 )
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x742=((IkReal(1.00000000000000))*(sj7));
IkReal x743=((cj6)*(r02));
IkReal x744=((IkReal(1.00000000000000))*(cj7));
IkReal x745=((r12)*(sj6));
if( IKabs(((((IkReal(-1.00000000000000))*(x743)*(x744)))+(((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x744)*(x745))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(x742)*(x745)))+(((IkReal(-1.00000000000000))*(x742)*(x743)))+(((IkReal(-1.00000000000000))*(r22)*(x744))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(x743)*(x744)))+(((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x744)*(x745)))))+IKsqr(((((IkReal(-1.00000000000000))*(x742)*(x745)))+(((IkReal(-1.00000000000000))*(x742)*(x743)))+(((IkReal(-1.00000000000000))*(r22)*(x744)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((((IkReal(-1.00000000000000))*(x743)*(x744)))+(((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x744)*(x745)))), ((((IkReal(-1.00000000000000))*(x742)*(x745)))+(((IkReal(-1.00000000000000))*(x742)*(x743)))+(((IkReal(-1.00000000000000))*(r22)*(x744)))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x746=IKsin(j10);
IkReal x747=IKcos(j10);
IkReal x748=((IkReal(1.00000000000000))*(r22));
IkReal x749=((cj7)*(sj11));
IkReal x750=((r10)*(sj6));
IkReal x751=((cj11)*(r21));
IkReal x752=((sj11)*(sj7));
IkReal x753=((cj6)*(r01));
IkReal x754=((r11)*(sj6));
IkReal x755=((cj6)*(r02));
IkReal x756=((r12)*(sj6));
IkReal x757=((IkReal(1.00000000000000))*(sj7));
IkReal x758=((cj11)*(sj7));
IkReal x759=((cj6)*(r00));
IkReal x760=((IkReal(1.00000000000000))*(cj11)*(cj7));
evalcond[0]=((((IkReal(-1.00000000000000))*(sj7)*(x748)))+(((cj7)*(x756)))+(x746)+(((cj7)*(x755))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x747)))+(((IkReal(-1.00000000000000))*(x755)*(x757)))+(((IkReal(-1.00000000000000))*(cj7)*(x748)))+(((IkReal(-1.00000000000000))*(x756)*(x757))));
evalcond[2]=((((x754)*(x758)))+(x746)+(((x750)*(x752)))+(((x752)*(x759)))+(((x753)*(x758)))+(((r20)*(x749)))+(((cj7)*(x751))));
evalcond[3]=((((r20)*(x752)))+(x747)+(((IkReal(-1.00000000000000))*(x754)*(x760)))+(((sj7)*(x751)))+(((IkReal(-1.00000000000000))*(x749)*(x750)))+(((IkReal(-1.00000000000000))*(x753)*(x760)))+(((IkReal(-1.00000000000000))*(x749)*(x759))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
IkReal x761=((IkReal(0.0620002000000000))*(cj7));
IkReal x762=((r21)*(sj11));
IkReal x763=((IkReal(1.00000000000000))*(cj11));
IkReal x764=((r01)*(sj6));
IkReal x765=((r20)*(sj7));
IkReal x766=((IkReal(1.00000000000000))*(cj6));
IkReal x767=((IkReal(0.0620002000000000))*(cj11));
IkReal x768=((IkReal(1.00000000000000))*(sj7));
IkReal x769=((IkReal(0.0620002000000000))*(sj11));
IkReal x770=((cj6)*(r10));
IkReal x771=((IkReal(1.00000000000000))*(cj7));
IkReal x772=((cj7)*(sj6));
IkReal x773=((r00)*(sj6));
IkReal x774=((r11)*(sj11));
IkReal x775=((IkReal(0.0620002000000000))*(sj7));
IkReal x776=((cj11)*(r10));
IkReal x777=((IkReal(1.00000000000000))*(sj11));
IkReal x778=((cj6)*(r11));
IkReal x779=((r01)*(sj11));
IkReal x780=((cj11)*(cj6)*(r00));
IkReal x781=((r11)*(sj6)*(sj7));
IkReal x782=((r10)*(sj6)*(sj7));
IkReal x783=((cj6)*(sj7)*(x779));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j9, IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x766))));
evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x764)*(x777)))+(((IkReal(-1.00000000000000))*(x763)*(x770)))+(((cj11)*(x773)))+(((cj6)*(x774))));
evalcond[3]=((((sj11)*(x770)))+(((IkReal(-1.00000000000000))*(x773)*(x777)))+(((IkReal(-1.00000000000000))*(x763)*(x764)))+(((cj11)*(x778))));
evalcond[4]=((IkReal(0.0515917000000000))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(py)*(x766)))+(((x767)*(x770)))+(((IkReal(-1.00000000000000))*(x769)*(x778)))+(((x764)*(x769)))+(((IkReal(-1.00000000000000))*(x767)*(x773))));
evalcond[5]=((((cj7)*(x762)))+(x783)+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj7)*(x763)))+(((IkReal(-1.00000000000000))*(x763)*(x782)))+(((sj6)*(sj7)*(x774)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x763))));
evalcond[6]=((((IkReal(-1.00000000000000))*(x763)*(x765)))+(((IkReal(-1.00000000000000))*(cj7)*(x766)*(x779)))+(((x772)*(x776)))+(((sj7)*(x762)))+(((cj7)*(x780)))+(((IkReal(-1.00000000000000))*(sj6)*(x771)*(x774))));
evalcond[7]=((IkReal(-0.0686246300000000))+(((IkReal(-1.00000000000000))*(x761)*(x780)))+(((IkReal(-1.00000000000000))*(pz)*(x768)))+(((IkReal(-1.00000000000000))*(sj6)*(x761)*(x776)))+(((sj6)*(x761)*(x774)))+(((IkReal(-1.00000000000000))*(x762)*(x775)))+(((cj6)*(cj7)*(px)))+(((py)*(x772)))+(((cj6)*(x761)*(x779)))+(((x765)*(x767))));
evalcond[8]=((IkReal(-0.0320000000000000))+(((x767)*(x782)))+(((cj6)*(r00)*(sj7)*(x767)))+(((IkReal(-1.00000000000000))*(px)*(sj7)*(x766)))+(((cj11)*(r20)*(x761)))+(((IkReal(-1.00000000000000))*(pz)*(x771)))+(((IkReal(-1.00000000000000))*(x761)*(x762)))+(((IkReal(-1.00000000000000))*(x769)*(x781)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj7)*(x769)))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x768))));
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 )
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x784=((IkReal(1.00000000000000))*(r22));
IkReal x785=((cj6)*(r02));
IkReal x786=((r12)*(sj6));
IkReal x787=((IkReal(1.00000000000000))*(sj7));
if( IKabs(((((cj7)*(x785)))+(((cj7)*(x786)))+(((IkReal(-1.00000000000000))*(sj7)*(x784))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj7)*(x784)))+(((IkReal(-1.00000000000000))*(x786)*(x787)))+(((IkReal(-1.00000000000000))*(x785)*(x787))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj7)*(x785)))+(((cj7)*(x786)))+(((IkReal(-1.00000000000000))*(sj7)*(x784)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj7)*(x784)))+(((IkReal(-1.00000000000000))*(x786)*(x787)))+(((IkReal(-1.00000000000000))*(x785)*(x787)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((((cj7)*(x785)))+(((cj7)*(x786)))+(((IkReal(-1.00000000000000))*(sj7)*(x784)))), ((((IkReal(-1.00000000000000))*(cj7)*(x784)))+(((IkReal(-1.00000000000000))*(x786)*(x787)))+(((IkReal(-1.00000000000000))*(x785)*(x787)))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x788=IKsin(j10);
IkReal x789=((IkReal(1.00000000000000))*(cj6));
IkReal x790=((cj7)*(sj11));
IkReal x791=((r10)*(sj6));
IkReal x792=((cj11)*(r21));
IkReal x793=((sj11)*(sj7));
IkReal x794=((cj11)*(cj7));
IkReal x795=((IkReal(1.00000000000000))*(sj7));
IkReal x796=((r11)*(sj6));
IkReal x797=((r12)*(sj6));
IkReal x798=((cj11)*(sj7));
IkReal x799=((IkReal(1.00000000000000))*(IKcos(j10)));
evalcond[0]=((((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(x788)))+(((cj7)*(x797)))+(((IkReal(-1.00000000000000))*(r22)*(x795))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x799)))+(((IkReal(-1.00000000000000))*(x795)*(x797)))+(((IkReal(-1.00000000000000))*(cj7)*(r22)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x789))));
evalcond[2]=((((x796)*(x798)))+(x788)+(((cj6)*(r00)*(x793)))+(((cj6)*(r01)*(x798)))+(((x791)*(x793)))+(((cj7)*(x792)))+(((r20)*(x790))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r00)*(x789)*(x790)))+(((r20)*(x793)))+(((sj7)*(x792)))+(((IkReal(-1.00000000000000))*(x799)))+(((IkReal(-1.00000000000000))*(x794)*(x796)))+(((IkReal(-1.00000000000000))*(r01)*(x789)*(x794)))+(((IkReal(-1.00000000000000))*(x790)*(x791))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
if( 1 )
{
continue;
} else
{
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x800=((IkReal(1.00000000000000))*(r02));
if( IKabs(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(sj6)*(x800)))+(((cj6)*(r12))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r12)*(sj6)*(sj7)))+(((IkReal(-1.00000000000000))*(cj6)*(sj7)*(x800)))+(((IkReal(-1.00000000000000))*(cj7)*(r22))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(sj6)*(x800)))+(((cj6)*(r12)))))))+IKsqr(((((IkReal(-1.00000000000000))*(r12)*(sj6)*(sj7)))+(((IkReal(-1.00000000000000))*(cj6)*(sj7)*(x800)))+(((IkReal(-1.00000000000000))*(cj7)*(r22)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(sj6)*(x800)))+(((cj6)*(r12)))))), ((((IkReal(-1.00000000000000))*(r12)*(sj6)*(sj7)))+(((IkReal(-1.00000000000000))*(cj6)*(sj7)*(x800)))+(((IkReal(-1.00000000000000))*(cj7)*(r22)))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[6];
IkReal x801=IKsin(j10);
IkReal x802=IKcos(j10);
IkReal x803=((IkReal(1.00000000000000))*(sj6));
IkReal x804=((cj7)*(sj11));
IkReal x805=((cj11)*(r01));
IkReal x806=((IkReal(1.00000000000000))*(cj7));
IkReal x807=((IkReal(1.00000000000000))*(cj6));
IkReal x808=((cj11)*(r21));
IkReal x809=((r10)*(sj11));
IkReal x810=((sj6)*(sj7));
IkReal x811=((cj11)*(r11));
IkReal x812=((r00)*(sj11));
IkReal x813=((cj6)*(sj7));
evalcond[0]=((((r02)*(sj6)))+(((sj9)*(x801)))+(((IkReal(-1.00000000000000))*(r12)*(x807))));
evalcond[1]=((((cj7)*(r12)*(sj6)))+(((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj9)*(x801))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x803)*(x812)))+(((sj9)*(x802)))+(((IkReal(-1.00000000000000))*(x803)*(x805)))+(((cj6)*(x811)))+(((cj6)*(x809))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj7)*(x803)))+(((IkReal(-1.00000000000000))*(x802)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x807)))+(((IkReal(-1.00000000000000))*(r22)*(x806))));
evalcond[4]=((((x812)*(x813)))+(x801)+(((x805)*(x813)))+(((cj7)*(x808)))+(((x810)*(x811)))+(((x809)*(x810)))+(((r20)*(x804))));
evalcond[5]=((((IkReal(-1.00000000000000))*(cj6)*(x805)*(x806)))+(((r20)*(sj11)*(sj7)))+(((cj9)*(x802)))+(((IkReal(-1.00000000000000))*(cj7)*(x803)*(x811)))+(((IkReal(-1.00000000000000))*(r10)*(x803)*(x804)))+(((IkReal(-1.00000000000000))*(r00)*(x804)*(x807)))+(((sj7)*(x808))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x814=((IkReal(1.00000000000000))*(cj6));
if( IKabs(((gconst7)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x814)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x814)))+(((r00)*(sj11)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst7)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6)))))), ((gconst7)*(((((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x814)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x814)))+(((r00)*(sj11)*(sj6)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[6];
IkReal x815=IKsin(j10);
IkReal x816=IKcos(j10);
IkReal x817=((IkReal(1.00000000000000))*(sj6));
IkReal x818=((cj7)*(sj11));
IkReal x819=((cj11)*(r01));
IkReal x820=((IkReal(1.00000000000000))*(cj7));
IkReal x821=((IkReal(1.00000000000000))*(cj6));
IkReal x822=((cj11)*(r21));
IkReal x823=((r10)*(sj11));
IkReal x824=((sj6)*(sj7));
IkReal x825=((cj11)*(r11));
IkReal x826=((r00)*(sj11));
IkReal x827=((cj6)*(sj7));
evalcond[0]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x821)))+(((sj9)*(x815))));
evalcond[1]=((((cj7)*(r12)*(sj6)))+(((cj9)*(x815)))+(((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(r22)*(sj7))));
evalcond[2]=((((cj6)*(x825)))+(((sj9)*(x816)))+(((IkReal(-1.00000000000000))*(x817)*(x819)))+(((IkReal(-1.00000000000000))*(x817)*(x826)))+(((cj6)*(x823))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r02)*(sj7)*(x821)))+(((IkReal(-1.00000000000000))*(r12)*(sj7)*(x817)))+(((IkReal(-1.00000000000000))*(x816)))+(((IkReal(-1.00000000000000))*(r22)*(x820))));
evalcond[4]=((((x824)*(x825)))+(((x823)*(x824)))+(((cj7)*(x822)))+(((x826)*(x827)))+(((r20)*(x818)))+(x815)+(((x819)*(x827))));
evalcond[5]=((((IkReal(-1.00000000000000))*(r00)*(x818)*(x821)))+(((IkReal(-1.00000000000000))*(cj6)*(x819)*(x820)))+(((IkReal(-1.00000000000000))*(r10)*(x817)*(x818)))+(((r20)*(sj11)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x817)*(x825)))+(((sj7)*(x822)))+(((cj9)*(x816))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
IkReal x828=((IkReal(0.0620002000000000))*(sj11));
IkReal x829=((cj7)*(r21));
IkReal x830=((IkReal(1.00000000000000))*(cj11));
IkReal x831=((cj7)*(r20));
IkReal x832=((cj6)*(sj7));
IkReal x833=((r01)*(sj6));
IkReal x834=((IkReal(0.0620002000000000))*(cj11));
IkReal x835=((cj6)*(r10));
IkReal x836=((IkReal(1.00000000000000))*(cj7));
IkReal x837=((cj7)*(sj6));
IkReal x838=((r00)*(sj6));
IkReal x839=((r11)*(sj11));
IkReal x840=((IkReal(1.00000000000000))*(py));
IkReal x841=((sj6)*(sj7));
IkReal x842=((r21)*(sj7));
IkReal x843=((cj6)*(cj7));
IkReal x844=((r01)*(x832));
IkReal x845=((cj11)*(r20)*(sj7));
IkReal x846=((r11)*(x841));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j8)), IkReal(6.28318530717959))));
evalcond[1]=((((IkReal(-1.00000000000000))*(sj11)*(x833)))+(((IkReal(-1.00000000000000))*(x830)*(x835)))+(cj9)+(((cj11)*(x838)))+(((cj6)*(x839))));
evalcond[2]=((IkReal(-0.0100933000000000))+(((IkReal(-0.00151567000000000))*(sj9)))+(((IkReal(-0.0616850000000000))*(cj9)))+(((x834)*(x835)))+(((IkReal(-1.00000000000000))*(x834)*(x838)))+(((px)*(sj6)))+(((x828)*(x833)))+(((IkReal(-1.00000000000000))*(cj6)*(x840)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x828))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(x830)*(x841)))+(((sj11)*(x844)))+(((sj11)*(x829)))+(((IkReal(-1.00000000000000))*(x830)*(x831)))+(((x839)*(x841)))+(((IkReal(-1.00000000000000))*(r00)*(x830)*(x832))));
evalcond[4]=((sj9)+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj11)*(x836)))+(((IkReal(-1.00000000000000))*(sj6)*(x836)*(x839)))+(((cj11)*(r10)*(x837)))+(((IkReal(-1.00000000000000))*(r20)*(sj7)*(x830)))+(((sj11)*(x842)))+(((cj11)*(r00)*(x843))));
evalcond[5]=((IkReal(0.0701403000000000))+(((r01)*(x828)*(x843)))+(((py)*(x837)))+(((IkReal(-1.00000000000000))*(pz)*(sj7)))+(((r20)*(sj7)*(x834)))+(((r11)*(x828)*(x837)))+(((IkReal(0.00151567000000000))*(cj9)))+(((IkReal(-1.00000000000000))*(r10)*(x834)*(x837)))+(((IkReal(-1.00000000000000))*(x828)*(x842)))+(((IkReal(-0.0616850000000000))*(sj9)))+(((IkReal(-1.00000000000000))*(r00)*(x834)*(x843)))+(((px)*(x843))));
evalcond[6]=((IkReal(0.572000000000000))+(((r10)*(x834)*(x841)))+(((x831)*(x834)))+(((IkReal(-1.00000000000000))*(x828)*(x829)))+(((IkReal(-1.00000000000000))*(x828)*(x846)))+(((IkReal(-1.00000000000000))*(x840)*(x841)))+(((IkReal(-1.00000000000000))*(pz)*(x836)))+(((IkReal(-1.00000000000000))*(x828)*(x844)))+(((r00)*(x832)*(x834)))+(((IkReal(-1.00000000000000))*(px)*(x832))));
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 dummyeval[1];
IkReal gconst8;
gconst8=IKsign(sj9);
dummyeval[0]=sj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
dummyeval[0]=sj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal evalcond[9];
IkReal x847=((IkReal(0.0620002000000000))*(cj7));
IkReal x848=((r21)*(sj11));
IkReal x849=((IkReal(1.00000000000000))*(cj11));
IkReal x850=((r01)*(sj6));
IkReal x851=((r20)*(sj7));
IkReal x852=((IkReal(1.00000000000000))*(cj6));
IkReal x853=((IkReal(0.0620002000000000))*(cj11));
IkReal x854=((IkReal(1.00000000000000))*(sj7));
IkReal x855=((IkReal(0.0620002000000000))*(sj11));
IkReal x856=((cj6)*(r10));
IkReal x857=((IkReal(1.00000000000000))*(cj7));
IkReal x858=((cj7)*(sj6));
IkReal x859=((r00)*(sj6));
IkReal x860=((r11)*(sj11));
IkReal x861=((IkReal(0.0620002000000000))*(sj7));
IkReal x862=((cj11)*(r10));
IkReal x863=((IkReal(1.00000000000000))*(sj11));
IkReal x864=((cj6)*(r11));
IkReal x865=((r01)*(sj11));
IkReal x866=((cj11)*(cj6)*(r00));
IkReal x867=((r11)*(sj6)*(sj7));
IkReal x868=((r10)*(sj6)*(sj7));
IkReal x869=((cj6)*(sj7)*(x865));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j9)), IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x852))));
evalcond[2]=((IkReal(1.00000000000000))+(((cj11)*(x859)))+(((IkReal(-1.00000000000000))*(x850)*(x863)))+(((cj6)*(x860)))+(((IkReal(-1.00000000000000))*(x849)*(x856))));
evalcond[3]=((((sj11)*(x856)))+(((IkReal(-1.00000000000000))*(x859)*(x863)))+(((IkReal(-1.00000000000000))*(x849)*(x850)))+(((cj11)*(x864))));
evalcond[4]=((IkReal(-0.0717783000000000))+(((IkReal(-1.00000000000000))*(x855)*(x864)))+(((px)*(sj6)))+(((x853)*(x856)))+(((x850)*(x855)))+(((IkReal(-1.00000000000000))*(x853)*(x859)))+(((IkReal(-1.00000000000000))*(py)*(x852))));
evalcond[5]=((((sj6)*(sj7)*(x860)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj7)*(x849)))+(x869)+(((IkReal(-1.00000000000000))*(x849)*(x868)))+(((cj7)*(x848)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x849))));
evalcond[6]=((((IkReal(-1.00000000000000))*(x849)*(x851)))+(((IkReal(-1.00000000000000))*(cj7)*(x852)*(x865)))+(((sj7)*(x848)))+(((x858)*(x862)))+(((cj7)*(x866)))+(((IkReal(-1.00000000000000))*(sj6)*(x857)*(x860))));
evalcond[7]=((IkReal(0.0716559700000000))+(((sj6)*(x847)*(x860)))+(((IkReal(-1.00000000000000))*(pz)*(x854)))+(((x851)*(x853)))+(((cj6)*(cj7)*(px)))+(((cj6)*(x847)*(x865)))+(((IkReal(-1.00000000000000))*(x847)*(x866)))+(((IkReal(-1.00000000000000))*(x848)*(x861)))+(((IkReal(-1.00000000000000))*(sj6)*(x847)*(x862)))+(((py)*(x858))));
evalcond[8]=((IkReal(0.572000000000000))+(((cj6)*(r00)*(sj7)*(x853)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj7)*(x855)))+(((cj11)*(r20)*(x847)))+(((IkReal(-1.00000000000000))*(pz)*(x857)))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x854)))+(((x853)*(x868)))+(((IkReal(-1.00000000000000))*(x847)*(x848)))+(((IkReal(-1.00000000000000))*(px)*(sj7)*(x852)))+(((IkReal(-1.00000000000000))*(x855)*(x867))));
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 )
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x870=((r12)*(sj6));
IkReal x871=((cj6)*(r02));
if( IKabs(((((cj7)*(x871)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(x870))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj7)*(r22)))+(((sj7)*(x871)))+(((sj7)*(x870))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj7)*(x871)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(x870)))))+IKsqr(((((cj7)*(r22)))+(((sj7)*(x871)))+(((sj7)*(x870)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((((cj7)*(x871)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(x870)))), ((((cj7)*(r22)))+(((sj7)*(x871)))+(((sj7)*(x870)))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x872=IKcos(j10);
IkReal x873=((IkReal(1.00000000000000))*(cj6));
IkReal x874=((cj7)*(sj11));
IkReal x875=((r10)*(sj6));
IkReal x876=((cj11)*(r21));
IkReal x877=((sj11)*(sj7));
IkReal x878=((cj11)*(cj7));
IkReal x879=((IkReal(1.00000000000000))*(sj7));
IkReal x880=((r11)*(sj6));
IkReal x881=((r12)*(sj6));
IkReal x882=((cj11)*(sj7));
IkReal x883=((IkReal(1.00000000000000))*(IKsin(j10)));
evalcond[0]=((((cj7)*(x881)))+(((IkReal(-1.00000000000000))*(x883)))+(((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(r22)*(x879))));
evalcond[1]=((((IkReal(-1.00000000000000))*(r02)*(sj7)*(x873)))+(x872)+(((IkReal(-1.00000000000000))*(x879)*(x881)))+(((IkReal(-1.00000000000000))*(cj7)*(r22))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x883)))+(((cj6)*(r00)*(x877)))+(((cj7)*(x876)))+(((x875)*(x877)))+(((x880)*(x882)))+(((cj6)*(r01)*(x882)))+(((r20)*(x874))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x874)*(x875)))+(((sj7)*(x876)))+(((IkReal(-1.00000000000000))*(x872)))+(((r20)*(x877)))+(((IkReal(-1.00000000000000))*(x878)*(x880)))+(((IkReal(-1.00000000000000))*(r00)*(x873)*(x874)))+(((IkReal(-1.00000000000000))*(r01)*(x873)*(x878))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
IkReal x884=((IkReal(0.0620002000000000))*(cj7));
IkReal x885=((r21)*(sj11));
IkReal x886=((IkReal(1.00000000000000))*(cj11));
IkReal x887=((r01)*(sj6));
IkReal x888=((r20)*(sj7));
IkReal x889=((IkReal(1.00000000000000))*(cj6));
IkReal x890=((IkReal(0.0620002000000000))*(cj11));
IkReal x891=((IkReal(1.00000000000000))*(sj7));
IkReal x892=((IkReal(0.0620002000000000))*(sj11));
IkReal x893=((cj6)*(r10));
IkReal x894=((IkReal(1.00000000000000))*(cj7));
IkReal x895=((cj7)*(sj6));
IkReal x896=((r00)*(sj6));
IkReal x897=((r11)*(sj11));
IkReal x898=((IkReal(0.0620002000000000))*(sj7));
IkReal x899=((cj11)*(r10));
IkReal x900=((IkReal(1.00000000000000))*(sj11));
IkReal x901=((cj6)*(r11));
IkReal x902=((r01)*(sj11));
IkReal x903=((cj11)*(cj6)*(r00));
IkReal x904=((r11)*(sj6)*(sj7));
IkReal x905=((r10)*(sj6)*(sj7));
IkReal x906=((cj6)*(sj7)*(x902));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j9, IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x889))));
evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x886)*(x893)))+(((cj11)*(x896)))+(((cj6)*(x897)))+(((IkReal(-1.00000000000000))*(x887)*(x900))));
evalcond[3]=((((cj11)*(x901)))+(((sj11)*(x893)))+(((IkReal(-1.00000000000000))*(x886)*(x887)))+(((IkReal(-1.00000000000000))*(x896)*(x900))));
evalcond[4]=((IkReal(0.0515917000000000))+(((x887)*(x892)))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(x892)*(x901)))+(((x890)*(x893)))+(((IkReal(-1.00000000000000))*(py)*(x889)))+(((IkReal(-1.00000000000000))*(x890)*(x896))));
evalcond[5]=((((IkReal(-1.00000000000000))*(x886)*(x905)))+(((cj7)*(x885)))+(((sj6)*(sj7)*(x897)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x886)))+(x906)+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj7)*(x886))));
evalcond[6]=((((x895)*(x899)))+(((IkReal(-1.00000000000000))*(cj7)*(x889)*(x902)))+(((cj7)*(x903)))+(((IkReal(-1.00000000000000))*(x886)*(x888)))+(((sj7)*(x885)))+(((IkReal(-1.00000000000000))*(sj6)*(x894)*(x897))));
evalcond[7]=((IkReal(0.0686246300000000))+(((IkReal(-1.00000000000000))*(x884)*(x903)))+(((sj6)*(x884)*(x897)))+(((py)*(x895)))+(((cj6)*(cj7)*(px)))+(((IkReal(-1.00000000000000))*(pz)*(x891)))+(((IkReal(-1.00000000000000))*(sj6)*(x884)*(x899)))+(((IkReal(-1.00000000000000))*(x885)*(x898)))+(((cj6)*(x884)*(x902)))+(((x888)*(x890))));
evalcond[8]=((IkReal(0.572000000000000))+(((IkReal(-1.00000000000000))*(px)*(sj7)*(x889)))+(((cj11)*(r20)*(x884)))+(((IkReal(-1.00000000000000))*(x884)*(x885)))+(((x890)*(x905)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj7)*(x892)))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x891)))+(((IkReal(-1.00000000000000))*(pz)*(x894)))+(((IkReal(-1.00000000000000))*(x892)*(x904)))+(((cj6)*(r00)*(sj7)*(x890))));
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 )
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x907=((r12)*(sj6));
IkReal x908=((cj6)*(r02));
IkReal x909=((IkReal(1.00000000000000))*(cj7));
if( IKabs(((((IkReal(-1.00000000000000))*(x907)*(x909)))+(((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x908)*(x909))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj7)*(r22)))+(((sj7)*(x908)))+(((sj7)*(x907))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(x907)*(x909)))+(((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x908)*(x909)))))+IKsqr(((((cj7)*(r22)))+(((sj7)*(x908)))+(((sj7)*(x907)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((((IkReal(-1.00000000000000))*(x907)*(x909)))+(((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x908)*(x909)))), ((((cj7)*(r22)))+(((sj7)*(x908)))+(((sj7)*(x907)))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x910=IKsin(j10);
IkReal x911=IKcos(j10);
IkReal x912=((IkReal(1.00000000000000))*(r22));
IkReal x913=((cj6)*(r00));
IkReal x914=((r10)*(sj6));
IkReal x915=((r20)*(sj11));
IkReal x916=((cj11)*(r21));
IkReal x917=((sj11)*(sj7));
IkReal x918=((cj6)*(r01));
IkReal x919=((r11)*(sj6));
IkReal x920=((cj6)*(r02));
IkReal x921=((r12)*(sj6));
IkReal x922=((IkReal(1.00000000000000))*(sj7));
IkReal x923=((cj11)*(sj7));
IkReal x924=((IkReal(1.00000000000000))*(cj7)*(sj11));
IkReal x925=((IkReal(1.00000000000000))*(cj11)*(cj7));
evalcond[0]=((((cj7)*(x921)))+(((IkReal(-1.00000000000000))*(sj7)*(x912)))+(((cj7)*(x920)))+(x910));
evalcond[1]=((((IkReal(-1.00000000000000))*(cj7)*(x912)))+(((IkReal(-1.00000000000000))*(x921)*(x922)))+(x911)+(((IkReal(-1.00000000000000))*(x920)*(x922))));
evalcond[2]=((((x919)*(x923)))+(((x914)*(x917)))+(((cj7)*(x916)))+(((x918)*(x923)))+(((IkReal(-1.00000000000000))*(x910)))+(((cj7)*(x915)))+(((x913)*(x917))));
evalcond[3]=((((sj7)*(x916)))+(((IkReal(-1.00000000000000))*(x919)*(x925)))+(((IkReal(-1.00000000000000))*(x914)*(x924)))+(((sj7)*(x915)))+(x911)+(((IkReal(-1.00000000000000))*(x918)*(x925)))+(((IkReal(-1.00000000000000))*(x913)*(x924))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
if( 1 )
{
continue;
} else
{
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
if( IKabs(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj7)*(r22)))+(((cj6)*(r02)*(sj7)))+(((r12)*(sj6)*(sj7))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6)))))))+IKsqr(((((cj7)*(r22)))+(((cj6)*(r02)*(sj7)))+(((r12)*(sj6)*(sj7)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6)))))), ((((cj7)*(r22)))+(((cj6)*(r02)*(sj7)))+(((r12)*(sj6)*(sj7)))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[6];
IkReal x926=IKsin(j10);
IkReal x927=IKcos(j10);
IkReal x928=((IkReal(1.00000000000000))*(cj9));
IkReal x929=((IkReal(1.00000000000000))*(sj6));
IkReal x930=((cj7)*(sj11));
IkReal x931=((cj11)*(r01));
IkReal x932=((IkReal(1.00000000000000))*(cj7));
IkReal x933=((IkReal(1.00000000000000))*(cj6));
IkReal x934=((cj11)*(r21));
IkReal x935=((r10)*(sj11));
IkReal x936=((sj6)*(sj7));
IkReal x937=((cj11)*(r11));
IkReal x938=((r00)*(sj11));
IkReal x939=((cj6)*(sj7));
evalcond[0]=((((r02)*(sj6)))+(((sj9)*(x926)))+(((IkReal(-1.00000000000000))*(r12)*(x933))));
evalcond[1]=((((cj7)*(r12)*(sj6)))+(((IkReal(-1.00000000000000))*(x926)*(x928)))+(((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(r22)*(sj7))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x929)*(x938)))+(((sj9)*(x927)))+(((IkReal(-1.00000000000000))*(x929)*(x931)))+(((cj6)*(x937)))+(((cj6)*(x935))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj7)*(x929)))+(x927)+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x933)))+(((IkReal(-1.00000000000000))*(r22)*(x932))));
evalcond[4]=((((IkReal(-1.00000000000000))*(x926)))+(((x931)*(x939)))+(((x935)*(x936)))+(((x938)*(x939)))+(((r20)*(x930)))+(((cj7)*(x934)))+(((x936)*(x937))));
evalcond[5]=((((IkReal(-1.00000000000000))*(cj6)*(x931)*(x932)))+(((IkReal(-1.00000000000000))*(r10)*(x929)*(x930)))+(((IkReal(-1.00000000000000))*(cj7)*(x929)*(x937)))+(((r20)*(sj11)*(sj7)))+(((IkReal(-1.00000000000000))*(r00)*(x930)*(x933)))+(((IkReal(-1.00000000000000))*(x927)*(x928)))+(((sj7)*(x934))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x940=((IkReal(1.00000000000000))*(cj6));
if( IKabs(((gconst8)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst8)*(((((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x940)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x940)))+(((r00)*(sj11)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst8)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6)))))), ((gconst8)*(((((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x940)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x940)))+(((r00)*(sj11)*(sj6)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[6];
IkReal x941=IKsin(j10);
IkReal x942=IKcos(j10);
IkReal x943=((IkReal(1.00000000000000))*(cj9));
IkReal x944=((IkReal(1.00000000000000))*(sj6));
IkReal x945=((cj7)*(sj11));
IkReal x946=((cj11)*(r01));
IkReal x947=((IkReal(1.00000000000000))*(cj7));
IkReal x948=((IkReal(1.00000000000000))*(cj6));
IkReal x949=((cj11)*(r21));
IkReal x950=((r10)*(sj11));
IkReal x951=((sj6)*(sj7));
IkReal x952=((cj11)*(r11));
IkReal x953=((r00)*(sj11));
IkReal x954=((cj6)*(sj7));
evalcond[0]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x948)))+(((sj9)*(x941))));
evalcond[1]=((((cj7)*(r12)*(sj6)))+(((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x941)*(x943))));
evalcond[2]=((((cj6)*(x950)))+(((IkReal(-1.00000000000000))*(x944)*(x946)))+(((cj6)*(x952)))+(((sj9)*(x942)))+(((IkReal(-1.00000000000000))*(x944)*(x953))));
evalcond[3]=((x942)+(((IkReal(-1.00000000000000))*(r22)*(x947)))+(((IkReal(-1.00000000000000))*(r12)*(sj7)*(x944)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x948))));
evalcond[4]=((((x953)*(x954)))+(((x950)*(x951)))+(((r20)*(x945)))+(((x951)*(x952)))+(((IkReal(-1.00000000000000))*(x941)))+(((x946)*(x954)))+(((cj7)*(x949))));
evalcond[5]=((((sj7)*(x949)))+(((IkReal(-1.00000000000000))*(cj6)*(x946)*(x947)))+(((r20)*(sj11)*(sj7)))+(((IkReal(-1.00000000000000))*(r00)*(x945)*(x948)))+(((IkReal(-1.00000000000000))*(x942)*(x943)))+(((IkReal(-1.00000000000000))*(cj7)*(x944)*(x952)))+(((IkReal(-1.00000000000000))*(r10)*(x944)*(x945))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
if( 1 )
{
continue;
} else
{
}
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x955=((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30);
IkReal x956=((cj9)*(sj8));
IkReal x957=((cj6)*(r12));
IkReal x958=((cj7)*(sj9));
IkReal x959=((IkReal(1.00000000000000))*(r02)*(sj6));
if( IKabs(((x955)*(((x957)+(((IkReal(-1.00000000000000))*(x959))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x955)*(((IKabs(cj8) != 0)?((IkReal)1/(cj8)):(IkReal)1.0e30))*(((((x956)*(x957)))+(((cj6)*(r02)*(x958)))+(((r12)*(sj6)*(x958)))+(((IkReal(-1.00000000000000))*(x956)*(x959)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(sj9))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x955)*(((x957)+(((IkReal(-1.00000000000000))*(x959)))))))+IKsqr(((x955)*(((IKabs(cj8) != 0)?((IkReal)1/(cj8)):(IkReal)1.0e30))*(((((x956)*(x957)))+(((cj6)*(r02)*(x958)))+(((r12)*(sj6)*(x958)))+(((IkReal(-1.00000000000000))*(x956)*(x959)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(sj9)))))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((x955)*(((x957)+(((IkReal(-1.00000000000000))*(x959)))))), ((x955)*(((IKabs(cj8) != 0)?((IkReal)1/(cj8)):(IkReal)1.0e30))*(((((x956)*(x957)))+(((cj6)*(r02)*(x958)))+(((r12)*(sj6)*(x958)))+(((IkReal(-1.00000000000000))*(x956)*(x959)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(sj9)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[6];
IkReal x960=IKsin(j10);
IkReal x961=IKcos(j10);
IkReal x962=((cj9)*(sj8));
IkReal x963=((cj6)*(cj7));
IkReal x964=((IkReal(1.00000000000000))*(cj7));
IkReal x965=((IkReal(1.00000000000000))*(cj6));
IkReal x966=((r20)*(sj11));
IkReal x967=((cj11)*(r21));
IkReal x968=((cj6)*(sj11));
IkReal x969=((cj11)*(cj6));
IkReal x970=((r12)*(sj6));
IkReal x971=((IkReal(1.00000000000000))*(sj7));
IkReal x972=((cj8)*(x960));
IkReal x973=((IkReal(1.00000000000000))*(cj11)*(r01));
IkReal x974=((r10)*(sj11)*(sj6));
IkReal x975=((cj11)*(r11)*(sj6));
IkReal x976=((IkReal(1.00000000000000))*(r00)*(sj11));
IkReal x977=((IkReal(1.00000000000000))*(cj8)*(x961));
evalcond[0]=((((r02)*(sj6)))+(((sj9)*(x960)))+(((IkReal(-1.00000000000000))*(r12)*(x965))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x977)))+(((r02)*(x963)))+(((x960)*(x962)))+(((cj7)*(x970)))+(((IkReal(-1.00000000000000))*(r22)*(x971))));
evalcond[2]=((((IkReal(-1.00000000000000))*(sj6)*(x973)))+(((r11)*(x969)))+(((sj9)*(x961)))+(((r10)*(x968)))+(((IkReal(-1.00000000000000))*(sj6)*(x976))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x970)*(x971)))+(((IkReal(-1.00000000000000))*(r22)*(x964)))+(((IkReal(-1.00000000000000))*(sj8)*(x961)))+(((IkReal(-1.00000000000000))*(cj9)*(x972)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x965))));
evalcond[4]=((((r00)*(sj7)*(x968)))+(((IkReal(-1.00000000000000))*(cj9)*(x977)))+(((r01)*(sj7)*(x969)))+(((cj7)*(x966)))+(((sj8)*(x960)))+(((sj7)*(x975)))+(((sj7)*(x974)))+(((cj7)*(x967))));
evalcond[5]=((x972)+(((IkReal(-1.00000000000000))*(x964)*(x974)))+(((IkReal(-1.00000000000000))*(x963)*(x973)))+(((sj7)*(x967)))+(((x961)*(x962)))+(((IkReal(-1.00000000000000))*(x963)*(x976)))+(((sj7)*(x966)))+(((IkReal(-1.00000000000000))*(x964)*(x975))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x978=((IkReal(1.00000000000000))*(cj6));
if( IKabs(((gconst2)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x978)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x978)))+(((r00)*(sj11)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst2)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6)))))), ((gconst2)*(((((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x978)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x978)))+(((r00)*(sj11)*(sj6)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[6];
IkReal x979=IKsin(j10);
IkReal x980=IKcos(j10);
IkReal x981=((cj9)*(sj8));
IkReal x982=((cj6)*(cj7));
IkReal x983=((IkReal(1.00000000000000))*(cj7));
IkReal x984=((IkReal(1.00000000000000))*(cj6));
IkReal x985=((r20)*(sj11));
IkReal x986=((cj11)*(r21));
IkReal x987=((cj6)*(sj11));
IkReal x988=((cj11)*(cj6));
IkReal x989=((r12)*(sj6));
IkReal x990=((IkReal(1.00000000000000))*(sj7));
IkReal x991=((cj8)*(x979));
IkReal x992=((IkReal(1.00000000000000))*(cj11)*(r01));
IkReal x993=((r10)*(sj11)*(sj6));
IkReal x994=((cj11)*(r11)*(sj6));
IkReal x995=((IkReal(1.00000000000000))*(r00)*(sj11));
IkReal x996=((IkReal(1.00000000000000))*(cj8)*(x980));
evalcond[0]=((((r02)*(sj6)))+(((sj9)*(x979)))+(((IkReal(-1.00000000000000))*(r12)*(x984))));
evalcond[1]=((((x979)*(x981)))+(((r02)*(x982)))+(((IkReal(-1.00000000000000))*(x996)))+(((IkReal(-1.00000000000000))*(r22)*(x990)))+(((cj7)*(x989))));
evalcond[2]=((((IkReal(-1.00000000000000))*(sj6)*(x995)))+(((r10)*(x987)))+(((sj9)*(x980)))+(((r11)*(x988)))+(((IkReal(-1.00000000000000))*(sj6)*(x992))));
evalcond[3]=((((IkReal(-1.00000000000000))*(cj9)*(x991)))+(((IkReal(-1.00000000000000))*(sj8)*(x980)))+(((IkReal(-1.00000000000000))*(x989)*(x990)))+(((IkReal(-1.00000000000000))*(r22)*(x983)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x984))));
evalcond[4]=((((r00)*(sj7)*(x987)))+(((IkReal(-1.00000000000000))*(cj9)*(x996)))+(((cj7)*(x985)))+(((sj7)*(x993)))+(((r01)*(sj7)*(x988)))+(((cj7)*(x986)))+(((sj7)*(x994)))+(((sj8)*(x979))));
evalcond[5]=((((sj7)*(x985)))+(((sj7)*(x986)))+(((IkReal(-1.00000000000000))*(x983)*(x993)))+(((IkReal(-1.00000000000000))*(x982)*(x995)))+(((x980)*(x981)))+(((IkReal(-1.00000000000000))*(x982)*(x992)))+(((IkReal(-1.00000000000000))*(x983)*(x994)))+(x991));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x997=((sj11)*(sj7));
IkReal x998=((cj11)*(cj7));
IkReal x999=((r10)*(sj6));
IkReal x1000=((cj6)*(r00));
IkReal x1001=((cj11)*(sj7));
IkReal x1002=((cj7)*(sj11));
IkReal x1003=((IkReal(1.00000000000000))*(r11)*(sj6));
IkReal x1004=((IkReal(1.00000000000000))*(cj6)*(r01));
if( IKabs(((gconst0)*(((((x1000)*(x998)))+(((IkReal(-1.00000000000000))*(r20)*(x1001)))+(((IkReal(-1.00000000000000))*(x1002)*(x1003)))+(((IkReal(-1.00000000000000))*(x1002)*(x1004)))+(((r21)*(x997)))+(((x998)*(x999))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(r21)*(x1002)))+(((IkReal(-1.00000000000000))*(x1003)*(x997)))+(((IkReal(-1.00000000000000))*(x1004)*(x997)))+(((x1001)*(x999)))+(((r20)*(x998)))+(((x1000)*(x1001))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j8array[0]=IKatan2(((gconst0)*(((((x1000)*(x998)))+(((IkReal(-1.00000000000000))*(r20)*(x1001)))+(((IkReal(-1.00000000000000))*(x1002)*(x1003)))+(((IkReal(-1.00000000000000))*(x1002)*(x1004)))+(((r21)*(x997)))+(((x998)*(x999)))))), ((gconst0)*(((((IkReal(-1.00000000000000))*(r21)*(x1002)))+(((IkReal(-1.00000000000000))*(x1003)*(x997)))+(((IkReal(-1.00000000000000))*(x1004)*(x997)))+(((x1001)*(x999)))+(((r20)*(x998)))+(((x1000)*(x1001)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[4];
IkReal x1005=IKsin(j8);
IkReal x1006=IKcos(j8);
IkReal x1007=((IkReal(0.0620002000000000))*(sj11));
IkReal x1008=((cj7)*(r21));
IkReal x1009=((IkReal(1.00000000000000))*(cj11));
IkReal x1010=((cj7)*(r20));
IkReal x1011=((IkReal(0.00151567000000000))*(cj9));
IkReal x1012=((r21)*(sj7));
IkReal x1013=((IkReal(1.00000000000000))*(cj7));
IkReal x1014=((cj7)*(sj6));
IkReal x1015=((IkReal(0.0620002000000000))*(cj11));
IkReal x1016=((IkReal(1.00000000000000))*(sj7));
IkReal x1017=((cj6)*(px));
IkReal x1018=((cj6)*(r01));
IkReal x1019=((cj11)*(r20)*(sj7));
IkReal x1020=((sj7)*(x1018));
IkReal x1021=((sj9)*(x1006));
IkReal x1022=((cj6)*(r00)*(sj7));
IkReal x1023=((r11)*(sj6)*(sj7));
IkReal x1024=((sj9)*(x1005));
IkReal x1025=((r10)*(sj6)*(sj7));
IkReal x1026=((cj6)*(cj7)*(r00));
evalcond[0]=((((IkReal(-1.00000000000000))*(x1009)*(x1022)))+(((sj11)*(x1008)))+(((IkReal(-1.00000000000000))*(x1009)*(x1010)))+(x1021)+(((IkReal(-1.00000000000000))*(x1009)*(x1025)))+(((sj11)*(x1023)))+(((sj11)*(x1020))));
evalcond[1]=((((sj11)*(x1012)))+(((cj11)*(r10)*(x1014)))+(((IkReal(-1.00000000000000))*(sj11)*(x1013)*(x1018)))+(((cj11)*(x1026)))+(((IkReal(-1.00000000000000))*(x1024)))+(((IkReal(-1.00000000000000))*(r11)*(sj11)*(sj6)*(x1013)))+(((IkReal(-1.00000000000000))*(r20)*(sj7)*(x1009))));
evalcond[2]=((((r11)*(x1007)*(x1014)))+(((cj7)*(x1017)))+(((IkReal(0.0616850000000000))*(x1024)))+(((IkReal(-0.0701403000000000))*(x1005)))+(((IkReal(-1.00000000000000))*(r10)*(x1014)*(x1015)))+(((IkReal(-1.00000000000000))*(pz)*(x1016)))+(((cj7)*(x1007)*(x1018)))+(((IkReal(-0.302000000000000))*(x1006)))+(((IkReal(-1.00000000000000))*(x1005)*(x1011)))+(((r20)*(sj7)*(x1015)))+(((py)*(x1014)))+(((IkReal(-1.00000000000000))*(x1007)*(x1012)))+(((IkReal(-1.00000000000000))*(x1015)*(x1026))));
evalcond[3]=((IkReal(0.270000000000000))+(((x1006)*(x1011)))+(((IkReal(-1.00000000000000))*(x1007)*(x1023)))+(((IkReal(-1.00000000000000))*(x1007)*(x1020)))+(((x1015)*(x1022)))+(((IkReal(0.0701403000000000))*(x1006)))+(((IkReal(-0.302000000000000))*(x1005)))+(((x1015)*(x1025)))+(((IkReal(-0.0616850000000000))*(x1021)))+(((IkReal(-1.00000000000000))*(pz)*(x1013)))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x1016)))+(((x1010)*(x1015)))+(((IkReal(-1.00000000000000))*(x1007)*(x1008)))+(((IkReal(-1.00000000000000))*(x1016)*(x1017))));
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 gconst2;
gconst2=IKsign(sj9);
dummyeval[0]=sj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[2];
dummyeval[0]=sj9;
dummyeval[1]=cj8;
if( IKabs(dummyeval[0]) < 0.0000010000000000 || IKabs(dummyeval[1]) < 0.0000010000000000 )
{
{
IkReal evalcond[9];
IkReal x1027=((IkReal(0.0620002000000000))*(cj7));
IkReal x1028=((r21)*(sj11));
IkReal x1029=((IkReal(1.00000000000000))*(cj11));
IkReal x1030=((r01)*(sj6));
IkReal x1031=((r20)*(sj7));
IkReal x1032=((IkReal(1.00000000000000))*(cj6));
IkReal x1033=((IkReal(0.0620002000000000))*(cj11));
IkReal x1034=((IkReal(1.00000000000000))*(sj7));
IkReal x1035=((IkReal(0.0620002000000000))*(sj11));
IkReal x1036=((cj6)*(r10));
IkReal x1037=((IkReal(1.00000000000000))*(cj7));
IkReal x1038=((cj7)*(sj6));
IkReal x1039=((r00)*(sj6));
IkReal x1040=((r11)*(sj11));
IkReal x1041=((IkReal(0.0620002000000000))*(sj7));
IkReal x1042=((cj11)*(r10));
IkReal x1043=((IkReal(1.00000000000000))*(sj11));
IkReal x1044=((cj6)*(r11));
IkReal x1045=((r01)*(sj11));
IkReal x1046=((cj11)*(cj6)*(r00));
IkReal x1047=((r11)*(sj6)*(sj7));
IkReal x1048=((r10)*(sj6)*(sj7));
IkReal x1049=((cj6)*(sj7)*(x1045));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j9)), IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1032))));
evalcond[2]=((IkReal(1.00000000000000))+(((cj11)*(x1039)))+(((IkReal(-1.00000000000000))*(x1029)*(x1036)))+(((IkReal(-1.00000000000000))*(x1030)*(x1043)))+(((cj6)*(x1040))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1029)*(x1030)))+(((cj11)*(x1044)))+(((IkReal(-1.00000000000000))*(x1039)*(x1043)))+(((sj11)*(x1036))));
evalcond[4]=((IkReal(-0.0717783000000000))+(((IkReal(-1.00000000000000))*(py)*(x1032)))+(((IkReal(-1.00000000000000))*(x1033)*(x1039)))+(((px)*(sj6)))+(((x1033)*(x1036)))+(((x1030)*(x1035)))+(((IkReal(-1.00000000000000))*(x1035)*(x1044))));
evalcond[5]=((x1049)+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj7)*(x1029)))+(((sj6)*(sj7)*(x1040)))+(((cj7)*(x1028)))+(((IkReal(-1.00000000000000))*(x1029)*(x1048)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1029))));
evalcond[6]=((((IkReal(-1.00000000000000))*(cj7)*(x1032)*(x1045)))+(((cj7)*(x1046)))+(((IkReal(-1.00000000000000))*(x1029)*(x1031)))+(((sj7)*(x1028)))+(((x1038)*(x1042)))+(((IkReal(-1.00000000000000))*(sj6)*(x1037)*(x1040))));
evalcond[7]=((((IkReal(-0.302000000000000))*(cj8)))+(((IkReal(-0.0716559700000000))*(sj8)))+(((IkReal(-1.00000000000000))*(x1027)*(x1046)))+(((cj6)*(x1027)*(x1045)))+(((cj6)*(cj7)*(px)))+(((sj6)*(x1027)*(x1040)))+(((IkReal(-1.00000000000000))*(sj6)*(x1027)*(x1042)))+(((IkReal(-1.00000000000000))*(x1028)*(x1041)))+(((IkReal(-1.00000000000000))*(pz)*(x1034)))+(((py)*(x1038)))+(((x1031)*(x1033))));
evalcond[8]=((IkReal(0.270000000000000))+(((IkReal(0.0716559700000000))*(cj8)))+(((x1033)*(x1048)))+(((cj11)*(r20)*(x1027)))+(((IkReal(-1.00000000000000))*(x1027)*(x1028)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj7)*(x1035)))+(((IkReal(-1.00000000000000))*(px)*(sj7)*(x1032)))+(((IkReal(-1.00000000000000))*(x1035)*(x1047)))+(((cj6)*(r00)*(sj7)*(x1033)))+(((IkReal(-1.00000000000000))*(pz)*(x1037)))+(((IkReal(-0.302000000000000))*(sj8)))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x1034))));
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 )
{
{
IkReal dummyeval[1];
IkReal gconst3;
gconst3=IKsign((((cj8)*(cj8))+((sj8)*(sj8))));
dummyeval[0]=(((cj8)*(cj8))+((sj8)*(sj8)));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
IkReal gconst4;
gconst4=IKsign(((((IkReal(-1.00000000000000))*((sj8)*(sj8))))+(((IkReal(-1.00000000000000))*((cj8)*(cj8))))));
dummyeval[0]=((((IkReal(-1.00000000000000))*((sj8)*(sj8))))+(((IkReal(-1.00000000000000))*((cj8)*(cj8)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
continue;
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1050=((sj7)*(sj8));
IkReal x1051=((cj11)*(r21));
IkReal x1052=((r20)*(sj11));
IkReal x1053=((cj8)*(sj7));
IkReal x1054=((IkReal(1.00000000000000))*(sj11));
IkReal x1055=((IkReal(1.00000000000000))*(cj11));
IkReal x1056=((cj7)*(sj6)*(sj8));
IkReal x1057=((cj6)*(cj7)*(sj8));
IkReal x1058=((cj7)*(cj8)*(sj6));
IkReal x1059=((cj6)*(cj7)*(cj8));
if( IKabs(((gconst4)*(((((r02)*(x1057)))+(((x1052)*(x1053)))+(((IkReal(-1.00000000000000))*(r00)*(x1054)*(x1059)))+(((IkReal(-1.00000000000000))*(r01)*(x1055)*(x1059)))+(((IkReal(-1.00000000000000))*(r11)*(x1055)*(x1058)))+(((x1051)*(x1053)))+(((IkReal(-1.00000000000000))*(r22)*(x1050)))+(((r12)*(x1056)))+(((IkReal(-1.00000000000000))*(r10)*(x1054)*(x1058))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(r01)*(x1055)*(x1057)))+(((IkReal(-1.00000000000000))*(r12)*(x1058)))+(((IkReal(-1.00000000000000))*(r00)*(x1054)*(x1057)))+(((IkReal(-1.00000000000000))*(r11)*(x1055)*(x1056)))+(((x1050)*(x1052)))+(((x1050)*(x1051)))+(((r22)*(x1053)))+(((IkReal(-1.00000000000000))*(r02)*(x1059)))+(((IkReal(-1.00000000000000))*(r10)*(x1054)*(x1056))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst4)*(((((r02)*(x1057)))+(((x1052)*(x1053)))+(((IkReal(-1.00000000000000))*(r00)*(x1054)*(x1059)))+(((IkReal(-1.00000000000000))*(r01)*(x1055)*(x1059)))+(((IkReal(-1.00000000000000))*(r11)*(x1055)*(x1058)))+(((x1051)*(x1053)))+(((IkReal(-1.00000000000000))*(r22)*(x1050)))+(((r12)*(x1056)))+(((IkReal(-1.00000000000000))*(r10)*(x1054)*(x1058)))))), ((gconst4)*(((((IkReal(-1.00000000000000))*(r01)*(x1055)*(x1057)))+(((IkReal(-1.00000000000000))*(r12)*(x1058)))+(((IkReal(-1.00000000000000))*(r00)*(x1054)*(x1057)))+(((IkReal(-1.00000000000000))*(r11)*(x1055)*(x1056)))+(((x1050)*(x1052)))+(((x1050)*(x1051)))+(((r22)*(x1053)))+(((IkReal(-1.00000000000000))*(r02)*(x1059)))+(((IkReal(-1.00000000000000))*(r10)*(x1054)*(x1056)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x1060=IKcos(j10);
IkReal x1061=IKsin(j10);
IkReal x1062=((IkReal(1.00000000000000))*(cj7));
IkReal x1063=((cj6)*(r02));
IkReal x1064=((IkReal(1.00000000000000))*(sj7));
IkReal x1065=((r20)*(sj11));
IkReal x1066=((cj11)*(r21));
IkReal x1067=((r12)*(sj6));
IkReal x1068=((sj8)*(x1061));
IkReal x1069=((cj8)*(x1061));
IkReal x1070=((r10)*(sj11)*(sj6));
IkReal x1071=((IkReal(1.00000000000000))*(x1060));
IkReal x1072=((cj11)*(cj6)*(r01));
IkReal x1073=((cj11)*(r11)*(sj6));
IkReal x1074=((cj6)*(r00)*(sj11));
IkReal x1075=((cj8)*(x1071));
evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x1064)))+(((cj7)*(x1063)))+(((IkReal(-1.00000000000000))*(x1075)))+(x1068)+(((cj7)*(x1067))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x1063)*(x1064)))+(((IkReal(-1.00000000000000))*(sj8)*(x1071)))+(((IkReal(-1.00000000000000))*(r22)*(x1062)))+(((IkReal(-1.00000000000000))*(x1064)*(x1067)))+(((IkReal(-1.00000000000000))*(x1069))));
evalcond[2]=((((sj7)*(x1074)))+(((sj7)*(x1073)))+(((IkReal(-1.00000000000000))*(x1075)))+(((sj7)*(x1072)))+(x1068)+(((sj7)*(x1070)))+(((cj7)*(x1065)))+(((cj7)*(x1066))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1062)*(x1073)))+(((sj7)*(x1066)))+(((sj7)*(x1065)))+(((sj8)*(x1060)))+(x1069)+(((IkReal(-1.00000000000000))*(x1062)*(x1074)))+(((IkReal(-1.00000000000000))*(x1062)*(x1072)))+(((IkReal(-1.00000000000000))*(x1062)*(x1070))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1076=((cj7)*(cj8));
IkReal x1077=((r12)*(sj6));
IkReal x1078=((IkReal(1.00000000000000))*(sj7));
IkReal x1079=((cj6)*(r02));
IkReal x1080=((IkReal(1.00000000000000))*(r22));
IkReal x1081=((cj7)*(sj8));
if( IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(x1076)*(x1080)))+(((IkReal(-1.00000000000000))*(x1077)*(x1081)))+(((IkReal(-1.00000000000000))*(x1079)*(x1081)))+(((r22)*(sj7)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(x1077)*(x1078)))+(((IkReal(-1.00000000000000))*(cj8)*(x1078)*(x1079))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(sj8)*(x1078)*(x1079)))+(((x1076)*(x1079)))+(((IkReal(-1.00000000000000))*(cj8)*(r22)*(x1078)))+(((IkReal(-1.00000000000000))*(x1080)*(x1081)))+(((x1076)*(x1077)))+(((IkReal(-1.00000000000000))*(sj8)*(x1077)*(x1078))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst3)*(((((IkReal(-1.00000000000000))*(x1076)*(x1080)))+(((IkReal(-1.00000000000000))*(x1077)*(x1081)))+(((IkReal(-1.00000000000000))*(x1079)*(x1081)))+(((r22)*(sj7)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(x1077)*(x1078)))+(((IkReal(-1.00000000000000))*(cj8)*(x1078)*(x1079)))))), ((gconst3)*(((((IkReal(-1.00000000000000))*(sj8)*(x1078)*(x1079)))+(((x1076)*(x1079)))+(((IkReal(-1.00000000000000))*(cj8)*(r22)*(x1078)))+(((IkReal(-1.00000000000000))*(x1080)*(x1081)))+(((x1076)*(x1077)))+(((IkReal(-1.00000000000000))*(sj8)*(x1077)*(x1078)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x1082=IKcos(j10);
IkReal x1083=IKsin(j10);
IkReal x1084=((IkReal(1.00000000000000))*(cj7));
IkReal x1085=((cj6)*(r02));
IkReal x1086=((IkReal(1.00000000000000))*(sj7));
IkReal x1087=((r20)*(sj11));
IkReal x1088=((cj11)*(r21));
IkReal x1089=((r12)*(sj6));
IkReal x1090=((sj8)*(x1083));
IkReal x1091=((cj8)*(x1083));
IkReal x1092=((r10)*(sj11)*(sj6));
IkReal x1093=((IkReal(1.00000000000000))*(x1082));
IkReal x1094=((cj11)*(cj6)*(r01));
IkReal x1095=((cj11)*(r11)*(sj6));
IkReal x1096=((cj6)*(r00)*(sj11));
IkReal x1097=((cj8)*(x1093));
evalcond[0]=((((cj7)*(x1089)))+(((IkReal(-1.00000000000000))*(x1097)))+(((cj7)*(x1085)))+(((IkReal(-1.00000000000000))*(r22)*(x1086)))+(x1090));
evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(x1084)))+(((IkReal(-1.00000000000000))*(sj8)*(x1093)))+(((IkReal(-1.00000000000000))*(x1091)))+(((IkReal(-1.00000000000000))*(x1086)*(x1089)))+(((IkReal(-1.00000000000000))*(x1085)*(x1086))));
evalcond[2]=((((cj7)*(x1088)))+(((sj7)*(x1092)))+(((sj7)*(x1096)))+(((IkReal(-1.00000000000000))*(x1097)))+(((sj7)*(x1095)))+(((cj7)*(x1087)))+(x1090)+(((sj7)*(x1094))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1084)*(x1092)))+(((IkReal(-1.00000000000000))*(x1084)*(x1094)))+(((sj8)*(x1082)))+(((sj7)*(x1088)))+(((IkReal(-1.00000000000000))*(x1084)*(x1095)))+(((IkReal(-1.00000000000000))*(x1084)*(x1096)))+(x1091)+(((sj7)*(x1087))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
IkReal x1098=((IkReal(0.0620002000000000))*(cj7));
IkReal x1099=((r21)*(sj11));
IkReal x1100=((IkReal(1.00000000000000))*(cj11));
IkReal x1101=((r01)*(sj6));
IkReal x1102=((r20)*(sj7));
IkReal x1103=((IkReal(1.00000000000000))*(cj6));
IkReal x1104=((IkReal(0.0620002000000000))*(cj11));
IkReal x1105=((IkReal(1.00000000000000))*(sj7));
IkReal x1106=((IkReal(0.0620002000000000))*(sj11));
IkReal x1107=((cj6)*(r10));
IkReal x1108=((IkReal(1.00000000000000))*(cj7));
IkReal x1109=((cj7)*(sj6));
IkReal x1110=((r00)*(sj6));
IkReal x1111=((r11)*(sj11));
IkReal x1112=((IkReal(0.0620002000000000))*(sj7));
IkReal x1113=((cj11)*(r10));
IkReal x1114=((IkReal(1.00000000000000))*(sj11));
IkReal x1115=((cj6)*(r11));
IkReal x1116=((r01)*(sj11));
IkReal x1117=((cj11)*(cj6)*(r00));
IkReal x1118=((r11)*(sj6)*(sj7));
IkReal x1119=((r10)*(sj6)*(sj7));
IkReal x1120=((cj6)*(sj7)*(x1116));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j9, IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1103))));
evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1100)*(x1107)))+(((cj6)*(x1111)))+(((IkReal(-1.00000000000000))*(x1101)*(x1114)))+(((cj11)*(x1110))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1100)*(x1101)))+(((IkReal(-1.00000000000000))*(x1110)*(x1114)))+(((cj11)*(x1115)))+(((sj11)*(x1107))));
evalcond[4]=((IkReal(0.0515917000000000))+(((IkReal(-1.00000000000000))*(x1104)*(x1110)))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(py)*(x1103)))+(((IkReal(-1.00000000000000))*(x1106)*(x1115)))+(((x1104)*(x1107)))+(((x1101)*(x1106))));
evalcond[5]=((((sj6)*(sj7)*(x1111)))+(((IkReal(-1.00000000000000))*(x1100)*(x1119)))+(x1120)+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj7)*(x1100)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1100)))+(((cj7)*(x1099))));
evalcond[6]=((((x1109)*(x1113)))+(((IkReal(-1.00000000000000))*(sj6)*(x1108)*(x1111)))+(((IkReal(-1.00000000000000))*(cj7)*(x1103)*(x1116)))+(((cj7)*(x1117)))+(((IkReal(-1.00000000000000))*(x1100)*(x1102)))+(((sj7)*(x1099))));
evalcond[7]=((((IkReal(-1.00000000000000))*(x1099)*(x1112)))+(((IkReal(-1.00000000000000))*(pz)*(x1105)))+(((IkReal(-0.302000000000000))*(cj8)))+(((x1102)*(x1104)))+(((IkReal(-0.0686246300000000))*(sj8)))+(((cj6)*(x1098)*(x1116)))+(((py)*(x1109)))+(((cj6)*(cj7)*(px)))+(((sj6)*(x1098)*(x1111)))+(((IkReal(-1.00000000000000))*(sj6)*(x1098)*(x1113)))+(((IkReal(-1.00000000000000))*(x1098)*(x1117))));
evalcond[8]=((IkReal(0.270000000000000))+(((cj6)*(r00)*(sj7)*(x1104)))+(((cj11)*(r20)*(x1098)))+(((IkReal(-1.00000000000000))*(pz)*(x1108)))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x1105)))+(((IkReal(-1.00000000000000))*(x1106)*(x1118)))+(((x1104)*(x1119)))+(((IkReal(0.0686246300000000))*(cj8)))+(((IkReal(-1.00000000000000))*(px)*(sj7)*(x1103)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj7)*(x1106)))+(((IkReal(-1.00000000000000))*(x1098)*(x1099)))+(((IkReal(-0.302000000000000))*(sj8))));
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 )
{
{
IkReal dummyeval[1];
IkReal gconst5;
gconst5=IKsign((((cj8)*(cj8))+((sj8)*(sj8))));
dummyeval[0]=(((cj8)*(cj8))+((sj8)*(sj8)));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
IkReal gconst6;
gconst6=IKsign(((((IkReal(-1.00000000000000))*((sj8)*(sj8))))+(((IkReal(-1.00000000000000))*((cj8)*(cj8))))));
dummyeval[0]=((((IkReal(-1.00000000000000))*((sj8)*(sj8))))+(((IkReal(-1.00000000000000))*((cj8)*(cj8)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
continue;
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1121=((IkReal(1.00000000000000))*(cj8));
IkReal x1122=((cj6)*(sj7));
IkReal x1123=((cj8)*(sj11));
IkReal x1124=((cj7)*(r20));
IkReal x1125=((sj6)*(sj7));
IkReal x1126=((sj11)*(sj8));
IkReal x1127=((cj7)*(r22));
IkReal x1128=((cj11)*(sj8));
IkReal x1129=((cj11)*(cj8));
IkReal x1130=((cj11)*(cj7)*(r21));
if( IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(r12)*(x1121)*(x1125)))+(((cj7)*(r21)*(x1128)))+(((IkReal(-1.00000000000000))*(x1121)*(x1127)))+(((r00)*(x1122)*(x1126)))+(((x1124)*(x1126)))+(((IkReal(-1.00000000000000))*(r02)*(x1121)*(x1122)))+(((r11)*(x1125)*(x1128)))+(((r01)*(x1122)*(x1128)))+(((r10)*(x1125)*(x1126))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((r10)*(x1123)*(x1125)))+(((r01)*(x1122)*(x1129)))+(((r12)*(sj8)*(x1125)))+(((sj8)*(x1127)))+(((r02)*(sj8)*(x1122)))+(((x1123)*(x1124)))+(((r00)*(x1122)*(x1123)))+(((cj7)*(r21)*(x1129)))+(((r11)*(x1125)*(x1129))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst6)*(((((IkReal(-1.00000000000000))*(r12)*(x1121)*(x1125)))+(((cj7)*(r21)*(x1128)))+(((IkReal(-1.00000000000000))*(x1121)*(x1127)))+(((r00)*(x1122)*(x1126)))+(((x1124)*(x1126)))+(((IkReal(-1.00000000000000))*(r02)*(x1121)*(x1122)))+(((r11)*(x1125)*(x1128)))+(((r01)*(x1122)*(x1128)))+(((r10)*(x1125)*(x1126)))))), ((gconst6)*(((((r10)*(x1123)*(x1125)))+(((r01)*(x1122)*(x1129)))+(((r12)*(sj8)*(x1125)))+(((sj8)*(x1127)))+(((r02)*(sj8)*(x1122)))+(((x1123)*(x1124)))+(((r00)*(x1122)*(x1123)))+(((cj7)*(r21)*(x1129)))+(((r11)*(x1125)*(x1129)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x1131=IKsin(j10);
IkReal x1132=IKcos(j10);
IkReal x1133=((IkReal(1.00000000000000))*(cj7));
IkReal x1134=((cj6)*(r02));
IkReal x1135=((IkReal(1.00000000000000))*(sj7));
IkReal x1136=((r20)*(sj11));
IkReal x1137=((cj11)*(r21));
IkReal x1138=((r12)*(sj6));
IkReal x1139=((cj8)*(x1131));
IkReal x1140=((r10)*(sj11)*(sj6));
IkReal x1141=((IkReal(1.00000000000000))*(x1132));
IkReal x1142=((cj11)*(cj6)*(r01));
IkReal x1143=((sj8)*(x1131));
IkReal x1144=((cj11)*(r11)*(sj6));
IkReal x1145=((cj6)*(r00)*(sj11));
IkReal x1146=((sj8)*(x1141));
evalcond[0]=((((cj7)*(x1134)))+(((IkReal(-1.00000000000000))*(cj8)*(x1141)))+(((IkReal(-1.00000000000000))*(r22)*(x1135)))+(((cj7)*(x1138)))+(((IkReal(-1.00000000000000))*(x1143))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x1134)*(x1135)))+(((IkReal(-1.00000000000000))*(r22)*(x1133)))+(x1139)+(((IkReal(-1.00000000000000))*(x1135)*(x1138)))+(((IkReal(-1.00000000000000))*(x1146))));
evalcond[2]=((((cj8)*(x1132)))+(((sj7)*(x1145)))+(x1143)+(((sj7)*(x1144)))+(((sj7)*(x1142)))+(((cj7)*(x1136)))+(((cj7)*(x1137)))+(((sj7)*(x1140))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1133)*(x1142)))+(((IkReal(-1.00000000000000))*(x1133)*(x1140)))+(((sj7)*(x1136)))+(x1139)+(((sj7)*(x1137)))+(((IkReal(-1.00000000000000))*(x1133)*(x1145)))+(((IkReal(-1.00000000000000))*(x1133)*(x1144)))+(((IkReal(-1.00000000000000))*(x1146))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1147=((cj6)*(r02));
IkReal x1148=((cj7)*(cj8));
IkReal x1149=((r12)*(sj6));
IkReal x1150=((cj8)*(sj7));
IkReal x1151=((cj7)*(sj8));
IkReal x1152=((cj7)*(x1149));
IkReal x1153=((IkReal(1.00000000000000))*(r22)*(sj7));
IkReal x1154=((IkReal(1.00000000000000))*(sj7)*(sj8));
if( IKabs(((gconst5)*(((((x1147)*(x1151)))+(((r22)*(x1148)))+(((x1149)*(x1151)))+(((IkReal(-1.00000000000000))*(sj8)*(x1153)))+(((x1149)*(x1150)))+(((x1147)*(x1150))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((((x1147)*(x1148)))+(((x1148)*(x1149)))+(((IkReal(-1.00000000000000))*(r22)*(x1151)))+(((IkReal(-1.00000000000000))*(x1149)*(x1154)))+(((IkReal(-1.00000000000000))*(r22)*(x1150)))+(((IkReal(-1.00000000000000))*(x1147)*(x1154))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst5)*(((((x1147)*(x1151)))+(((r22)*(x1148)))+(((x1149)*(x1151)))+(((IkReal(-1.00000000000000))*(sj8)*(x1153)))+(((x1149)*(x1150)))+(((x1147)*(x1150)))))), ((gconst5)*(((((x1147)*(x1148)))+(((x1148)*(x1149)))+(((IkReal(-1.00000000000000))*(r22)*(x1151)))+(((IkReal(-1.00000000000000))*(x1149)*(x1154)))+(((IkReal(-1.00000000000000))*(r22)*(x1150)))+(((IkReal(-1.00000000000000))*(x1147)*(x1154)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x1155=IKsin(j10);
IkReal x1156=IKcos(j10);
IkReal x1157=((IkReal(1.00000000000000))*(cj7));
IkReal x1158=((cj6)*(r02));
IkReal x1159=((IkReal(1.00000000000000))*(sj7));
IkReal x1160=((r20)*(sj11));
IkReal x1161=((cj11)*(r21));
IkReal x1162=((r12)*(sj6));
IkReal x1163=((cj8)*(x1155));
IkReal x1164=((r10)*(sj11)*(sj6));
IkReal x1165=((IkReal(1.00000000000000))*(x1156));
IkReal x1166=((cj11)*(cj6)*(r01));
IkReal x1167=((sj8)*(x1155));
IkReal x1168=((cj11)*(r11)*(sj6));
IkReal x1169=((cj6)*(r00)*(sj11));
IkReal x1170=((sj8)*(x1165));
evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x1159)))+(((IkReal(-1.00000000000000))*(x1167)))+(((cj7)*(x1162)))+(((IkReal(-1.00000000000000))*(cj8)*(x1165)))+(((cj7)*(x1158))));
evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(x1157)))+(x1163)+(((IkReal(-1.00000000000000))*(x1170)))+(((IkReal(-1.00000000000000))*(x1159)*(x1162)))+(((IkReal(-1.00000000000000))*(x1158)*(x1159))));
evalcond[2]=((((cj7)*(x1161)))+(((cj7)*(x1160)))+(x1167)+(((sj7)*(x1166)))+(((cj8)*(x1156)))+(((sj7)*(x1168)))+(((sj7)*(x1169)))+(((sj7)*(x1164))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1157)*(x1169)))+(((IkReal(-1.00000000000000))*(x1157)*(x1168)))+(x1163)+(((IkReal(-1.00000000000000))*(x1170)))+(((IkReal(-1.00000000000000))*(x1157)*(x1164)))+(((sj7)*(x1160)))+(((sj7)*(x1161)))+(((IkReal(-1.00000000000000))*(x1157)*(x1166))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
IkReal x1171=((IkReal(0.0620002000000000))*(sj11));
IkReal x1172=((cj7)*(r21));
IkReal x1173=((IkReal(1.00000000000000))*(cj11));
IkReal x1174=((cj7)*(r20));
IkReal x1175=((cj6)*(sj7));
IkReal x1176=((r01)*(sj6));
IkReal x1177=((IkReal(0.0620002000000000))*(cj11));
IkReal x1178=((cj6)*(r10));
IkReal x1179=((IkReal(1.00000000000000))*(cj7));
IkReal x1180=((cj7)*(sj6));
IkReal x1181=((r00)*(sj6));
IkReal x1182=((r11)*(sj11));
IkReal x1183=((IkReal(1.00000000000000))*(py));
IkReal x1184=((sj6)*(sj7));
IkReal x1185=((r21)*(sj7));
IkReal x1186=((cj6)*(cj7));
IkReal x1187=((r01)*(x1175));
IkReal x1188=((cj11)*(r20)*(sj7));
IkReal x1189=((r11)*(x1184));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j8)), IkReal(6.28318530717959))));
evalcond[1]=((((cj11)*(x1181)))+(((cj6)*(x1182)))+(((IkReal(-1.00000000000000))*(sj11)*(x1176)))+(cj9)+(((IkReal(-1.00000000000000))*(x1173)*(x1178))));
evalcond[2]=((IkReal(-0.0100933000000000))+(((IkReal(-0.00151567000000000))*(sj9)))+(((IkReal(-0.0616850000000000))*(cj9)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x1171)))+(((x1177)*(x1178)))+(((px)*(sj6)))+(((x1171)*(x1176)))+(((IkReal(-1.00000000000000))*(cj6)*(x1183)))+(((IkReal(-1.00000000000000))*(x1177)*(x1181))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r00)*(x1173)*(x1175)))+(((sj11)*(x1172)))+(((IkReal(-1.00000000000000))*(r10)*(x1173)*(x1184)))+(((x1182)*(x1184)))+(((sj11)*(x1187)))+(((IkReal(-1.00000000000000))*(x1173)*(x1174))));
evalcond[4]=((((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj11)*(x1179)))+(((IkReal(-1.00000000000000))*(r20)*(sj7)*(x1173)))+(((IkReal(-1.00000000000000))*(sj9)))+(((cj11)*(r00)*(x1186)))+(((sj11)*(x1185)))+(((IkReal(-1.00000000000000))*(sj6)*(x1179)*(x1182)))+(((cj11)*(r10)*(x1180))));
evalcond[5]=((IkReal(-0.0701403000000000))+(((r01)*(x1171)*(x1186)))+(((py)*(x1180)))+(((IkReal(-1.00000000000000))*(pz)*(sj7)))+(((r11)*(x1171)*(x1180)))+(((IkReal(0.0616850000000000))*(sj9)))+(((r20)*(sj7)*(x1177)))+(((IkReal(-1.00000000000000))*(r10)*(x1177)*(x1180)))+(((IkReal(-1.00000000000000))*(x1171)*(x1185)))+(((IkReal(-1.00000000000000))*(r00)*(x1177)*(x1186)))+(((px)*(x1186)))+(((IkReal(-0.00151567000000000))*(cj9))));
evalcond[6]=((IkReal(-0.0320000000000000))+(((IkReal(-1.00000000000000))*(x1183)*(x1184)))+(((x1174)*(x1177)))+(((IkReal(-1.00000000000000))*(x1171)*(x1172)))+(((IkReal(-1.00000000000000))*(x1171)*(x1189)))+(((IkReal(-1.00000000000000))*(x1171)*(x1187)))+(((r10)*(x1177)*(x1184)))+(((r00)*(x1175)*(x1177)))+(((IkReal(-1.00000000000000))*(pz)*(x1179)))+(((IkReal(-1.00000000000000))*(px)*(x1175))));
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 dummyeval[1];
IkReal gconst7;
gconst7=IKsign(sj9);
dummyeval[0]=sj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
dummyeval[0]=sj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal evalcond[9];
IkReal x1190=((IkReal(0.0620002000000000))*(cj7));
IkReal x1191=((r21)*(sj11));
IkReal x1192=((IkReal(1.00000000000000))*(cj11));
IkReal x1193=((r01)*(sj6));
IkReal x1194=((r20)*(sj7));
IkReal x1195=((IkReal(1.00000000000000))*(cj6));
IkReal x1196=((IkReal(0.0620002000000000))*(cj11));
IkReal x1197=((IkReal(1.00000000000000))*(sj7));
IkReal x1198=((IkReal(0.0620002000000000))*(sj11));
IkReal x1199=((cj6)*(r10));
IkReal x1200=((IkReal(1.00000000000000))*(cj7));
IkReal x1201=((cj7)*(sj6));
IkReal x1202=((r00)*(sj6));
IkReal x1203=((r11)*(sj11));
IkReal x1204=((IkReal(0.0620002000000000))*(sj7));
IkReal x1205=((cj11)*(r10));
IkReal x1206=((IkReal(1.00000000000000))*(sj11));
IkReal x1207=((cj6)*(r11));
IkReal x1208=((r01)*(sj11));
IkReal x1209=((cj11)*(cj6)*(r00));
IkReal x1210=((r11)*(sj6)*(sj7));
IkReal x1211=((r10)*(sj6)*(sj7));
IkReal x1212=((cj6)*(sj7)*(x1208));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j9)), IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1195))));
evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1192)*(x1199)))+(((cj11)*(x1202)))+(((IkReal(-1.00000000000000))*(x1193)*(x1206)))+(((cj6)*(x1203))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1202)*(x1206)))+(((cj11)*(x1207)))+(((IkReal(-1.00000000000000))*(x1192)*(x1193)))+(((sj11)*(x1199))));
evalcond[4]=((IkReal(-0.0717783000000000))+(((x1196)*(x1199)))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(x1198)*(x1207)))+(((IkReal(-1.00000000000000))*(x1196)*(x1202)))+(((IkReal(-1.00000000000000))*(py)*(x1195)))+(((x1193)*(x1198))));
evalcond[5]=((((sj6)*(sj7)*(x1203)))+(((cj7)*(x1191)))+(((IkReal(-1.00000000000000))*(x1192)*(x1211)))+(x1212)+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj7)*(x1192)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1192))));
evalcond[6]=((((x1201)*(x1205)))+(((sj7)*(x1191)))+(((IkReal(-1.00000000000000))*(x1192)*(x1194)))+(((IkReal(-1.00000000000000))*(cj7)*(x1195)*(x1208)))+(((IkReal(-1.00000000000000))*(sj6)*(x1200)*(x1203)))+(((cj7)*(x1209))));
evalcond[7]=((IkReal(-0.0716559700000000))+(((sj6)*(x1190)*(x1203)))+(((IkReal(-1.00000000000000))*(x1191)*(x1204)))+(((cj6)*(x1190)*(x1208)))+(((cj6)*(cj7)*(px)))+(((IkReal(-1.00000000000000))*(sj6)*(x1190)*(x1205)))+(((IkReal(-1.00000000000000))*(pz)*(x1197)))+(((IkReal(-1.00000000000000))*(x1190)*(x1209)))+(((py)*(x1201)))+(((x1194)*(x1196))));
evalcond[8]=((IkReal(-0.0320000000000000))+(((IkReal(-1.00000000000000))*(x1190)*(x1191)))+(((cj6)*(r00)*(sj7)*(x1196)))+(((IkReal(-1.00000000000000))*(x1198)*(x1210)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj7)*(x1198)))+(((IkReal(-1.00000000000000))*(pz)*(x1200)))+(((IkReal(-1.00000000000000))*(px)*(sj7)*(x1195)))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x1197)))+(((cj11)*(r20)*(x1190)))+(((x1196)*(x1211))));
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 )
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1213=((IkReal(1.00000000000000))*(sj7));
IkReal x1214=((cj6)*(r02));
IkReal x1215=((IkReal(1.00000000000000))*(cj7));
IkReal x1216=((r12)*(sj6));
if( IKabs(((((IkReal(-1.00000000000000))*(x1214)*(x1215)))+(((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x1215)*(x1216))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(x1213)*(x1214)))+(((IkReal(-1.00000000000000))*(x1213)*(x1216)))+(((IkReal(-1.00000000000000))*(r22)*(x1215))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(x1214)*(x1215)))+(((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x1215)*(x1216)))))+IKsqr(((((IkReal(-1.00000000000000))*(x1213)*(x1214)))+(((IkReal(-1.00000000000000))*(x1213)*(x1216)))+(((IkReal(-1.00000000000000))*(r22)*(x1215)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((((IkReal(-1.00000000000000))*(x1214)*(x1215)))+(((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x1215)*(x1216)))), ((((IkReal(-1.00000000000000))*(x1213)*(x1214)))+(((IkReal(-1.00000000000000))*(x1213)*(x1216)))+(((IkReal(-1.00000000000000))*(r22)*(x1215)))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x1217=IKsin(j10);
IkReal x1218=IKcos(j10);
IkReal x1219=((IkReal(1.00000000000000))*(r22));
IkReal x1220=((cj7)*(sj11));
IkReal x1221=((r10)*(sj6));
IkReal x1222=((cj11)*(r21));
IkReal x1223=((sj11)*(sj7));
IkReal x1224=((cj6)*(r01));
IkReal x1225=((r11)*(sj6));
IkReal x1226=((cj6)*(r02));
IkReal x1227=((r12)*(sj6));
IkReal x1228=((IkReal(1.00000000000000))*(sj7));
IkReal x1229=((cj11)*(sj7));
IkReal x1230=((cj6)*(r00));
IkReal x1231=((IkReal(1.00000000000000))*(cj11)*(cj7));
evalcond[0]=((((cj7)*(x1226)))+(((cj7)*(x1227)))+(((IkReal(-1.00000000000000))*(sj7)*(x1219)))+(x1217));
evalcond[1]=((((IkReal(-1.00000000000000))*(x1227)*(x1228)))+(((IkReal(-1.00000000000000))*(x1226)*(x1228)))+(((IkReal(-1.00000000000000))*(x1218)))+(((IkReal(-1.00000000000000))*(cj7)*(x1219))));
evalcond[2]=((((x1224)*(x1229)))+(((x1223)*(x1230)))+(((r20)*(x1220)))+(((cj7)*(x1222)))+(((x1221)*(x1223)))+(x1217)+(((x1225)*(x1229))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1220)*(x1221)))+(((IkReal(-1.00000000000000))*(x1224)*(x1231)))+(((r20)*(x1223)))+(((IkReal(-1.00000000000000))*(x1220)*(x1230)))+(((IkReal(-1.00000000000000))*(x1225)*(x1231)))+(((sj7)*(x1222)))+(x1218));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
IkReal x1232=((IkReal(0.0620002000000000))*(cj7));
IkReal x1233=((r21)*(sj11));
IkReal x1234=((IkReal(1.00000000000000))*(cj11));
IkReal x1235=((r01)*(sj6));
IkReal x1236=((r20)*(sj7));
IkReal x1237=((IkReal(1.00000000000000))*(cj6));
IkReal x1238=((IkReal(0.0620002000000000))*(cj11));
IkReal x1239=((IkReal(1.00000000000000))*(sj7));
IkReal x1240=((IkReal(0.0620002000000000))*(sj11));
IkReal x1241=((cj6)*(r10));
IkReal x1242=((IkReal(1.00000000000000))*(cj7));
IkReal x1243=((cj7)*(sj6));
IkReal x1244=((r00)*(sj6));
IkReal x1245=((r11)*(sj11));
IkReal x1246=((IkReal(0.0620002000000000))*(sj7));
IkReal x1247=((cj11)*(r10));
IkReal x1248=((IkReal(1.00000000000000))*(sj11));
IkReal x1249=((cj6)*(r11));
IkReal x1250=((r01)*(sj11));
IkReal x1251=((cj11)*(cj6)*(r00));
IkReal x1252=((r11)*(sj6)*(sj7));
IkReal x1253=((r10)*(sj6)*(sj7));
IkReal x1254=((cj6)*(sj7)*(x1250));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j9, IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1237))));
evalcond[2]=((IkReal(-1.00000000000000))+(((cj6)*(x1245)))+(((cj11)*(x1244)))+(((IkReal(-1.00000000000000))*(x1235)*(x1248)))+(((IkReal(-1.00000000000000))*(x1234)*(x1241))));
evalcond[3]=((((cj11)*(x1249)))+(((IkReal(-1.00000000000000))*(x1244)*(x1248)))+(((sj11)*(x1241)))+(((IkReal(-1.00000000000000))*(x1234)*(x1235))));
evalcond[4]=((IkReal(0.0515917000000000))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(x1240)*(x1249)))+(((IkReal(-1.00000000000000))*(x1238)*(x1244)))+(((x1235)*(x1240)))+(((IkReal(-1.00000000000000))*(py)*(x1237)))+(((x1238)*(x1241))));
evalcond[5]=((((sj6)*(sj7)*(x1245)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj7)*(x1234)))+(x1254)+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1234)))+(((cj7)*(x1233)))+(((IkReal(-1.00000000000000))*(x1234)*(x1253))));
evalcond[6]=((((IkReal(-1.00000000000000))*(cj7)*(x1237)*(x1250)))+(((x1243)*(x1247)))+(((cj7)*(x1251)))+(((IkReal(-1.00000000000000))*(sj6)*(x1242)*(x1245)))+(((sj7)*(x1233)))+(((IkReal(-1.00000000000000))*(x1234)*(x1236))));
evalcond[7]=((IkReal(-0.0686246300000000))+(((IkReal(-1.00000000000000))*(sj6)*(x1232)*(x1247)))+(((IkReal(-1.00000000000000))*(x1232)*(x1251)))+(((IkReal(-1.00000000000000))*(pz)*(x1239)))+(((sj6)*(x1232)*(x1245)))+(((cj6)*(x1232)*(x1250)))+(((cj6)*(cj7)*(px)))+(((x1236)*(x1238)))+(((IkReal(-1.00000000000000))*(x1233)*(x1246)))+(((py)*(x1243))));
evalcond[8]=((IkReal(-0.0320000000000000))+(((IkReal(-1.00000000000000))*(px)*(sj7)*(x1237)))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x1239)))+(((x1238)*(x1253)))+(((IkReal(-1.00000000000000))*(pz)*(x1242)))+(((IkReal(-1.00000000000000))*(x1232)*(x1233)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj7)*(x1240)))+(((cj11)*(r20)*(x1232)))+(((cj6)*(r00)*(sj7)*(x1238)))+(((IkReal(-1.00000000000000))*(x1240)*(x1252))));
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 )
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1255=((IkReal(1.00000000000000))*(r22));
IkReal x1256=((cj6)*(r02));
IkReal x1257=((r12)*(sj6));
IkReal x1258=((IkReal(1.00000000000000))*(sj7));
if( IKabs(((((cj7)*(x1257)))+(((cj7)*(x1256)))+(((IkReal(-1.00000000000000))*(sj7)*(x1255))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(x1257)*(x1258)))+(((IkReal(-1.00000000000000))*(cj7)*(x1255)))+(((IkReal(-1.00000000000000))*(x1256)*(x1258))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj7)*(x1257)))+(((cj7)*(x1256)))+(((IkReal(-1.00000000000000))*(sj7)*(x1255)))))+IKsqr(((((IkReal(-1.00000000000000))*(x1257)*(x1258)))+(((IkReal(-1.00000000000000))*(cj7)*(x1255)))+(((IkReal(-1.00000000000000))*(x1256)*(x1258)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((((cj7)*(x1257)))+(((cj7)*(x1256)))+(((IkReal(-1.00000000000000))*(sj7)*(x1255)))), ((((IkReal(-1.00000000000000))*(x1257)*(x1258)))+(((IkReal(-1.00000000000000))*(cj7)*(x1255)))+(((IkReal(-1.00000000000000))*(x1256)*(x1258)))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x1259=IKsin(j10);
IkReal x1260=((IkReal(1.00000000000000))*(cj6));
IkReal x1261=((cj7)*(sj11));
IkReal x1262=((r10)*(sj6));
IkReal x1263=((cj11)*(r21));
IkReal x1264=((sj11)*(sj7));
IkReal x1265=((cj11)*(cj7));
IkReal x1266=((IkReal(1.00000000000000))*(sj7));
IkReal x1267=((r11)*(sj6));
IkReal x1268=((r12)*(sj6));
IkReal x1269=((cj11)*(sj7));
IkReal x1270=((IkReal(1.00000000000000))*(IKcos(j10)));
evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x1266)))+(((cj6)*(cj7)*(r02)))+(((cj7)*(x1268)))+(((IkReal(-1.00000000000000))*(x1259))));
evalcond[1]=((((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1260)))+(((IkReal(-1.00000000000000))*(x1270)))+(((IkReal(-1.00000000000000))*(cj7)*(r22)))+(((IkReal(-1.00000000000000))*(x1266)*(x1268))));
evalcond[2]=((((r20)*(x1261)))+(((cj6)*(r00)*(x1264)))+(((x1262)*(x1264)))+(x1259)+(((x1267)*(x1269)))+(((cj6)*(r01)*(x1269)))+(((cj7)*(x1263))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r00)*(x1260)*(x1261)))+(((IkReal(-1.00000000000000))*(x1261)*(x1262)))+(((IkReal(-1.00000000000000))*(x1270)))+(((r20)*(x1264)))+(((sj7)*(x1263)))+(((IkReal(-1.00000000000000))*(x1265)*(x1267)))+(((IkReal(-1.00000000000000))*(r01)*(x1260)*(x1265))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
if( 1 )
{
continue;
} else
{
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1271=((IkReal(1.00000000000000))*(r02));
if( IKabs(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(sj6)*(x1271))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj6)*(sj7)*(x1271)))+(((IkReal(-1.00000000000000))*(r12)*(sj6)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(r22))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(sj6)*(x1271)))))))+IKsqr(((((IkReal(-1.00000000000000))*(cj6)*(sj7)*(x1271)))+(((IkReal(-1.00000000000000))*(r12)*(sj6)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(r22)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(sj6)*(x1271)))))), ((((IkReal(-1.00000000000000))*(cj6)*(sj7)*(x1271)))+(((IkReal(-1.00000000000000))*(r12)*(sj6)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(r22)))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[6];
IkReal x1272=IKsin(j10);
IkReal x1273=IKcos(j10);
IkReal x1274=((IkReal(1.00000000000000))*(sj6));
IkReal x1275=((cj7)*(sj11));
IkReal x1276=((cj11)*(r01));
IkReal x1277=((IkReal(1.00000000000000))*(cj7));
IkReal x1278=((IkReal(1.00000000000000))*(cj6));
IkReal x1279=((cj11)*(r21));
IkReal x1280=((r10)*(sj11));
IkReal x1281=((sj6)*(sj7));
IkReal x1282=((cj11)*(r11));
IkReal x1283=((r00)*(sj11));
IkReal x1284=((cj6)*(sj7));
evalcond[0]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1278)))+(((sj9)*(x1272))));
evalcond[1]=((((cj7)*(r12)*(sj6)))+(((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj9)*(x1272))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x1274)*(x1276)))+(((IkReal(-1.00000000000000))*(x1274)*(x1283)))+(((sj9)*(x1273)))+(((cj6)*(x1282)))+(((cj6)*(x1280))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r22)*(x1277)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1278)))+(((IkReal(-1.00000000000000))*(r12)*(sj7)*(x1274)))+(((IkReal(-1.00000000000000))*(x1273))));
evalcond[4]=((((x1281)*(x1282)))+(((cj7)*(x1279)))+(x1272)+(((x1283)*(x1284)))+(((x1280)*(x1281)))+(((x1276)*(x1284)))+(((r20)*(x1275))));
evalcond[5]=((((IkReal(-1.00000000000000))*(cj6)*(x1276)*(x1277)))+(((cj9)*(x1273)))+(((IkReal(-1.00000000000000))*(r10)*(x1274)*(x1275)))+(((r20)*(sj11)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x1274)*(x1282)))+(((sj7)*(x1279)))+(((IkReal(-1.00000000000000))*(r00)*(x1275)*(x1278))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1285=((IkReal(1.00000000000000))*(cj6));
if( IKabs(((gconst7)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x1285)))+(((r00)*(sj11)*(sj6)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x1285))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst7)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6)))))), ((gconst7)*(((((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x1285)))+(((r00)*(sj11)*(sj6)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x1285)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[6];
IkReal x1286=IKsin(j10);
IkReal x1287=IKcos(j10);
IkReal x1288=((IkReal(1.00000000000000))*(sj6));
IkReal x1289=((cj7)*(sj11));
IkReal x1290=((cj11)*(r01));
IkReal x1291=((IkReal(1.00000000000000))*(cj7));
IkReal x1292=((IkReal(1.00000000000000))*(cj6));
IkReal x1293=((cj11)*(r21));
IkReal x1294=((r10)*(sj11));
IkReal x1295=((sj6)*(sj7));
IkReal x1296=((cj11)*(r11));
IkReal x1297=((r00)*(sj11));
IkReal x1298=((cj6)*(sj7));
evalcond[0]=((((r02)*(sj6)))+(((sj9)*(x1286)))+(((IkReal(-1.00000000000000))*(r12)*(x1292))));
evalcond[1]=((((cj7)*(r12)*(sj6)))+(((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj9)*(x1286))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x1288)*(x1297)))+(((IkReal(-1.00000000000000))*(x1288)*(x1290)))+(((cj6)*(x1296)))+(((sj9)*(x1287)))+(((cj6)*(x1294))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1287)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1292)))+(((IkReal(-1.00000000000000))*(r22)*(x1291)))+(((IkReal(-1.00000000000000))*(r12)*(sj7)*(x1288))));
evalcond[4]=((((r20)*(x1289)))+(((x1294)*(x1295)))+(((x1295)*(x1296)))+(((x1297)*(x1298)))+(((cj7)*(x1293)))+(x1286)+(((x1290)*(x1298))));
evalcond[5]=((((IkReal(-1.00000000000000))*(cj6)*(x1290)*(x1291)))+(((IkReal(-1.00000000000000))*(cj7)*(x1288)*(x1296)))+(((IkReal(-1.00000000000000))*(r10)*(x1288)*(x1289)))+(((cj9)*(x1287)))+(((IkReal(-1.00000000000000))*(r00)*(x1289)*(x1292)))+(((r20)*(sj11)*(sj7)))+(((sj7)*(x1293))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
IkReal x1299=((IkReal(0.0620002000000000))*(sj11));
IkReal x1300=((cj7)*(r21));
IkReal x1301=((IkReal(1.00000000000000))*(cj11));
IkReal x1302=((cj7)*(r20));
IkReal x1303=((cj6)*(sj7));
IkReal x1304=((r01)*(sj6));
IkReal x1305=((IkReal(0.0620002000000000))*(cj11));
IkReal x1306=((cj6)*(r10));
IkReal x1307=((IkReal(1.00000000000000))*(cj7));
IkReal x1308=((cj7)*(sj6));
IkReal x1309=((r00)*(sj6));
IkReal x1310=((r11)*(sj11));
IkReal x1311=((IkReal(1.00000000000000))*(py));
IkReal x1312=((sj6)*(sj7));
IkReal x1313=((r21)*(sj7));
IkReal x1314=((cj6)*(cj7));
IkReal x1315=((r01)*(x1303));
IkReal x1316=((cj11)*(r20)*(sj7));
IkReal x1317=((r11)*(x1312));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j8)), IkReal(6.28318530717959))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x1301)*(x1306)))+(((cj11)*(x1309)))+(cj9)+(((cj6)*(x1310)))+(((IkReal(-1.00000000000000))*(sj11)*(x1304))));
evalcond[2]=((IkReal(-0.0100933000000000))+(((IkReal(-0.00151567000000000))*(sj9)))+(((IkReal(-0.0616850000000000))*(cj9)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x1299)))+(((x1305)*(x1306)))+(((x1299)*(x1304)))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(cj6)*(x1311)))+(((IkReal(-1.00000000000000))*(x1305)*(x1309))));
evalcond[3]=((((x1310)*(x1312)))+(((sj11)*(x1300)))+(((IkReal(-1.00000000000000))*(r10)*(x1301)*(x1312)))+(((IkReal(-1.00000000000000))*(r00)*(x1301)*(x1303)))+(((sj11)*(x1315)))+(((IkReal(-1.00000000000000))*(x1301)*(x1302))));
evalcond[4]=((((sj11)*(x1313)))+(((cj11)*(r10)*(x1308)))+(sj9)+(((IkReal(-1.00000000000000))*(r20)*(sj7)*(x1301)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj11)*(x1307)))+(((IkReal(-1.00000000000000))*(sj6)*(x1307)*(x1310)))+(((cj11)*(r00)*(x1314))));
evalcond[5]=((IkReal(0.0701403000000000))+(((px)*(x1314)))+(((IkReal(-1.00000000000000))*(pz)*(sj7)))+(((IkReal(-1.00000000000000))*(r00)*(x1305)*(x1314)))+(((IkReal(0.00151567000000000))*(cj9)))+(((r11)*(x1299)*(x1308)))+(((r01)*(x1299)*(x1314)))+(((r20)*(sj7)*(x1305)))+(((py)*(x1308)))+(((IkReal(-0.0616850000000000))*(sj9)))+(((IkReal(-1.00000000000000))*(r10)*(x1305)*(x1308)))+(((IkReal(-1.00000000000000))*(x1299)*(x1313))));
evalcond[6]=((IkReal(0.572000000000000))+(((IkReal(-1.00000000000000))*(x1311)*(x1312)))+(((IkReal(-1.00000000000000))*(pz)*(x1307)))+(((r10)*(x1305)*(x1312)))+(((IkReal(-1.00000000000000))*(x1299)*(x1300)))+(((r00)*(x1303)*(x1305)))+(((IkReal(-1.00000000000000))*(px)*(x1303)))+(((x1302)*(x1305)))+(((IkReal(-1.00000000000000))*(x1299)*(x1315)))+(((IkReal(-1.00000000000000))*(x1299)*(x1317))));
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 dummyeval[1];
IkReal gconst8;
gconst8=IKsign(sj9);
dummyeval[0]=sj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
dummyeval[0]=sj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal evalcond[9];
IkReal x1318=((IkReal(0.0620002000000000))*(cj7));
IkReal x1319=((r21)*(sj11));
IkReal x1320=((IkReal(1.00000000000000))*(cj11));
IkReal x1321=((r01)*(sj6));
IkReal x1322=((r20)*(sj7));
IkReal x1323=((IkReal(1.00000000000000))*(cj6));
IkReal x1324=((IkReal(0.0620002000000000))*(cj11));
IkReal x1325=((IkReal(1.00000000000000))*(sj7));
IkReal x1326=((IkReal(0.0620002000000000))*(sj11));
IkReal x1327=((cj6)*(r10));
IkReal x1328=((IkReal(1.00000000000000))*(cj7));
IkReal x1329=((cj7)*(sj6));
IkReal x1330=((r00)*(sj6));
IkReal x1331=((r11)*(sj11));
IkReal x1332=((IkReal(0.0620002000000000))*(sj7));
IkReal x1333=((cj11)*(r10));
IkReal x1334=((IkReal(1.00000000000000))*(sj11));
IkReal x1335=((cj6)*(r11));
IkReal x1336=((r01)*(sj11));
IkReal x1337=((cj11)*(cj6)*(r00));
IkReal x1338=((r11)*(sj6)*(sj7));
IkReal x1339=((r10)*(sj6)*(sj7));
IkReal x1340=((cj6)*(sj7)*(x1336));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j9)), IkReal(6.28318530717959))));
evalcond[1]=((((IkReal(-1.00000000000000))*(r12)*(x1323)))+(((r02)*(sj6))));
evalcond[2]=((IkReal(1.00000000000000))+(((cj11)*(x1330)))+(((IkReal(-1.00000000000000))*(x1321)*(x1334)))+(((IkReal(-1.00000000000000))*(x1320)*(x1327)))+(((cj6)*(x1331))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1330)*(x1334)))+(((sj11)*(x1327)))+(((cj11)*(x1335)))+(((IkReal(-1.00000000000000))*(x1320)*(x1321))));
evalcond[4]=((IkReal(-0.0717783000000000))+(((IkReal(-1.00000000000000))*(x1326)*(x1335)))+(((IkReal(-1.00000000000000))*(x1324)*(x1330)))+(((px)*(sj6)))+(((x1321)*(x1326)))+(((IkReal(-1.00000000000000))*(py)*(x1323)))+(((x1324)*(x1327))));
evalcond[5]=((((IkReal(-1.00000000000000))*(x1320)*(x1339)))+(((cj7)*(x1319)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj7)*(x1320)))+(((sj6)*(sj7)*(x1331)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1320)))+(x1340));
evalcond[6]=((((x1329)*(x1333)))+(((cj7)*(x1337)))+(((IkReal(-1.00000000000000))*(cj7)*(x1323)*(x1336)))+(((sj7)*(x1319)))+(((IkReal(-1.00000000000000))*(x1320)*(x1322)))+(((IkReal(-1.00000000000000))*(sj6)*(x1328)*(x1331))));
evalcond[7]=((IkReal(0.0716559700000000))+(((cj6)*(x1318)*(x1336)))+(((cj6)*(cj7)*(px)))+(((py)*(x1329)))+(((IkReal(-1.00000000000000))*(x1318)*(x1337)))+(((IkReal(-1.00000000000000))*(pz)*(x1325)))+(((IkReal(-1.00000000000000))*(sj6)*(x1318)*(x1333)))+(((sj6)*(x1318)*(x1331)))+(((x1322)*(x1324)))+(((IkReal(-1.00000000000000))*(x1319)*(x1332))));
evalcond[8]=((IkReal(0.572000000000000))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x1325)))+(((x1324)*(x1339)))+(((cj6)*(r00)*(sj7)*(x1324)))+(((IkReal(-1.00000000000000))*(pz)*(x1328)))+(((IkReal(-1.00000000000000))*(px)*(sj7)*(x1323)))+(((IkReal(-1.00000000000000))*(x1326)*(x1338)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj7)*(x1326)))+(((IkReal(-1.00000000000000))*(x1318)*(x1319)))+(((cj11)*(r20)*(x1318))));
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 )
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1341=((r12)*(sj6));
IkReal x1342=((cj6)*(r02));
if( IKabs(((((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(x1342)))+(((cj7)*(x1341))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj7)*(r22)))+(((sj7)*(x1341)))+(((sj7)*(x1342))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(x1342)))+(((cj7)*(x1341)))))+IKsqr(((((cj7)*(r22)))+(((sj7)*(x1341)))+(((sj7)*(x1342)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(x1342)))+(((cj7)*(x1341)))), ((((cj7)*(r22)))+(((sj7)*(x1341)))+(((sj7)*(x1342)))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x1343=IKcos(j10);
IkReal x1344=((IkReal(1.00000000000000))*(cj6));
IkReal x1345=((cj7)*(sj11));
IkReal x1346=((r10)*(sj6));
IkReal x1347=((cj11)*(r21));
IkReal x1348=((sj11)*(sj7));
IkReal x1349=((cj11)*(cj7));
IkReal x1350=((IkReal(1.00000000000000))*(sj7));
IkReal x1351=((r11)*(sj6));
IkReal x1352=((r12)*(sj6));
IkReal x1353=((cj11)*(sj7));
IkReal x1354=((IkReal(1.00000000000000))*(IKsin(j10)));
evalcond[0]=((((cj6)*(cj7)*(r02)))+(((cj7)*(x1352)))+(((IkReal(-1.00000000000000))*(x1354)))+(((IkReal(-1.00000000000000))*(r22)*(x1350))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x1350)*(x1352)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1344)))+(x1343)+(((IkReal(-1.00000000000000))*(cj7)*(r22))));
evalcond[2]=((((r20)*(x1345)))+(((cj7)*(x1347)))+(((cj6)*(r01)*(x1353)))+(((cj6)*(r00)*(x1348)))+(((x1346)*(x1348)))+(((IkReal(-1.00000000000000))*(x1354)))+(((x1351)*(x1353))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1345)*(x1346)))+(((IkReal(-1.00000000000000))*(r00)*(x1344)*(x1345)))+(((r20)*(x1348)))+(((IkReal(-1.00000000000000))*(r01)*(x1344)*(x1349)))+(((IkReal(-1.00000000000000))*(x1343)))+(((sj7)*(x1347)))+(((IkReal(-1.00000000000000))*(x1349)*(x1351))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
IkReal x1355=((IkReal(0.0620002000000000))*(cj7));
IkReal x1356=((r21)*(sj11));
IkReal x1357=((IkReal(1.00000000000000))*(cj11));
IkReal x1358=((r01)*(sj6));
IkReal x1359=((r20)*(sj7));
IkReal x1360=((IkReal(1.00000000000000))*(cj6));
IkReal x1361=((IkReal(0.0620002000000000))*(cj11));
IkReal x1362=((IkReal(1.00000000000000))*(sj7));
IkReal x1363=((IkReal(0.0620002000000000))*(sj11));
IkReal x1364=((cj6)*(r10));
IkReal x1365=((IkReal(1.00000000000000))*(cj7));
IkReal x1366=((cj7)*(sj6));
IkReal x1367=((r00)*(sj6));
IkReal x1368=((r11)*(sj11));
IkReal x1369=((IkReal(0.0620002000000000))*(sj7));
IkReal x1370=((cj11)*(r10));
IkReal x1371=((IkReal(1.00000000000000))*(sj11));
IkReal x1372=((cj6)*(r11));
IkReal x1373=((r01)*(sj11));
IkReal x1374=((cj11)*(cj6)*(r00));
IkReal x1375=((r11)*(sj6)*(sj7));
IkReal x1376=((r10)*(sj6)*(sj7));
IkReal x1377=((cj6)*(sj7)*(x1373));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j9, IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1360))));
evalcond[2]=((IkReal(-1.00000000000000))+(((cj6)*(x1368)))+(((IkReal(-1.00000000000000))*(x1358)*(x1371)))+(((cj11)*(x1367)))+(((IkReal(-1.00000000000000))*(x1357)*(x1364))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1357)*(x1358)))+(((cj11)*(x1372)))+(((IkReal(-1.00000000000000))*(x1367)*(x1371)))+(((sj11)*(x1364))));
evalcond[4]=((IkReal(0.0515917000000000))+(((IkReal(-1.00000000000000))*(x1361)*(x1367)))+(((x1361)*(x1364)))+(((IkReal(-1.00000000000000))*(py)*(x1360)))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(x1363)*(x1372)))+(((x1358)*(x1363))));
evalcond[5]=((((IkReal(-1.00000000000000))*(x1357)*(x1376)))+(((sj6)*(sj7)*(x1368)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj7)*(x1357)))+(((cj7)*(x1356)))+(x1377)+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1357))));
evalcond[6]=((((x1366)*(x1370)))+(((IkReal(-1.00000000000000))*(sj6)*(x1365)*(x1368)))+(((cj7)*(x1374)))+(((sj7)*(x1356)))+(((IkReal(-1.00000000000000))*(x1357)*(x1359)))+(((IkReal(-1.00000000000000))*(cj7)*(x1360)*(x1373))));
evalcond[7]=((IkReal(0.0686246300000000))+(((IkReal(-1.00000000000000))*(pz)*(x1362)))+(((py)*(x1366)))+(((cj6)*(cj7)*(px)))+(((x1359)*(x1361)))+(((IkReal(-1.00000000000000))*(sj6)*(x1355)*(x1370)))+(((cj6)*(x1355)*(x1373)))+(((IkReal(-1.00000000000000))*(x1355)*(x1374)))+(((IkReal(-1.00000000000000))*(x1356)*(x1369)))+(((sj6)*(x1355)*(x1368))));
evalcond[8]=((IkReal(0.572000000000000))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(sj7)*(x1363)))+(((IkReal(-1.00000000000000))*(x1355)*(x1356)))+(((cj6)*(r00)*(sj7)*(x1361)))+(((cj11)*(r20)*(x1355)))+(((IkReal(-1.00000000000000))*(py)*(sj6)*(x1362)))+(((x1361)*(x1376)))+(((IkReal(-1.00000000000000))*(px)*(sj7)*(x1360)))+(((IkReal(-1.00000000000000))*(pz)*(x1365)))+(((IkReal(-1.00000000000000))*(x1363)*(x1375))));
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 )
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1378=((r12)*(sj6));
IkReal x1379=((cj6)*(r02));
IkReal x1380=((IkReal(1.00000000000000))*(cj7));
if( IKabs(((((IkReal(-1.00000000000000))*(x1378)*(x1380)))+(((IkReal(-1.00000000000000))*(x1379)*(x1380)))+(((r22)*(sj7))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((sj7)*(x1378)))+(((cj7)*(r22)))+(((sj7)*(x1379))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(x1378)*(x1380)))+(((IkReal(-1.00000000000000))*(x1379)*(x1380)))+(((r22)*(sj7)))))+IKsqr(((((sj7)*(x1378)))+(((cj7)*(r22)))+(((sj7)*(x1379)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((((IkReal(-1.00000000000000))*(x1378)*(x1380)))+(((IkReal(-1.00000000000000))*(x1379)*(x1380)))+(((r22)*(sj7)))), ((((sj7)*(x1378)))+(((cj7)*(r22)))+(((sj7)*(x1379)))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[4];
IkReal x1381=IKsin(j10);
IkReal x1382=IKcos(j10);
IkReal x1383=((IkReal(1.00000000000000))*(r22));
IkReal x1384=((cj6)*(r00));
IkReal x1385=((r10)*(sj6));
IkReal x1386=((r20)*(sj11));
IkReal x1387=((cj11)*(r21));
IkReal x1388=((sj11)*(sj7));
IkReal x1389=((cj6)*(r01));
IkReal x1390=((r11)*(sj6));
IkReal x1391=((cj6)*(r02));
IkReal x1392=((r12)*(sj6));
IkReal x1393=((IkReal(1.00000000000000))*(sj7));
IkReal x1394=((cj11)*(sj7));
IkReal x1395=((IkReal(1.00000000000000))*(cj7)*(sj11));
IkReal x1396=((IkReal(1.00000000000000))*(cj11)*(cj7));
evalcond[0]=((x1381)+(((cj7)*(x1392)))+(((cj7)*(x1391)))+(((IkReal(-1.00000000000000))*(sj7)*(x1383))));
evalcond[1]=((x1382)+(((IkReal(-1.00000000000000))*(cj7)*(x1383)))+(((IkReal(-1.00000000000000))*(x1391)*(x1393)))+(((IkReal(-1.00000000000000))*(x1392)*(x1393))));
evalcond[2]=((((x1389)*(x1394)))+(((x1384)*(x1388)))+(((IkReal(-1.00000000000000))*(x1381)))+(((x1390)*(x1394)))+(((x1385)*(x1388)))+(((cj7)*(x1387)))+(((cj7)*(x1386))));
evalcond[3]=((x1382)+(((sj7)*(x1387)))+(((sj7)*(x1386)))+(((IkReal(-1.00000000000000))*(x1389)*(x1396)))+(((IkReal(-1.00000000000000))*(x1385)*(x1395)))+(((IkReal(-1.00000000000000))*(x1384)*(x1395)))+(((IkReal(-1.00000000000000))*(x1390)*(x1396))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
{
continue;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
if( 1 )
{
continue;
} else
{
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
if( IKabs(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj7)*(r22)))+(((cj6)*(r02)*(sj7)))+(((r12)*(sj6)*(sj7))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6)))))))+IKsqr(((((cj7)*(r22)))+(((cj6)*(r02)*(sj7)))+(((r12)*(sj6)*(sj7)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30))*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6)))))), ((((cj7)*(r22)))+(((cj6)*(r02)*(sj7)))+(((r12)*(sj6)*(sj7)))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[6];
IkReal x1397=IKsin(j10);
IkReal x1398=IKcos(j10);
IkReal x1399=((IkReal(1.00000000000000))*(cj9));
IkReal x1400=((IkReal(1.00000000000000))*(sj6));
IkReal x1401=((cj7)*(sj11));
IkReal x1402=((cj11)*(r01));
IkReal x1403=((IkReal(1.00000000000000))*(cj7));
IkReal x1404=((IkReal(1.00000000000000))*(cj6));
IkReal x1405=((cj11)*(r21));
IkReal x1406=((r10)*(sj11));
IkReal x1407=((sj6)*(sj7));
IkReal x1408=((cj11)*(r11));
IkReal x1409=((r00)*(sj11));
IkReal x1410=((cj6)*(sj7));
evalcond[0]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1404)))+(((sj9)*(x1397))));
evalcond[1]=((((cj7)*(r12)*(sj6)))+(((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x1397)*(x1399))));
evalcond[2]=((((cj6)*(x1408)))+(((cj6)*(x1406)))+(((IkReal(-1.00000000000000))*(x1400)*(x1402)))+(((sj9)*(x1398)))+(((IkReal(-1.00000000000000))*(x1400)*(x1409))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj7)*(x1400)))+(((IkReal(-1.00000000000000))*(r22)*(x1403)))+(x1398)+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1404))));
evalcond[4]=((((r20)*(x1401)))+(((cj7)*(x1405)))+(((x1409)*(x1410)))+(((x1406)*(x1407)))+(((x1407)*(x1408)))+(((x1402)*(x1410)))+(((IkReal(-1.00000000000000))*(x1397))));
evalcond[5]=((((IkReal(-1.00000000000000))*(cj7)*(x1400)*(x1408)))+(((IkReal(-1.00000000000000))*(cj6)*(x1402)*(x1403)))+(((IkReal(-1.00000000000000))*(x1398)*(x1399)))+(((sj7)*(x1405)))+(((r20)*(sj11)*(sj7)))+(((IkReal(-1.00000000000000))*(r10)*(x1400)*(x1401)))+(((IkReal(-1.00000000000000))*(r00)*(x1401)*(x1404))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1411=((IkReal(1.00000000000000))*(cj6));
if( IKabs(((gconst8)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst8)*(((((IkReal(-1.00000000000000))*(cj11)*(r11)*(x1411)))+(((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x1411)))+(((r00)*(sj11)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst8)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6)))))), ((gconst8)*(((((IkReal(-1.00000000000000))*(cj11)*(r11)*(x1411)))+(((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x1411)))+(((r00)*(sj11)*(sj6)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[6];
IkReal x1412=IKsin(j10);
IkReal x1413=IKcos(j10);
IkReal x1414=((IkReal(1.00000000000000))*(cj9));
IkReal x1415=((IkReal(1.00000000000000))*(sj6));
IkReal x1416=((cj7)*(sj11));
IkReal x1417=((cj11)*(r01));
IkReal x1418=((IkReal(1.00000000000000))*(cj7));
IkReal x1419=((IkReal(1.00000000000000))*(cj6));
IkReal x1420=((cj11)*(r21));
IkReal x1421=((r10)*(sj11));
IkReal x1422=((sj6)*(sj7));
IkReal x1423=((cj11)*(r11));
IkReal x1424=((r00)*(sj11));
IkReal x1425=((cj6)*(sj7));
evalcond[0]=((((r02)*(sj6)))+(((sj9)*(x1412)))+(((IkReal(-1.00000000000000))*(r12)*(x1419))));
evalcond[1]=((((cj7)*(r12)*(sj6)))+(((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x1412)*(x1414))));
evalcond[2]=((((cj6)*(x1421)))+(((cj6)*(x1423)))+(((IkReal(-1.00000000000000))*(x1415)*(x1424)))+(((IkReal(-1.00000000000000))*(x1415)*(x1417)))+(((sj9)*(x1413))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1419)))+(x1413)+(((IkReal(-1.00000000000000))*(r22)*(x1418)))+(((IkReal(-1.00000000000000))*(r12)*(sj7)*(x1415))));
evalcond[4]=((((cj7)*(x1420)))+(((x1424)*(x1425)))+(((x1422)*(x1423)))+(((x1421)*(x1422)))+(((r20)*(x1416)))+(((x1417)*(x1425)))+(((IkReal(-1.00000000000000))*(x1412))));
evalcond[5]=((((IkReal(-1.00000000000000))*(cj6)*(x1417)*(x1418)))+(((IkReal(-1.00000000000000))*(x1413)*(x1414)))+(((IkReal(-1.00000000000000))*(r10)*(x1415)*(x1416)))+(((IkReal(-1.00000000000000))*(r00)*(x1416)*(x1419)))+(((r20)*(sj11)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x1415)*(x1423)))+(((sj7)*(x1420))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
if( 1 )
{
continue;
} else
{
}
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1426=((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30);
IkReal x1427=((cj9)*(sj8));
IkReal x1428=((cj6)*(r12));
IkReal x1429=((cj7)*(sj9));
IkReal x1430=((IkReal(1.00000000000000))*(r02)*(sj6));
if( IKabs(((x1426)*(((x1428)+(((IkReal(-1.00000000000000))*(x1430))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x1426)*(((IKabs(cj8) != 0)?((IkReal)1/(cj8)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x1427)*(x1430)))+(((r12)*(sj6)*(x1429)))+(((x1427)*(x1428)))+(((cj6)*(r02)*(x1429)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(sj9))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1426)*(((x1428)+(((IkReal(-1.00000000000000))*(x1430)))))))+IKsqr(((x1426)*(((IKabs(cj8) != 0)?((IkReal)1/(cj8)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x1427)*(x1430)))+(((r12)*(sj6)*(x1429)))+(((x1427)*(x1428)))+(((cj6)*(r02)*(x1429)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(sj9)))))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j10array[0]=IKatan2(((x1426)*(((x1428)+(((IkReal(-1.00000000000000))*(x1430)))))), ((x1426)*(((IKabs(cj8) != 0)?((IkReal)1/(cj8)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x1427)*(x1430)))+(((r12)*(sj6)*(x1429)))+(((x1427)*(x1428)))+(((cj6)*(r02)*(x1429)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(sj9)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[6];
IkReal x1431=IKsin(j10);
IkReal x1432=IKcos(j10);
IkReal x1433=((cj9)*(sj8));
IkReal x1434=((cj6)*(cj7));
IkReal x1435=((IkReal(1.00000000000000))*(cj7));
IkReal x1436=((IkReal(1.00000000000000))*(cj6));
IkReal x1437=((r20)*(sj11));
IkReal x1438=((cj11)*(r21));
IkReal x1439=((cj6)*(sj11));
IkReal x1440=((cj11)*(cj6));
IkReal x1441=((r12)*(sj6));
IkReal x1442=((IkReal(1.00000000000000))*(sj7));
IkReal x1443=((cj8)*(x1431));
IkReal x1444=((IkReal(1.00000000000000))*(cj11)*(r01));
IkReal x1445=((r10)*(sj11)*(sj6));
IkReal x1446=((cj11)*(r11)*(sj6));
IkReal x1447=((IkReal(1.00000000000000))*(r00)*(sj11));
IkReal x1448=((IkReal(1.00000000000000))*(cj8)*(x1432));
evalcond[0]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1436)))+(((sj9)*(x1431))));
evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(x1442)))+(((cj7)*(x1441)))+(((x1431)*(x1433)))+(((r02)*(x1434)))+(((IkReal(-1.00000000000000))*(x1448))));
evalcond[2]=((((r10)*(x1439)))+(((r11)*(x1440)))+(((IkReal(-1.00000000000000))*(sj6)*(x1444)))+(((sj9)*(x1432)))+(((IkReal(-1.00000000000000))*(sj6)*(x1447))));
evalcond[3]=((((IkReal(-1.00000000000000))*(sj8)*(x1432)))+(((IkReal(-1.00000000000000))*(r22)*(x1435)))+(((IkReal(-1.00000000000000))*(cj9)*(x1443)))+(((IkReal(-1.00000000000000))*(x1441)*(x1442)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1436))));
evalcond[4]=((((cj7)*(x1438)))+(((r01)*(sj7)*(x1440)))+(((r00)*(sj7)*(x1439)))+(((sj8)*(x1431)))+(((sj7)*(x1445)))+(((cj7)*(x1437)))+(((sj7)*(x1446)))+(((IkReal(-1.00000000000000))*(cj9)*(x1448))));
evalcond[5]=((((x1432)*(x1433)))+(((IkReal(-1.00000000000000))*(x1434)*(x1447)))+(((IkReal(-1.00000000000000))*(x1435)*(x1445)))+(((IkReal(-1.00000000000000))*(x1435)*(x1446)))+(((sj7)*(x1438)))+(((sj7)*(x1437)))+(((IkReal(-1.00000000000000))*(x1434)*(x1444)))+(x1443));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1449=((IkReal(1.00000000000000))*(cj6));
if( IKabs(((gconst2)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x1449)))+(((r00)*(sj11)*(sj6)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x1449))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst2)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6)))))), ((gconst2)*(((((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x1449)))+(((r00)*(sj11)*(sj6)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x1449)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[6];
IkReal x1450=IKsin(j10);
IkReal x1451=IKcos(j10);
IkReal x1452=((cj9)*(sj8));
IkReal x1453=((cj6)*(cj7));
IkReal x1454=((IkReal(1.00000000000000))*(cj7));
IkReal x1455=((IkReal(1.00000000000000))*(cj6));
IkReal x1456=((r20)*(sj11));
IkReal x1457=((cj11)*(r21));
IkReal x1458=((cj6)*(sj11));
IkReal x1459=((cj11)*(cj6));
IkReal x1460=((r12)*(sj6));
IkReal x1461=((IkReal(1.00000000000000))*(sj7));
IkReal x1462=((cj8)*(x1450));
IkReal x1463=((IkReal(1.00000000000000))*(cj11)*(r01));
IkReal x1464=((r10)*(sj11)*(sj6));
IkReal x1465=((cj11)*(r11)*(sj6));
IkReal x1466=((IkReal(1.00000000000000))*(r00)*(sj11));
IkReal x1467=((IkReal(1.00000000000000))*(cj8)*(x1451));
evalcond[0]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1455)))+(((sj9)*(x1450))));
evalcond[1]=((((x1450)*(x1452)))+(((cj7)*(x1460)))+(((IkReal(-1.00000000000000))*(x1467)))+(((r02)*(x1453)))+(((IkReal(-1.00000000000000))*(r22)*(x1461))));
evalcond[2]=((((r10)*(x1458)))+(((sj9)*(x1451)))+(((r11)*(x1459)))+(((IkReal(-1.00000000000000))*(sj6)*(x1463)))+(((IkReal(-1.00000000000000))*(sj6)*(x1466))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1455)))+(((IkReal(-1.00000000000000))*(r22)*(x1454)))+(((IkReal(-1.00000000000000))*(sj8)*(x1451)))+(((IkReal(-1.00000000000000))*(x1460)*(x1461)))+(((IkReal(-1.00000000000000))*(cj9)*(x1462))));
evalcond[4]=((((IkReal(-1.00000000000000))*(cj9)*(x1467)))+(((sj7)*(x1465)))+(((sj7)*(x1464)))+(((cj7)*(x1456)))+(((cj7)*(x1457)))+(((sj8)*(x1450)))+(((r00)*(sj7)*(x1458)))+(((r01)*(sj7)*(x1459))));
evalcond[5]=((((x1451)*(x1452)))+(((IkReal(-1.00000000000000))*(x1454)*(x1465)))+(x1462)+(((IkReal(-1.00000000000000))*(x1454)*(x1464)))+(((sj7)*(x1456)))+(((IkReal(-1.00000000000000))*(x1453)*(x1466)))+(((IkReal(-1.00000000000000))*(x1453)*(x1463)))+(((sj7)*(x1457))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
}
}
}
}
} else
{
{
IkReal j10array[1], cj10array[1], sj10array[1];
bool j10valid[1]={false};
_nj10 = 1;
IkReal x1468=((IkReal(1.00000000000000))*(cj6));
if( IKabs(((gconst1)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(r10)*(sj11)*(x1468)))+(((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x1468)))+(((r00)*(sj11)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j10array[0]=IKatan2(((gconst1)*(((((cj6)*(r12)))+(((IkReal(-1.00000000000000))*(r02)*(sj6)))))), ((gconst1)*(((((IkReal(-1.00000000000000))*(r10)*(sj11)*(x1468)))+(((cj11)*(r01)*(sj6)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x1468)))+(((r00)*(sj11)*(sj6)))))));
sj10array[0]=IKsin(j10array[0]);
cj10array[0]=IKcos(j10array[0]);
if( j10array[0] > IKPI )
{
j10array[0]-=IK2PI;
}
else if( j10array[0] < -IKPI )
{ j10array[0]+=IK2PI;
}
j10valid[0] = true;
for(int ij10 = 0; ij10 < 1; ++ij10)
{
if( !j10valid[ij10] )
{
continue;
}
_ij10[0] = ij10; _ij10[1] = -1;
for(int iij10 = ij10+1; iij10 < 1; ++iij10)
{
if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
{
j10valid[iij10]=false; _ij10[1] = iij10; break;
}
}
j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
{
IkReal evalcond[2];
IkReal x1469=((IkReal(1.00000000000000))*(sj6));
evalcond[0]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(cj6)*(r12)))+(((sj9)*(IKsin(j10)))));
evalcond[1]=((((cj11)*(cj6)*(r11)))+(((IkReal(-1.00000000000000))*(r00)*(sj11)*(x1469)))+(((cj6)*(r10)*(sj11)))+(((sj9)*(IKcos(j10))))+(((IkReal(-1.00000000000000))*(cj11)*(r01)*(x1469))));
if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
{
continue;
}
}
{
IkReal dummyeval[1];
IkReal gconst9;
gconst9=IKsign((((cj10)*(cj10))+((((cj9)*(cj9))*((sj10)*(sj10))))));
dummyeval[0]=(((cj10)*(cj10))+((((cj9)*(cj9))*((sj10)*(sj10)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[2];
IkReal x1470=(cj9)*(cj9);
dummyeval[0]=((((sj10)*(x1470)))+(((IkReal(-1.00000000000000))*(sj10))));
dummyeval[1]=((((cj10)*(x1470)))+(((IkReal(-1.00000000000000))*(cj10))));
if( IKabs(dummyeval[0]) < 0.0000010000000000 || IKabs(dummyeval[1]) < 0.0000010000000000 )
{
{
IkReal dummyeval[2];
dummyeval[0]=sj9;
dummyeval[1]=cj10;
if( IKabs(dummyeval[0]) < 0.0000010000000000 || IKabs(dummyeval[1]) < 0.0000010000000000 )
{
{
IkReal evalcond[7];
IkReal x1471=((IkReal(1.00000000000000))*(cj11));
IkReal x1472=((r01)*(sj6));
IkReal x1473=((cj6)*(sj7));
IkReal x1474=((r01)*(sj11));
IkReal x1475=((IkReal(1.00000000000000))*(cj6));
IkReal x1476=((IkReal(0.0620002000000000))*(sj11));
IkReal x1477=((cj11)*(r10));
IkReal x1478=((r11)*(sj11));
IkReal x1479=((sj6)*(sj7));
IkReal x1480=((r21)*(sj11));
IkReal x1481=((r00)*(sj6));
IkReal x1482=((cj7)*(sj6));
IkReal x1483=((cj6)*(r10));
IkReal x1484=((IkReal(1.00000000000000))*(sj11));
IkReal x1485=((cj11)*(cj6));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j9)), IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1475))));
evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1472)*(x1484)))+(((cj11)*(x1481)))+(((IkReal(-1.00000000000000))*(x1471)*(x1483)))+(((cj6)*(x1478))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1471)*(x1472)))+(((sj11)*(x1483)))+(((r11)*(x1485)))+(((IkReal(-1.00000000000000))*(x1481)*(x1484))));
evalcond[4]=((IkReal(-0.0717783000000000))+(((IkReal(-0.0620002000000000))*(cj11)*(x1481)))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x1476)))+(((IkReal(-1.00000000000000))*(py)*(x1475)))+(((x1472)*(x1476)))+(((IkReal(0.0620002000000000))*(cj6)*(x1477))));
evalcond[5]=((((IkReal(-1.00000000000000))*(r00)*(x1471)*(x1473)))+(((x1478)*(x1479)))+(((cj7)*(x1480)))+(((x1473)*(x1474)))+(((IkReal(-1.00000000000000))*(r10)*(x1471)*(x1479)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1471))));
evalcond[6]=((((IkReal(-1.00000000000000))*(x1478)*(x1482)))+(((IkReal(-1.00000000000000))*(r20)*(sj7)*(x1471)))+(((cj7)*(r00)*(x1485)))+(((x1477)*(x1482)))+(((sj7)*(x1480)))+(((IkReal(-1.00000000000000))*(cj7)*(x1474)*(x1475))));
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 dummyeval[1];
IkReal gconst10;
gconst10=IKsign((((cj10)*(cj10))+((sj10)*(sj10))));
dummyeval[0]=(((cj10)*(cj10))+((sj10)*(sj10)));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
IkReal gconst11;
gconst11=IKsign(((((IkReal(-1.00000000000000))*((sj10)*(sj10))))+(((IkReal(-1.00000000000000))*((cj10)*(cj10))))));
dummyeval[0]=((((IkReal(-1.00000000000000))*((sj10)*(sj10))))+(((IkReal(-1.00000000000000))*((cj10)*(cj10)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
continue;
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1486=((sj10)*(sj7));
IkReal x1487=((cj11)*(r21));
IkReal x1488=((cj6)*(r02));
IkReal x1489=((cj7)*(sj10));
IkReal x1490=((r12)*(sj6));
IkReal x1491=((cj10)*(sj7));
IkReal x1492=((r20)*(sj11));
IkReal x1493=((IkReal(1.00000000000000))*(cj10)*(cj7));
IkReal x1494=((cj11)*(r11)*(sj6));
IkReal x1495=((r10)*(sj11)*(sj6));
IkReal x1496=((cj11)*(cj6)*(r01));
IkReal x1497=((cj6)*(r00)*(sj11));
if( IKabs(((gconst11)*(((((IkReal(-1.00000000000000))*(r22)*(x1486)))+(((IkReal(-1.00000000000000))*(x1493)*(x1495)))+(((x1489)*(x1490)))+(((IkReal(-1.00000000000000))*(x1493)*(x1494)))+(((x1491)*(x1492)))+(((x1488)*(x1489)))+(((IkReal(-1.00000000000000))*(x1493)*(x1496)))+(((x1487)*(x1491)))+(((IkReal(-1.00000000000000))*(x1493)*(x1497))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((((r22)*(x1491)))+(((IkReal(-1.00000000000000))*(x1489)*(x1496)))+(((IkReal(-1.00000000000000))*(x1489)*(x1495)))+(((IkReal(-1.00000000000000))*(x1489)*(x1494)))+(((IkReal(-1.00000000000000))*(x1489)*(x1497)))+(((x1486)*(x1492)))+(((IkReal(-1.00000000000000))*(x1490)*(x1493)))+(((x1486)*(x1487)))+(((IkReal(-1.00000000000000))*(x1488)*(x1493))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j8array[0]=IKatan2(((gconst11)*(((((IkReal(-1.00000000000000))*(r22)*(x1486)))+(((IkReal(-1.00000000000000))*(x1493)*(x1495)))+(((x1489)*(x1490)))+(((IkReal(-1.00000000000000))*(x1493)*(x1494)))+(((x1491)*(x1492)))+(((x1488)*(x1489)))+(((IkReal(-1.00000000000000))*(x1493)*(x1496)))+(((x1487)*(x1491)))+(((IkReal(-1.00000000000000))*(x1493)*(x1497)))))), ((gconst11)*(((((r22)*(x1491)))+(((IkReal(-1.00000000000000))*(x1489)*(x1496)))+(((IkReal(-1.00000000000000))*(x1489)*(x1495)))+(((IkReal(-1.00000000000000))*(x1489)*(x1494)))+(((IkReal(-1.00000000000000))*(x1489)*(x1497)))+(((x1486)*(x1492)))+(((IkReal(-1.00000000000000))*(x1490)*(x1493)))+(((x1486)*(x1487)))+(((IkReal(-1.00000000000000))*(x1488)*(x1493)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[6];
IkReal x1498=IKsin(j8);
IkReal x1499=IKcos(j8);
IkReal x1500=((cj7)*(sj11));
IkReal x1501=((IkReal(0.0620002000000000))*(r21));
IkReal x1502=((IkReal(1.00000000000000))*(sj6));
IkReal x1503=((cj6)*(sj7));
IkReal x1504=((IkReal(0.0620002000000000))*(cj11));
IkReal x1505=((IkReal(1.00000000000000))*(cj7));
IkReal x1506=((r20)*(sj7));
IkReal x1507=((cj11)*(cj7));
IkReal x1508=((sj11)*(sj7));
IkReal x1509=((r10)*(sj6));
IkReal x1510=((cj11)*(r01));
IkReal x1511=((cj6)*(cj7));
IkReal x1512=((cj7)*(sj6));
IkReal x1513=((r11)*(sj6));
IkReal x1514=((IkReal(1.00000000000000))*(sj7));
IkReal x1515=((IkReal(0.0620002000000000))*(r01));
IkReal x1516=((cj11)*(sj7));
IkReal x1517=((sj10)*(x1498));
IkReal x1518=((IkReal(1.00000000000000))*(x1499));
IkReal x1519=((cj10)*(x1498));
IkReal x1520=((cj10)*(x1518));
evalcond[0]=((((r02)*(x1511)))+(((IkReal(-1.00000000000000))*(x1520)))+(((r12)*(x1512)))+(((IkReal(-1.00000000000000))*(r22)*(x1514)))+(x1517));
evalcond[1]=((((IkReal(-1.00000000000000))*(r12)*(sj7)*(x1502)))+(((IkReal(-1.00000000000000))*(sj10)*(x1518)))+(((IkReal(-1.00000000000000))*(r02)*(x1503)))+(((IkReal(-1.00000000000000))*(r22)*(x1505)))+(((IkReal(-1.00000000000000))*(x1519))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x1520)))+(((r21)*(x1507)))+(((x1508)*(x1509)))+(((r00)*(sj11)*(x1503)))+(((x1503)*(x1510)))+(((x1513)*(x1516)))+(((r20)*(x1500)))+(x1517));
evalcond[3]=((((IkReal(-1.00000000000000))*(cj6)*(r00)*(x1500)))+(((IkReal(-1.00000000000000))*(cj6)*(x1505)*(x1510)))+(((IkReal(-1.00000000000000))*(r10)*(x1500)*(x1502)))+(((r21)*(x1516)))+(((sj11)*(x1506)))+(((sj10)*(x1499)))+(((IkReal(-1.00000000000000))*(r11)*(x1502)*(x1507)))+(x1519));
evalcond[4]=((((cj6)*(x1500)*(x1515)))+(((IkReal(-1.00000000000000))*(r00)*(x1504)*(x1511)))+(((IkReal(-0.302000000000000))*(x1499)))+(((x1504)*(x1506)))+(((IkReal(-1.00000000000000))*(cj7)*(x1504)*(x1509)))+(((IkReal(-0.0716559700000000))*(x1498)))+(((px)*(x1511)))+(((py)*(x1512)))+(((IkReal(-1.00000000000000))*(pz)*(x1514)))+(((IkReal(-1.00000000000000))*(x1501)*(x1508)))+(((IkReal(0.0620002000000000))*(x1500)*(x1513))));
evalcond[5]=((IkReal(0.270000000000000))+(((IkReal(-1.00000000000000))*(pz)*(x1505)))+(((IkReal(-0.0620002000000000))*(x1508)*(x1513)))+(((IkReal(-0.302000000000000))*(x1498)))+(((IkReal(-1.00000000000000))*(x1500)*(x1501)))+(((IkReal(-1.00000000000000))*(px)*(x1503)))+(((r00)*(x1503)*(x1504)))+(((IkReal(-1.00000000000000))*(py)*(sj7)*(x1502)))+(((cj7)*(r20)*(x1504)))+(((IkReal(0.0716559700000000))*(x1499)))+(((IkReal(-1.00000000000000))*(sj11)*(x1503)*(x1515)))+(((sj7)*(x1504)*(x1509))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1521=((cj10)*(cj7));
IkReal x1522=((IkReal(1.00000000000000))*(r22));
IkReal x1523=((r12)*(sj6));
IkReal x1524=((IkReal(1.00000000000000))*(sj10));
IkReal x1525=((cj6)*(r02));
IkReal x1526=((cj10)*(sj7));
if( IKabs(((gconst10)*(((((IkReal(-1.00000000000000))*(cj7)*(x1524)*(x1525)))+(((IkReal(-1.00000000000000))*(x1523)*(x1526)))+(((IkReal(-1.00000000000000))*(x1521)*(x1522)))+(((IkReal(-1.00000000000000))*(x1525)*(x1526)))+(((IkReal(-1.00000000000000))*(cj7)*(x1523)*(x1524)))+(((r22)*(sj10)*(sj7))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((((x1521)*(x1525)))+(((IkReal(-1.00000000000000))*(cj7)*(sj10)*(x1522)))+(((x1521)*(x1523)))+(((IkReal(-1.00000000000000))*(sj7)*(x1523)*(x1524)))+(((IkReal(-1.00000000000000))*(sj7)*(x1524)*(x1525)))+(((IkReal(-1.00000000000000))*(x1522)*(x1526))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j8array[0]=IKatan2(((gconst10)*(((((IkReal(-1.00000000000000))*(cj7)*(x1524)*(x1525)))+(((IkReal(-1.00000000000000))*(x1523)*(x1526)))+(((IkReal(-1.00000000000000))*(x1521)*(x1522)))+(((IkReal(-1.00000000000000))*(x1525)*(x1526)))+(((IkReal(-1.00000000000000))*(cj7)*(x1523)*(x1524)))+(((r22)*(sj10)*(sj7)))))), ((gconst10)*(((((x1521)*(x1525)))+(((IkReal(-1.00000000000000))*(cj7)*(sj10)*(x1522)))+(((x1521)*(x1523)))+(((IkReal(-1.00000000000000))*(sj7)*(x1523)*(x1524)))+(((IkReal(-1.00000000000000))*(sj7)*(x1524)*(x1525)))+(((IkReal(-1.00000000000000))*(x1522)*(x1526)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[6];
IkReal x1527=IKsin(j8);
IkReal x1528=IKcos(j8);
IkReal x1529=((cj7)*(sj11));
IkReal x1530=((IkReal(0.0620002000000000))*(r21));
IkReal x1531=((IkReal(1.00000000000000))*(sj6));
IkReal x1532=((cj6)*(sj7));
IkReal x1533=((IkReal(0.0620002000000000))*(cj11));
IkReal x1534=((IkReal(1.00000000000000))*(cj7));
IkReal x1535=((r20)*(sj7));
IkReal x1536=((cj11)*(cj7));
IkReal x1537=((sj11)*(sj7));
IkReal x1538=((r10)*(sj6));
IkReal x1539=((cj11)*(r01));
IkReal x1540=((cj6)*(cj7));
IkReal x1541=((cj7)*(sj6));
IkReal x1542=((r11)*(sj6));
IkReal x1543=((IkReal(1.00000000000000))*(sj7));
IkReal x1544=((IkReal(0.0620002000000000))*(r01));
IkReal x1545=((cj11)*(sj7));
IkReal x1546=((sj10)*(x1527));
IkReal x1547=((IkReal(1.00000000000000))*(x1528));
IkReal x1548=((cj10)*(x1527));
IkReal x1549=((cj10)*(x1547));
evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x1543)))+(((r12)*(x1541)))+(x1546)+(((IkReal(-1.00000000000000))*(x1549)))+(((r02)*(x1540))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x1548)))+(((IkReal(-1.00000000000000))*(r22)*(x1534)))+(((IkReal(-1.00000000000000))*(r12)*(sj7)*(x1531)))+(((IkReal(-1.00000000000000))*(r02)*(x1532)))+(((IkReal(-1.00000000000000))*(sj10)*(x1547))));
evalcond[2]=((((x1537)*(x1538)))+(x1546)+(((x1532)*(x1539)))+(((IkReal(-1.00000000000000))*(x1549)))+(((r21)*(x1536)))+(((r00)*(sj11)*(x1532)))+(((x1542)*(x1545)))+(((r20)*(x1529))));
evalcond[3]=((((r21)*(x1545)))+(((IkReal(-1.00000000000000))*(r10)*(x1529)*(x1531)))+(((sj11)*(x1535)))+(x1548)+(((IkReal(-1.00000000000000))*(cj6)*(x1534)*(x1539)))+(((sj10)*(x1528)))+(((IkReal(-1.00000000000000))*(r11)*(x1531)*(x1536)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(x1529))));
evalcond[4]=((((cj6)*(x1529)*(x1544)))+(((IkReal(-1.00000000000000))*(r00)*(x1533)*(x1540)))+(((px)*(x1540)))+(((IkReal(-1.00000000000000))*(cj7)*(x1533)*(x1538)))+(((IkReal(-0.0716559700000000))*(x1527)))+(((IkReal(-1.00000000000000))*(x1530)*(x1537)))+(((IkReal(-1.00000000000000))*(pz)*(x1543)))+(((x1533)*(x1535)))+(((py)*(x1541)))+(((IkReal(0.0620002000000000))*(x1529)*(x1542)))+(((IkReal(-0.302000000000000))*(x1528))));
evalcond[5]=((IkReal(0.270000000000000))+(((cj7)*(r20)*(x1533)))+(((sj7)*(x1533)*(x1538)))+(((IkReal(-1.00000000000000))*(pz)*(x1534)))+(((IkReal(-1.00000000000000))*(sj11)*(x1532)*(x1544)))+(((r00)*(x1532)*(x1533)))+(((IkReal(-1.00000000000000))*(px)*(x1532)))+(((IkReal(-0.0620002000000000))*(x1537)*(x1542)))+(((IkReal(-0.302000000000000))*(x1527)))+(((IkReal(-1.00000000000000))*(x1529)*(x1530)))+(((IkReal(0.0716559700000000))*(x1528)))+(((IkReal(-1.00000000000000))*(py)*(sj7)*(x1531))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
IkReal x1550=((IkReal(1.00000000000000))*(cj11));
IkReal x1551=((r01)*(sj6));
IkReal x1552=((cj6)*(sj7));
IkReal x1553=((r01)*(sj11));
IkReal x1554=((IkReal(1.00000000000000))*(cj6));
IkReal x1555=((IkReal(0.0620002000000000))*(sj11));
IkReal x1556=((cj11)*(r10));
IkReal x1557=((r11)*(sj11));
IkReal x1558=((sj6)*(sj7));
IkReal x1559=((r21)*(sj11));
IkReal x1560=((r00)*(sj6));
IkReal x1561=((cj7)*(sj6));
IkReal x1562=((cj6)*(r10));
IkReal x1563=((IkReal(1.00000000000000))*(sj11));
IkReal x1564=((cj11)*(cj6));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j9, IkReal(6.28318530717959))));
evalcond[1]=((((IkReal(-1.00000000000000))*(r12)*(x1554)))+(((r02)*(sj6))));
evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1550)*(x1562)))+(((cj11)*(x1560)))+(((IkReal(-1.00000000000000))*(x1551)*(x1563)))+(((cj6)*(x1557))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1550)*(x1551)))+(((r11)*(x1564)))+(((IkReal(-1.00000000000000))*(x1560)*(x1563)))+(((sj11)*(x1562))));
evalcond[4]=((IkReal(0.0515917000000000))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x1555)))+(((IkReal(-0.0620002000000000))*(cj11)*(x1560)))+(((px)*(sj6)))+(((x1551)*(x1555)))+(((IkReal(-1.00000000000000))*(py)*(x1554)))+(((IkReal(0.0620002000000000))*(cj6)*(x1556))));
evalcond[5]=((((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1550)))+(((IkReal(-1.00000000000000))*(r10)*(x1550)*(x1558)))+(((IkReal(-1.00000000000000))*(r00)*(x1550)*(x1552)))+(((cj7)*(x1559)))+(((x1552)*(x1553)))+(((x1557)*(x1558))));
evalcond[6]=((((IkReal(-1.00000000000000))*(r20)*(sj7)*(x1550)))+(((IkReal(-1.00000000000000))*(x1557)*(x1561)))+(((x1556)*(x1561)))+(((IkReal(-1.00000000000000))*(cj7)*(x1553)*(x1554)))+(((sj7)*(x1559)))+(((cj7)*(r00)*(x1564))));
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 dummyeval[1];
IkReal gconst12;
gconst12=IKsign((((cj10)*(cj10))+((sj10)*(sj10))));
dummyeval[0]=(((cj10)*(cj10))+((sj10)*(sj10)));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
IkReal gconst13;
gconst13=IKsign((((cj10)*(cj10))+((sj10)*(sj10))));
dummyeval[0]=(((cj10)*(cj10))+((sj10)*(sj10)));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
continue;
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1565=((r20)*(sj11));
IkReal x1566=((r10)*(sj6));
IkReal x1567=((IkReal(1.00000000000000))*(sj11));
IkReal x1568=((sj10)*(sj7));
IkReal x1569=((IkReal(1.00000000000000))*(cj11));
IkReal x1570=((r11)*(sj6));
IkReal x1571=((r12)*(sj6));
IkReal x1572=((cj7)*(sj10));
IkReal x1573=((cj10)*(sj7));
IkReal x1574=((IkReal(1.00000000000000))*(cj10)*(cj7));
IkReal x1575=((cj6)*(x1568));
IkReal x1576=((r01)*(x1569));
IkReal x1577=((cj6)*(x1573));
IkReal x1578=((x1567)*(x1573));
if( IKabs(((gconst13)*(((((IkReal(-1.00000000000000))*(r02)*(x1577)))+(((IkReal(-1.00000000000000))*(r00)*(x1567)*(x1575)))+(((IkReal(-1.00000000000000))*(r21)*(x1569)*(x1572)))+(((IkReal(-1.00000000000000))*(x1565)*(x1572)))+(((IkReal(-1.00000000000000))*(x1571)*(x1573)))+(((IkReal(-1.00000000000000))*(x1575)*(x1576)))+(((IkReal(-1.00000000000000))*(r22)*(x1574)))+(((IkReal(-1.00000000000000))*(x1568)*(x1569)*(x1570)))+(((IkReal(-1.00000000000000))*(x1566)*(x1567)*(x1568))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst13)*(((((IkReal(-1.00000000000000))*(x1566)*(x1578)))+(((IkReal(-1.00000000000000))*(x1565)*(x1574)))+(((IkReal(-1.00000000000000))*(r00)*(x1567)*(x1577)))+(((IkReal(-1.00000000000000))*(x1569)*(x1570)*(x1573)))+(((x1568)*(x1571)))+(((r22)*(x1572)))+(((IkReal(-1.00000000000000))*(cj10)*(cj7)*(r21)*(x1569)))+(((r02)*(x1575)))+(((IkReal(-1.00000000000000))*(x1576)*(x1577))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j8array[0]=IKatan2(((gconst13)*(((((IkReal(-1.00000000000000))*(r02)*(x1577)))+(((IkReal(-1.00000000000000))*(r00)*(x1567)*(x1575)))+(((IkReal(-1.00000000000000))*(r21)*(x1569)*(x1572)))+(((IkReal(-1.00000000000000))*(x1565)*(x1572)))+(((IkReal(-1.00000000000000))*(x1571)*(x1573)))+(((IkReal(-1.00000000000000))*(x1575)*(x1576)))+(((IkReal(-1.00000000000000))*(r22)*(x1574)))+(((IkReal(-1.00000000000000))*(x1568)*(x1569)*(x1570)))+(((IkReal(-1.00000000000000))*(x1566)*(x1567)*(x1568)))))), ((gconst13)*(((((IkReal(-1.00000000000000))*(x1566)*(x1578)))+(((IkReal(-1.00000000000000))*(x1565)*(x1574)))+(((IkReal(-1.00000000000000))*(r00)*(x1567)*(x1577)))+(((IkReal(-1.00000000000000))*(x1569)*(x1570)*(x1573)))+(((x1568)*(x1571)))+(((r22)*(x1572)))+(((IkReal(-1.00000000000000))*(cj10)*(cj7)*(r21)*(x1569)))+(((r02)*(x1575)))+(((IkReal(-1.00000000000000))*(x1576)*(x1577)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[6];
IkReal x1579=IKcos(j8);
IkReal x1580=IKsin(j8);
IkReal x1581=((cj7)*(sj11));
IkReal x1582=((IkReal(0.0620002000000000))*(r21));
IkReal x1583=((IkReal(1.00000000000000))*(sj6));
IkReal x1584=((cj6)*(sj7));
IkReal x1585=((IkReal(0.0620002000000000))*(cj11));
IkReal x1586=((IkReal(1.00000000000000))*(cj7));
IkReal x1587=((r20)*(sj7));
IkReal x1588=((cj11)*(cj7));
IkReal x1589=((sj11)*(sj7));
IkReal x1590=((r10)*(sj6));
IkReal x1591=((cj11)*(r01));
IkReal x1592=((cj6)*(cj7));
IkReal x1593=((cj7)*(sj6));
IkReal x1594=((IkReal(1.00000000000000))*(cj10));
IkReal x1595=((IkReal(1.00000000000000))*(sj7));
IkReal x1596=((IkReal(0.0620002000000000))*(r01));
IkReal x1597=((r11)*(sj6));
IkReal x1598=((cj11)*(sj7));
IkReal x1599=((sj10)*(x1579));
IkReal x1600=((sj10)*(x1580));
IkReal x1601=((x1580)*(x1594));
evalcond[0]=((((IkReal(-1.00000000000000))*(x1600)))+(((IkReal(-1.00000000000000))*(r22)*(x1595)))+(((IkReal(-1.00000000000000))*(x1579)*(x1594)))+(((r12)*(x1593)))+(((r02)*(x1592))));
evalcond[1]=((((IkReal(-1.00000000000000))*(r02)*(x1584)))+(x1599)+(((IkReal(-1.00000000000000))*(x1601)))+(((IkReal(-1.00000000000000))*(r22)*(x1586)))+(((IkReal(-1.00000000000000))*(r12)*(sj7)*(x1583))));
evalcond[2]=((((x1584)*(x1591)))+(((r00)*(sj11)*(x1584)))+(((cj10)*(x1579)))+(((r21)*(x1588)))+(((x1597)*(x1598)))+(x1600)+(((r20)*(x1581)))+(((x1589)*(x1590))));
evalcond[3]=((x1599)+(((IkReal(-1.00000000000000))*(r10)*(x1581)*(x1583)))+(((sj11)*(x1587)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(x1581)))+(((IkReal(-1.00000000000000))*(x1601)))+(((IkReal(-1.00000000000000))*(cj6)*(x1586)*(x1591)))+(((r21)*(x1598)))+(((IkReal(-1.00000000000000))*(r11)*(x1583)*(x1588))));
evalcond[4]=((((x1585)*(x1587)))+(((py)*(x1593)))+(((IkReal(-0.302000000000000))*(x1579)))+(((IkReal(-1.00000000000000))*(r00)*(x1585)*(x1592)))+(((IkReal(-1.00000000000000))*(cj7)*(x1585)*(x1590)))+(((px)*(x1592)))+(((IkReal(-0.0686246300000000))*(x1580)))+(((IkReal(-1.00000000000000))*(pz)*(x1595)))+(((IkReal(-1.00000000000000))*(x1582)*(x1589)))+(((cj6)*(x1581)*(x1596)))+(((IkReal(0.0620002000000000))*(x1581)*(x1597))));
evalcond[5]=((IkReal(0.270000000000000))+(((IkReal(-1.00000000000000))*(px)*(x1584)))+(((IkReal(-1.00000000000000))*(x1581)*(x1582)))+(((r00)*(x1584)*(x1585)))+(((sj7)*(x1585)*(x1590)))+(((IkReal(-0.302000000000000))*(x1580)))+(((IkReal(-0.0620002000000000))*(x1589)*(x1597)))+(((IkReal(0.0686246300000000))*(x1579)))+(((cj7)*(r20)*(x1585)))+(((IkReal(-1.00000000000000))*(pz)*(x1586)))+(((IkReal(-1.00000000000000))*(py)*(sj7)*(x1583)))+(((IkReal(-1.00000000000000))*(sj11)*(x1584)*(x1596))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1602=((IkReal(1.00000000000000))*(r22));
IkReal x1603=((sj10)*(sj7));
IkReal x1604=((cj10)*(cj7));
IkReal x1605=((cj6)*(r02));
IkReal x1606=((r12)*(sj6));
IkReal x1607=((cj7)*(sj10));
IkReal x1608=((cj10)*(sj7));
if( IKabs(((gconst12)*(((((x1605)*(x1607)))+(((IkReal(-1.00000000000000))*(x1602)*(x1603)))+(((IkReal(-1.00000000000000))*(x1606)*(x1608)))+(((IkReal(-1.00000000000000))*(x1602)*(x1604)))+(((IkReal(-1.00000000000000))*(x1605)*(x1608)))+(((x1606)*(x1607))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst12)*(((((x1604)*(x1605)))+(((x1604)*(x1606)))+(((r22)*(x1607)))+(((x1603)*(x1605)))+(((x1603)*(x1606)))+(((IkReal(-1.00000000000000))*(x1602)*(x1608))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j8array[0]=IKatan2(((gconst12)*(((((x1605)*(x1607)))+(((IkReal(-1.00000000000000))*(x1602)*(x1603)))+(((IkReal(-1.00000000000000))*(x1606)*(x1608)))+(((IkReal(-1.00000000000000))*(x1602)*(x1604)))+(((IkReal(-1.00000000000000))*(x1605)*(x1608)))+(((x1606)*(x1607)))))), ((gconst12)*(((((x1604)*(x1605)))+(((x1604)*(x1606)))+(((r22)*(x1607)))+(((x1603)*(x1605)))+(((x1603)*(x1606)))+(((IkReal(-1.00000000000000))*(x1602)*(x1608)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[6];
IkReal x1609=IKcos(j8);
IkReal x1610=IKsin(j8);
IkReal x1611=((cj7)*(sj11));
IkReal x1612=((IkReal(0.0620002000000000))*(r21));
IkReal x1613=((IkReal(1.00000000000000))*(sj6));
IkReal x1614=((cj6)*(sj7));
IkReal x1615=((IkReal(0.0620002000000000))*(cj11));
IkReal x1616=((IkReal(1.00000000000000))*(cj7));
IkReal x1617=((r20)*(sj7));
IkReal x1618=((cj11)*(cj7));
IkReal x1619=((sj11)*(sj7));
IkReal x1620=((r10)*(sj6));
IkReal x1621=((cj11)*(r01));
IkReal x1622=((cj6)*(cj7));
IkReal x1623=((cj7)*(sj6));
IkReal x1624=((IkReal(1.00000000000000))*(cj10));
IkReal x1625=((IkReal(1.00000000000000))*(sj7));
IkReal x1626=((IkReal(0.0620002000000000))*(r01));
IkReal x1627=((r11)*(sj6));
IkReal x1628=((cj11)*(sj7));
IkReal x1629=((sj10)*(x1609));
IkReal x1630=((sj10)*(x1610));
IkReal x1631=((x1610)*(x1624));
evalcond[0]=((((IkReal(-1.00000000000000))*(x1609)*(x1624)))+(((IkReal(-1.00000000000000))*(x1630)))+(((IkReal(-1.00000000000000))*(r22)*(x1625)))+(((r02)*(x1622)))+(((r12)*(x1623))));
evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(x1616)))+(x1629)+(((IkReal(-1.00000000000000))*(x1631)))+(((IkReal(-1.00000000000000))*(r02)*(x1614)))+(((IkReal(-1.00000000000000))*(r12)*(sj7)*(x1613))));
evalcond[2]=((((r21)*(x1618)))+(((r00)*(sj11)*(x1614)))+(((x1627)*(x1628)))+(x1630)+(((x1614)*(x1621)))+(((x1619)*(x1620)))+(((cj10)*(x1609)))+(((r20)*(x1611))));
evalcond[3]=((((r21)*(x1628)))+(x1629)+(((IkReal(-1.00000000000000))*(x1631)))+(((sj11)*(x1617)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(x1611)))+(((IkReal(-1.00000000000000))*(cj6)*(x1616)*(x1621)))+(((IkReal(-1.00000000000000))*(r10)*(x1611)*(x1613)))+(((IkReal(-1.00000000000000))*(r11)*(x1613)*(x1618))));
evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x1615)*(x1622)))+(((IkReal(-1.00000000000000))*(pz)*(x1625)))+(((IkReal(-0.302000000000000))*(x1609)))+(((IkReal(-0.0686246300000000))*(x1610)))+(((cj6)*(x1611)*(x1626)))+(((IkReal(0.0620002000000000))*(x1611)*(x1627)))+(((IkReal(-1.00000000000000))*(x1612)*(x1619)))+(((py)*(x1623)))+(((x1615)*(x1617)))+(((IkReal(-1.00000000000000))*(cj7)*(x1615)*(x1620)))+(((px)*(x1622))));
evalcond[5]=((IkReal(0.270000000000000))+(((sj7)*(x1615)*(x1620)))+(((IkReal(-1.00000000000000))*(sj11)*(x1614)*(x1626)))+(((IkReal(0.0686246300000000))*(x1609)))+(((IkReal(-1.00000000000000))*(px)*(x1614)))+(((IkReal(-0.0620002000000000))*(x1619)*(x1627)))+(((IkReal(-1.00000000000000))*(pz)*(x1616)))+(((IkReal(-1.00000000000000))*(x1611)*(x1612)))+(((r00)*(x1614)*(x1615)))+(((IkReal(-0.302000000000000))*(x1610)))+(((IkReal(-1.00000000000000))*(py)*(sj7)*(x1613)))+(((cj7)*(r20)*(x1615))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
IkReal x1632=((cj6)*(r10));
IkReal x1633=((IkReal(1.00000000000000))*(cj6));
IkReal x1634=((cj6)*(r11));
IkReal x1635=((r01)*(sj6));
IkReal x1636=((IkReal(0.0620002000000000))*(sj11));
IkReal x1637=((IkReal(0.0620002000000000))*(cj11));
IkReal x1638=((IkReal(1.00000000000000))*(cj11));
IkReal x1639=((r00)*(sj6));
IkReal x1640=((IkReal(1.00000000000000))*(sj11));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j10)), IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(sj9)+(((IkReal(-1.00000000000000))*(r12)*(x1633))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x1635)*(x1640)))+(((IkReal(-1.00000000000000))*(x1632)*(x1638)))+(((sj11)*(x1634)))+(((cj11)*(x1639)))+(cj9));
evalcond[3]=((((sj11)*(x1632)))+(((cj11)*(x1634)))+(((IkReal(-1.00000000000000))*(x1635)*(x1638)))+(((IkReal(-1.00000000000000))*(x1639)*(x1640))));
evalcond[4]=((IkReal(-0.0100933000000000))+(((IkReal(-0.00151567000000000))*(sj9)))+(((IkReal(-1.00000000000000))*(x1637)*(x1639)))+(((IkReal(-0.0616850000000000))*(cj9)))+(((x1635)*(x1636)))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(py)*(x1633)))+(((IkReal(-1.00000000000000))*(x1634)*(x1636)))+(((x1632)*(x1637))));
if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
IkReal gconst14;
gconst14=IKsign(cj9);
dummyeval[0]=cj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
dummyeval[0]=cj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
dummyeval[0]=cj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal evalcond[7];
IkReal x1641=((r01)*(sj6));
IkReal x1642=((IkReal(1.00000000000000))*(cj11));
IkReal x1643=((IkReal(1.00000000000000))*(r22));
IkReal x1644=((IkReal(1.00000000000000))*(cj6));
IkReal x1645=((IkReal(0.0620002000000000))*(sj11));
IkReal x1646=((cj6)*(r10));
IkReal x1647=((IkReal(0.0620002000000000))*(cj11));
IkReal x1648=((r00)*(sj6));
IkReal x1649=((cj6)*(r11));
IkReal x1650=((IkReal(1.00000000000000))*(sj11));
IkReal x1651=((r12)*(sj6));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j9)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(1.00000000000000))+(((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1644))));
evalcond[2]=((((sj11)*(x1649)))+(((cj11)*(x1648)))+(((IkReal(-1.00000000000000))*(x1642)*(x1646)))+(((IkReal(-1.00000000000000))*(x1641)*(x1650))));
evalcond[3]=((((cj6)*(cj7)*(r02)))+(((cj7)*(x1651)))+(((IkReal(-1.00000000000000))*(sj7)*(x1643))));
evalcond[4]=((((IkReal(-1.00000000000000))*(x1641)*(x1642)))+(((cj11)*(x1649)))+(((sj11)*(x1646)))+(((IkReal(-1.00000000000000))*(x1648)*(x1650))));
evalcond[5]=((((IkReal(-1.00000000000000))*(cj7)*(x1643)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1644)))+(((IkReal(-1.00000000000000))*(sj7)*(x1651))));
evalcond[6]=((IkReal(-0.0116089700000000))+(((IkReal(-1.00000000000000))*(x1647)*(x1648)))+(((x1641)*(x1645)))+(((x1646)*(x1647)))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(x1645)*(x1649)))+(((IkReal(-1.00000000000000))*(py)*(x1644))));
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 j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1652=((cj7)*(sj11));
IkReal x1653=((r10)*(sj6));
IkReal x1654=((cj6)*(r00));
IkReal x1655=((IkReal(1.00000000000000))*(sj7));
IkReal x1656=((cj11)*(r21));
IkReal x1657=((cj11)*(cj6)*(r01));
IkReal x1658=((cj11)*(r11)*(sj6));
if( IKabs(((((IkReal(-1.00000000000000))*(x1655)*(x1657)))+(((IkReal(-1.00000000000000))*(r20)*(x1652)))+(((IkReal(-1.00000000000000))*(x1655)*(x1658)))+(((IkReal(-1.00000000000000))*(cj7)*(x1656)))+(((IkReal(-1.00000000000000))*(sj11)*(x1653)*(x1655)))+(((IkReal(-1.00000000000000))*(sj11)*(x1654)*(x1655))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj7)*(x1657)))+(((x1652)*(x1654)))+(((IkReal(-1.00000000000000))*(r20)*(sj11)*(x1655)))+(((cj7)*(x1658)))+(((x1652)*(x1653)))+(((IkReal(-1.00000000000000))*(x1655)*(x1656))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(x1655)*(x1657)))+(((IkReal(-1.00000000000000))*(r20)*(x1652)))+(((IkReal(-1.00000000000000))*(x1655)*(x1658)))+(((IkReal(-1.00000000000000))*(cj7)*(x1656)))+(((IkReal(-1.00000000000000))*(sj11)*(x1653)*(x1655)))+(((IkReal(-1.00000000000000))*(sj11)*(x1654)*(x1655)))))+IKsqr(((((cj7)*(x1657)))+(((x1652)*(x1654)))+(((IkReal(-1.00000000000000))*(r20)*(sj11)*(x1655)))+(((cj7)*(x1658)))+(((x1652)*(x1653)))+(((IkReal(-1.00000000000000))*(x1655)*(x1656)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((((IkReal(-1.00000000000000))*(x1655)*(x1657)))+(((IkReal(-1.00000000000000))*(r20)*(x1652)))+(((IkReal(-1.00000000000000))*(x1655)*(x1658)))+(((IkReal(-1.00000000000000))*(cj7)*(x1656)))+(((IkReal(-1.00000000000000))*(sj11)*(x1653)*(x1655)))+(((IkReal(-1.00000000000000))*(sj11)*(x1654)*(x1655)))), ((((cj7)*(x1657)))+(((x1652)*(x1654)))+(((IkReal(-1.00000000000000))*(r20)*(sj11)*(x1655)))+(((cj7)*(x1658)))+(((x1652)*(x1653)))+(((IkReal(-1.00000000000000))*(x1655)*(x1656)))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[6];
IkReal x1659=IKsin(j8);
IkReal x1660=IKcos(j8);
IkReal x1661=((cj7)*(sj11));
IkReal x1662=((IkReal(0.0620002000000000))*(r21));
IkReal x1663=((r10)*(sj6));
IkReal x1664=((cj11)*(sj7));
IkReal x1665=((cj6)*(r00));
IkReal x1666=((IkReal(1.00000000000000))*(r20));
IkReal x1667=((sj11)*(sj7));
IkReal x1668=((cj6)*(r01));
IkReal x1669=((IkReal(0.0620002000000000))*(r20));
IkReal x1670=((cj11)*(cj7));
IkReal x1671=((IkReal(1.00000000000000))*(pz));
IkReal x1672=((py)*(sj6));
IkReal x1673=((r11)*(sj6));
IkReal x1674=((IkReal(1.00000000000000))*(sj7));
IkReal x1675=((cj6)*(px));
evalcond[0]=((((r21)*(x1661)))+(((IkReal(-1.00000000000000))*(x1663)*(x1664)))+(((IkReal(-1.00000000000000))*(x1666)*(x1670)))+(((x1667)*(x1668)))+(x1660)+(((IkReal(-1.00000000000000))*(x1664)*(x1665)))+(((x1667)*(x1673))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x1661)*(x1668)))+(((IkReal(-1.00000000000000))*(x1661)*(x1673)))+(((IkReal(-1.00000000000000))*(x1659)))+(((r21)*(x1667)))+(((x1663)*(x1670)))+(((x1665)*(x1670)))+(((IkReal(-1.00000000000000))*(x1664)*(x1666))));
evalcond[2]=((((r21)*(x1670)))+(((x1665)*(x1667)))+(((x1664)*(x1668)))+(((r20)*(x1661)))+(((x1663)*(x1667)))+(((x1664)*(x1673)))+(x1659));
evalcond[3]=((x1660)+(((IkReal(-1.00000000000000))*(x1670)*(x1673)))+(((IkReal(-1.00000000000000))*(x1661)*(x1665)))+(((r20)*(x1667)))+(((IkReal(-1.00000000000000))*(x1661)*(x1663)))+(((r21)*(x1664)))+(((IkReal(-1.00000000000000))*(x1668)*(x1670))));
evalcond[4]=((((IkReal(-1.00000000000000))*(sj7)*(x1671)))+(((IkReal(-0.0620002000000000))*(x1663)*(x1670)))+(((IkReal(0.0620002000000000))*(x1661)*(x1673)))+(((IkReal(-0.0620002000000000))*(x1665)*(x1670)))+(((cj7)*(x1675)))+(((IkReal(-0.00845530000000000))*(x1659)))+(((IkReal(-0.302000000000000))*(x1660)))+(((x1664)*(x1669)))+(((IkReal(-1.00000000000000))*(x1662)*(x1667)))+(((cj7)*(x1672)))+(((IkReal(0.0620002000000000))*(x1661)*(x1668))));
evalcond[5]=((IkReal(0.270000000000000))+(((IkReal(0.0620002000000000))*(x1664)*(x1665)))+(((IkReal(0.0620002000000000))*(x1663)*(x1664)))+(((IkReal(0.00845530000000000))*(x1660)))+(((IkReal(-1.00000000000000))*(x1672)*(x1674)))+(((IkReal(-1.00000000000000))*(x1661)*(x1662)))+(((IkReal(-0.0620002000000000))*(x1667)*(x1668)))+(((IkReal(-1.00000000000000))*(cj7)*(x1671)))+(((IkReal(-0.0620002000000000))*(x1667)*(x1673)))+(((x1669)*(x1670)))+(((IkReal(-1.00000000000000))*(x1674)*(x1675)))+(((IkReal(-0.302000000000000))*(x1659))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
IkReal x1676=((r01)*(sj6));
IkReal x1677=((IkReal(1.00000000000000))*(cj11));
IkReal x1678=((IkReal(1.00000000000000))*(r22));
IkReal x1679=((IkReal(1.00000000000000))*(cj6));
IkReal x1680=((IkReal(0.0620002000000000))*(sj11));
IkReal x1681=((cj6)*(r10));
IkReal x1682=((IkReal(0.0620002000000000))*(cj11));
IkReal x1683=((r00)*(sj6));
IkReal x1684=((cj6)*(r11));
IkReal x1685=((IkReal(1.00000000000000))*(sj11));
IkReal x1686=((r12)*(sj6));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j9)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(-1.00000000000000))+(((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1679))));
evalcond[2]=((((cj11)*(x1683)))+(((IkReal(-1.00000000000000))*(x1676)*(x1685)))+(((IkReal(-1.00000000000000))*(x1677)*(x1681)))+(((sj11)*(x1684))));
evalcond[3]=((((cj7)*(x1686)))+(((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(sj7)*(x1678))));
evalcond[4]=((((IkReal(-1.00000000000000))*(x1683)*(x1685)))+(((cj11)*(x1684)))+(((IkReal(-1.00000000000000))*(x1676)*(x1677)))+(((sj11)*(x1681))));
evalcond[5]=((((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1679)))+(((IkReal(-1.00000000000000))*(sj7)*(x1686)))+(((IkReal(-1.00000000000000))*(cj7)*(x1678))));
evalcond[6]=((IkReal(-0.00857763000000000))+(((IkReal(-1.00000000000000))*(py)*(x1679)))+(((x1681)*(x1682)))+(((IkReal(-1.00000000000000))*(x1682)*(x1683)))+(((px)*(sj6)))+(((x1676)*(x1680)))+(((IkReal(-1.00000000000000))*(x1680)*(x1684))));
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 j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1687=((cj7)*(sj11));
IkReal x1688=((r10)*(sj6));
IkReal x1689=((cj6)*(r00));
IkReal x1690=((IkReal(1.00000000000000))*(sj7));
IkReal x1691=((cj11)*(r21));
IkReal x1692=((cj11)*(cj6)*(r01));
IkReal x1693=((cj11)*(r11)*(sj6));
if( IKabs(((((IkReal(-1.00000000000000))*(x1690)*(x1692)))+(((IkReal(-1.00000000000000))*(x1690)*(x1693)))+(((IkReal(-1.00000000000000))*(r20)*(x1687)))+(((IkReal(-1.00000000000000))*(cj7)*(x1691)))+(((IkReal(-1.00000000000000))*(sj11)*(x1689)*(x1690)))+(((IkReal(-1.00000000000000))*(sj11)*(x1688)*(x1690))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(x1690)*(x1691)))+(((x1687)*(x1688)))+(((x1687)*(x1689)))+(((cj7)*(x1692)))+(((cj7)*(x1693)))+(((IkReal(-1.00000000000000))*(r20)*(sj11)*(x1690))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(x1690)*(x1692)))+(((IkReal(-1.00000000000000))*(x1690)*(x1693)))+(((IkReal(-1.00000000000000))*(r20)*(x1687)))+(((IkReal(-1.00000000000000))*(cj7)*(x1691)))+(((IkReal(-1.00000000000000))*(sj11)*(x1689)*(x1690)))+(((IkReal(-1.00000000000000))*(sj11)*(x1688)*(x1690)))))+IKsqr(((((IkReal(-1.00000000000000))*(x1690)*(x1691)))+(((x1687)*(x1688)))+(((x1687)*(x1689)))+(((cj7)*(x1692)))+(((cj7)*(x1693)))+(((IkReal(-1.00000000000000))*(r20)*(sj11)*(x1690)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((((IkReal(-1.00000000000000))*(x1690)*(x1692)))+(((IkReal(-1.00000000000000))*(x1690)*(x1693)))+(((IkReal(-1.00000000000000))*(r20)*(x1687)))+(((IkReal(-1.00000000000000))*(cj7)*(x1691)))+(((IkReal(-1.00000000000000))*(sj11)*(x1689)*(x1690)))+(((IkReal(-1.00000000000000))*(sj11)*(x1688)*(x1690)))), ((((IkReal(-1.00000000000000))*(x1690)*(x1691)))+(((x1687)*(x1688)))+(((x1687)*(x1689)))+(((cj7)*(x1692)))+(((cj7)*(x1693)))+(((IkReal(-1.00000000000000))*(r20)*(sj11)*(x1690)))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[6];
IkReal x1694=IKsin(j8);
IkReal x1695=IKcos(j8);
IkReal x1696=((cj7)*(sj11));
IkReal x1697=((IkReal(0.0620002000000000))*(r21));
IkReal x1698=((r10)*(sj6));
IkReal x1699=((cj11)*(sj7));
IkReal x1700=((cj6)*(r00));
IkReal x1701=((IkReal(1.00000000000000))*(r20));
IkReal x1702=((sj11)*(sj7));
IkReal x1703=((cj6)*(r01));
IkReal x1704=((IkReal(0.0620002000000000))*(r20));
IkReal x1705=((cj11)*(cj7));
IkReal x1706=((IkReal(1.00000000000000))*(pz));
IkReal x1707=((py)*(sj6));
IkReal x1708=((r11)*(sj6));
IkReal x1709=((IkReal(1.00000000000000))*(sj7));
IkReal x1710=((cj6)*(px));
evalcond[0]=((((x1702)*(x1708)))+(((IkReal(-1.00000000000000))*(x1699)*(x1700)))+(((IkReal(-1.00000000000000))*(x1701)*(x1705)))+(((IkReal(-1.00000000000000))*(x1695)))+(((IkReal(-1.00000000000000))*(x1698)*(x1699)))+(((r21)*(x1696)))+(((x1702)*(x1703))));
evalcond[1]=((((x1698)*(x1705)))+(((IkReal(-1.00000000000000))*(x1696)*(x1708)))+(((IkReal(-1.00000000000000))*(x1696)*(x1703)))+(((x1700)*(x1705)))+(x1694)+(((r21)*(x1702)))+(((IkReal(-1.00000000000000))*(x1699)*(x1701))));
evalcond[2]=((((x1699)*(x1708)))+(((x1700)*(x1702)))+(((x1699)*(x1703)))+(((x1698)*(x1702)))+(((r21)*(x1705)))+(x1694)+(((r20)*(x1696))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1696)*(x1698)))+(((r20)*(x1702)))+(((r21)*(x1699)))+(x1695)+(((IkReal(-1.00000000000000))*(x1705)*(x1708)))+(((IkReal(-1.00000000000000))*(x1696)*(x1700)))+(((IkReal(-1.00000000000000))*(x1703)*(x1705))));
evalcond[4]=((((IkReal(-0.0620002000000000))*(x1698)*(x1705)))+(((x1699)*(x1704)))+(((IkReal(-0.0620002000000000))*(x1700)*(x1705)))+(((IkReal(-1.00000000000000))*(x1697)*(x1702)))+(((IkReal(-0.131825300000000))*(x1694)))+(((IkReal(-0.302000000000000))*(x1695)))+(((IkReal(-1.00000000000000))*(sj7)*(x1706)))+(((cj7)*(x1707)))+(((cj7)*(x1710)))+(((IkReal(0.0620002000000000))*(x1696)*(x1703)))+(((IkReal(0.0620002000000000))*(x1696)*(x1708))));
evalcond[5]=((IkReal(0.270000000000000))+(((IkReal(-0.302000000000000))*(x1694)))+(((IkReal(0.131825300000000))*(x1695)))+(((IkReal(-1.00000000000000))*(cj7)*(x1706)))+(((IkReal(-1.00000000000000))*(x1696)*(x1697)))+(((IkReal(-0.0620002000000000))*(x1702)*(x1708)))+(((IkReal(-0.0620002000000000))*(x1702)*(x1703)))+(((IkReal(0.0620002000000000))*(x1698)*(x1699)))+(((IkReal(0.0620002000000000))*(x1699)*(x1700)))+(((x1704)*(x1705)))+(((IkReal(-1.00000000000000))*(x1709)*(x1710)))+(((IkReal(-1.00000000000000))*(x1707)*(x1709))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
if( 1 )
{
continue;
} else
{
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1711=((IkReal(1.00000000000000))*(cj7));
IkReal x1712=((IkReal(1.00000000000000))*(cj6)*(sj7));
IkReal x1713=((IkReal(1.00000000000000))*(sj6)*(sj7));
if( IKabs(((((IkReal(-1.00000000000000))*(r00)*(sj11)*(x1712)))+(((IkReal(-1.00000000000000))*(cj11)*(r01)*(x1712)))+(((IkReal(-1.00000000000000))*(cj11)*(r21)*(x1711)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x1713)))+(((IkReal(-1.00000000000000))*(r20)*(sj11)*(x1711)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x1713))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(cj9) != 0)?((IkReal)1/(cj9)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(r02)*(x1712)))+(((IkReal(-1.00000000000000))*(r22)*(x1711)))+(((IkReal(-1.00000000000000))*(r12)*(x1713))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r00)*(sj11)*(x1712)))+(((IkReal(-1.00000000000000))*(cj11)*(r01)*(x1712)))+(((IkReal(-1.00000000000000))*(cj11)*(r21)*(x1711)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x1713)))+(((IkReal(-1.00000000000000))*(r20)*(sj11)*(x1711)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x1713)))))+IKsqr(((((IKabs(cj9) != 0)?((IkReal)1/(cj9)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(r02)*(x1712)))+(((IkReal(-1.00000000000000))*(r22)*(x1711)))+(((IkReal(-1.00000000000000))*(r12)*(x1713)))))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r00)*(sj11)*(x1712)))+(((IkReal(-1.00000000000000))*(cj11)*(r01)*(x1712)))+(((IkReal(-1.00000000000000))*(cj11)*(r21)*(x1711)))+(((IkReal(-1.00000000000000))*(cj11)*(r11)*(x1713)))+(((IkReal(-1.00000000000000))*(r20)*(sj11)*(x1711)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(x1713)))), ((((IKabs(cj9) != 0)?((IkReal)1/(cj9)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(r02)*(x1712)))+(((IkReal(-1.00000000000000))*(r22)*(x1711)))+(((IkReal(-1.00000000000000))*(r12)*(x1713)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[8];
IkReal x1714=IKcos(j8);
IkReal x1715=IKsin(j8);
IkReal x1716=((cj7)*(sj11));
IkReal x1717=((IkReal(0.0620002000000000))*(r21));
IkReal x1718=((r10)*(sj6));
IkReal x1719=((cj11)*(sj7));
IkReal x1720=((cj6)*(r00));
IkReal x1721=((IkReal(1.00000000000000))*(r20));
IkReal x1722=((sj11)*(sj7));
IkReal x1723=((cj6)*(r01));
IkReal x1724=((IkReal(1.00000000000000))*(cj7));
IkReal x1725=((IkReal(0.0620002000000000))*(r20));
IkReal x1726=((cj11)*(cj7));
IkReal x1727=((IkReal(1.00000000000000))*(sj7));
IkReal x1728=((cj6)*(r02));
IkReal x1729=((py)*(sj6));
IkReal x1730=((r11)*(sj6));
IkReal x1731=((cj6)*(px));
IkReal x1732=((r12)*(sj6));
IkReal x1733=((cj9)*(x1715));
IkReal x1734=((sj9)*(x1714));
IkReal x1735=((cj9)*(x1714));
IkReal x1736=((sj9)*(x1715));
evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x1727)))+(((cj7)*(x1728)))+(x1733)+(((cj7)*(x1732))));
evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(x1724)))+(((IkReal(-1.00000000000000))*(x1735)))+(((IkReal(-1.00000000000000))*(x1727)*(x1732)))+(((IkReal(-1.00000000000000))*(x1727)*(x1728))));
evalcond[2]=((((x1722)*(x1723)))+(((r21)*(x1716)))+(((x1722)*(x1730)))+(((IkReal(-1.00000000000000))*(x1719)*(x1720)))+(((IkReal(-1.00000000000000))*(x1721)*(x1726)))+(x1734)+(((IkReal(-1.00000000000000))*(x1718)*(x1719))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1716)*(x1723)))+(((x1720)*(x1726)))+(((IkReal(-1.00000000000000))*(x1736)))+(((IkReal(-1.00000000000000))*(x1716)*(x1730)))+(((r21)*(x1722)))+(((IkReal(-1.00000000000000))*(x1719)*(x1721)))+(((x1718)*(x1726))));
evalcond[4]=((((r21)*(x1726)))+(((x1718)*(x1722)))+(x1715)+(((x1719)*(x1730)))+(((x1720)*(x1722)))+(((x1719)*(x1723)))+(((r20)*(x1716))));
evalcond[5]=((x1714)+(((r20)*(x1722)))+(((r21)*(x1719)))+(((IkReal(-1.00000000000000))*(x1716)*(x1718)))+(((IkReal(-1.00000000000000))*(cj11)*(x1724)*(x1730)))+(((IkReal(-1.00000000000000))*(cj11)*(x1723)*(x1724)))+(((IkReal(-1.00000000000000))*(x1716)*(x1720))));
evalcond[6]=((((IkReal(-0.0701403000000000))*(x1715)))+(((IkReal(-0.0620002000000000))*(x1720)*(x1726)))+(((IkReal(-0.0620002000000000))*(x1718)*(x1726)))+(((IkReal(0.0616850000000000))*(x1736)))+(((IkReal(-1.00000000000000))*(x1717)*(x1722)))+(((IkReal(-0.302000000000000))*(x1714)))+(((cj7)*(x1731)))+(((x1719)*(x1725)))+(((IkReal(0.0620002000000000))*(x1716)*(x1723)))+(((IkReal(0.0620002000000000))*(x1716)*(x1730)))+(((IkReal(-1.00000000000000))*(pz)*(x1727)))+(((IkReal(-0.00151567000000000))*(x1733)))+(((cj7)*(x1729))));
evalcond[7]=((IkReal(0.270000000000000))+(((IkReal(-0.0620002000000000))*(x1722)*(x1723)))+(((IkReal(-1.00000000000000))*(x1727)*(x1731)))+(((IkReal(0.0620002000000000))*(x1719)*(x1720)))+(((IkReal(0.0701403000000000))*(x1714)))+(((IkReal(-1.00000000000000))*(pz)*(x1724)))+(((x1725)*(x1726)))+(((IkReal(-0.302000000000000))*(x1715)))+(((IkReal(-0.0616850000000000))*(x1734)))+(((IkReal(-1.00000000000000))*(x1716)*(x1717)))+(((IkReal(0.00151567000000000))*(x1735)))+(((IkReal(-1.00000000000000))*(x1727)*(x1729)))+(((IkReal(-0.0620002000000000))*(x1722)*(x1730)))+(((IkReal(0.0620002000000000))*(x1718)*(x1719))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1737=((cj7)*(sj11));
IkReal x1738=((IkReal(1.00000000000000))*(sj7));
IkReal x1739=((cj11)*(cj7));
IkReal x1740=((IkReal(1.00000000000000))*(cj7));
if( IKabs(((((IKabs(cj9) != 0)?((IkReal)1/(cj9)):(IkReal)1.0e30))*(((((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(x1740)))+(((IkReal(-1.00000000000000))*(r12)*(sj6)*(x1740))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj11)*(r21)*(x1738)))+(((r11)*(sj6)*(x1739)))+(((cj6)*(r00)*(x1737)))+(((cj6)*(r01)*(x1739)))+(((IkReal(-1.00000000000000))*(r20)*(sj11)*(x1738)))+(((r10)*(sj6)*(x1737))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(cj9) != 0)?((IkReal)1/(cj9)):(IkReal)1.0e30))*(((((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(x1740)))+(((IkReal(-1.00000000000000))*(r12)*(sj6)*(x1740)))))))+IKsqr(((((IkReal(-1.00000000000000))*(cj11)*(r21)*(x1738)))+(((r11)*(sj6)*(x1739)))+(((cj6)*(r00)*(x1737)))+(((cj6)*(r01)*(x1739)))+(((IkReal(-1.00000000000000))*(r20)*(sj11)*(x1738)))+(((r10)*(sj6)*(x1737)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((((IKabs(cj9) != 0)?((IkReal)1/(cj9)):(IkReal)1.0e30))*(((((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(x1740)))+(((IkReal(-1.00000000000000))*(r12)*(sj6)*(x1740)))))), ((((IkReal(-1.00000000000000))*(cj11)*(r21)*(x1738)))+(((r11)*(sj6)*(x1739)))+(((cj6)*(r00)*(x1737)))+(((cj6)*(r01)*(x1739)))+(((IkReal(-1.00000000000000))*(r20)*(sj11)*(x1738)))+(((r10)*(sj6)*(x1737)))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[8];
IkReal x1741=IKcos(j8);
IkReal x1742=IKsin(j8);
IkReal x1743=((cj7)*(sj11));
IkReal x1744=((IkReal(0.0620002000000000))*(r21));
IkReal x1745=((r10)*(sj6));
IkReal x1746=((cj11)*(sj7));
IkReal x1747=((cj6)*(r00));
IkReal x1748=((IkReal(1.00000000000000))*(r20));
IkReal x1749=((sj11)*(sj7));
IkReal x1750=((cj6)*(r01));
IkReal x1751=((IkReal(1.00000000000000))*(cj7));
IkReal x1752=((IkReal(0.0620002000000000))*(r20));
IkReal x1753=((cj11)*(cj7));
IkReal x1754=((IkReal(1.00000000000000))*(sj7));
IkReal x1755=((cj6)*(r02));
IkReal x1756=((py)*(sj6));
IkReal x1757=((r11)*(sj6));
IkReal x1758=((cj6)*(px));
IkReal x1759=((r12)*(sj6));
IkReal x1760=((cj9)*(x1742));
IkReal x1761=((sj9)*(x1741));
IkReal x1762=((cj9)*(x1741));
IkReal x1763=((sj9)*(x1742));
evalcond[0]=((x1760)+(((IkReal(-1.00000000000000))*(r22)*(x1754)))+(((cj7)*(x1755)))+(((cj7)*(x1759))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x1762)))+(((IkReal(-1.00000000000000))*(r22)*(x1751)))+(((IkReal(-1.00000000000000))*(x1754)*(x1759)))+(((IkReal(-1.00000000000000))*(x1754)*(x1755))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x1746)*(x1747)))+(x1761)+(((r21)*(x1743)))+(((x1749)*(x1750)))+(((x1749)*(x1757)))+(((IkReal(-1.00000000000000))*(x1745)*(x1746)))+(((IkReal(-1.00000000000000))*(x1748)*(x1753))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1746)*(x1748)))+(((IkReal(-1.00000000000000))*(x1743)*(x1750)))+(((x1745)*(x1753)))+(((IkReal(-1.00000000000000))*(x1763)))+(((r21)*(x1749)))+(((x1747)*(x1753)))+(((IkReal(-1.00000000000000))*(x1743)*(x1757))));
evalcond[4]=((((x1745)*(x1749)))+(((x1747)*(x1749)))+(((x1746)*(x1757)))+(x1742)+(((r20)*(x1743)))+(((r21)*(x1753)))+(((x1746)*(x1750))));
evalcond[5]=((((r20)*(x1749)))+(((IkReal(-1.00000000000000))*(cj11)*(x1750)*(x1751)))+(((r21)*(x1746)))+(x1741)+(((IkReal(-1.00000000000000))*(x1743)*(x1747)))+(((IkReal(-1.00000000000000))*(cj11)*(x1751)*(x1757)))+(((IkReal(-1.00000000000000))*(x1743)*(x1745))));
evalcond[6]=((((IkReal(-1.00000000000000))*(pz)*(x1754)))+(((IkReal(-0.00151567000000000))*(x1760)))+(((IkReal(0.0620002000000000))*(x1743)*(x1750)))+(((IkReal(-0.0701403000000000))*(x1742)))+(((IkReal(-0.0620002000000000))*(x1745)*(x1753)))+(((IkReal(-0.0620002000000000))*(x1747)*(x1753)))+(((cj7)*(x1756)))+(((IkReal(0.0620002000000000))*(x1743)*(x1757)))+(((IkReal(-0.302000000000000))*(x1741)))+(((IkReal(0.0616850000000000))*(x1763)))+(((cj7)*(x1758)))+(((x1746)*(x1752)))+(((IkReal(-1.00000000000000))*(x1744)*(x1749))));
evalcond[7]=((IkReal(0.270000000000000))+(((x1752)*(x1753)))+(((IkReal(-0.0620002000000000))*(x1749)*(x1757)))+(((IkReal(-1.00000000000000))*(x1743)*(x1744)))+(((IkReal(-1.00000000000000))*(pz)*(x1751)))+(((IkReal(0.0620002000000000))*(x1745)*(x1746)))+(((IkReal(0.00151567000000000))*(x1762)))+(((IkReal(-0.302000000000000))*(x1742)))+(((IkReal(0.0701403000000000))*(x1741)))+(((IkReal(0.0620002000000000))*(x1746)*(x1747)))+(((IkReal(-1.00000000000000))*(x1754)*(x1758)))+(((IkReal(-1.00000000000000))*(x1754)*(x1756)))+(((IkReal(-0.0620002000000000))*(x1749)*(x1750)))+(((IkReal(-0.0616850000000000))*(x1761))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1764=((IkReal(1.00000000000000))*(sj7));
IkReal x1765=((cj6)*(r02));
IkReal x1766=((IkReal(1.00000000000000))*(cj7));
IkReal x1767=((r12)*(sj6));
if( IKabs(((gconst14)*(((((IkReal(-1.00000000000000))*(x1766)*(x1767)))+(((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x1765)*(x1766))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst14)*(((((IkReal(-1.00000000000000))*(x1764)*(x1767)))+(((IkReal(-1.00000000000000))*(x1764)*(x1765)))+(((IkReal(-1.00000000000000))*(r22)*(x1766))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j8array[0]=IKatan2(((gconst14)*(((((IkReal(-1.00000000000000))*(x1766)*(x1767)))+(((r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x1765)*(x1766)))))), ((gconst14)*(((((IkReal(-1.00000000000000))*(x1764)*(x1767)))+(((IkReal(-1.00000000000000))*(x1764)*(x1765)))+(((IkReal(-1.00000000000000))*(r22)*(x1766)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[8];
IkReal x1768=IKcos(j8);
IkReal x1769=IKsin(j8);
IkReal x1770=((cj7)*(sj11));
IkReal x1771=((IkReal(0.0620002000000000))*(r21));
IkReal x1772=((r10)*(sj6));
IkReal x1773=((cj11)*(sj7));
IkReal x1774=((cj6)*(r00));
IkReal x1775=((IkReal(1.00000000000000))*(r20));
IkReal x1776=((sj11)*(sj7));
IkReal x1777=((cj6)*(r01));
IkReal x1778=((IkReal(1.00000000000000))*(cj7));
IkReal x1779=((IkReal(0.0620002000000000))*(r20));
IkReal x1780=((cj11)*(cj7));
IkReal x1781=((IkReal(1.00000000000000))*(sj7));
IkReal x1782=((cj6)*(r02));
IkReal x1783=((py)*(sj6));
IkReal x1784=((r11)*(sj6));
IkReal x1785=((cj6)*(px));
IkReal x1786=((r12)*(sj6));
IkReal x1787=((cj9)*(x1769));
IkReal x1788=((sj9)*(x1768));
IkReal x1789=((cj9)*(x1768));
IkReal x1790=((sj9)*(x1769));
evalcond[0]=((x1787)+(((cj7)*(x1786)))+(((cj7)*(x1782)))+(((IkReal(-1.00000000000000))*(r22)*(x1781))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x1781)*(x1786)))+(((IkReal(-1.00000000000000))*(x1781)*(x1782)))+(((IkReal(-1.00000000000000))*(r22)*(x1778)))+(((IkReal(-1.00000000000000))*(x1789))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x1775)*(x1780)))+(((x1776)*(x1777)))+(x1788)+(((x1776)*(x1784)))+(((r21)*(x1770)))+(((IkReal(-1.00000000000000))*(x1773)*(x1774)))+(((IkReal(-1.00000000000000))*(x1772)*(x1773))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1790)))+(((IkReal(-1.00000000000000))*(x1770)*(x1784)))+(((r21)*(x1776)))+(((IkReal(-1.00000000000000))*(x1770)*(x1777)))+(((x1772)*(x1780)))+(((x1774)*(x1780)))+(((IkReal(-1.00000000000000))*(x1773)*(x1775))));
evalcond[4]=((((r20)*(x1770)))+(((x1773)*(x1784)))+(x1769)+(((x1773)*(x1777)))+(((r21)*(x1780)))+(((x1774)*(x1776)))+(((x1772)*(x1776))));
evalcond[5]=((((IkReal(-1.00000000000000))*(x1770)*(x1774)))+(((IkReal(-1.00000000000000))*(cj11)*(x1777)*(x1778)))+(x1768)+(((IkReal(-1.00000000000000))*(cj11)*(x1778)*(x1784)))+(((r21)*(x1773)))+(((IkReal(-1.00000000000000))*(x1770)*(x1772)))+(((r20)*(x1776))));
evalcond[6]=((((cj7)*(x1783)))+(((IkReal(0.0616850000000000))*(x1790)))+(((x1773)*(x1779)))+(((IkReal(-0.0620002000000000))*(x1774)*(x1780)))+(((IkReal(0.0620002000000000))*(x1770)*(x1784)))+(((IkReal(-0.302000000000000))*(x1768)))+(((IkReal(-0.00151567000000000))*(x1787)))+(((IkReal(-1.00000000000000))*(x1771)*(x1776)))+(((cj7)*(x1785)))+(((IkReal(-0.0701403000000000))*(x1769)))+(((IkReal(-0.0620002000000000))*(x1772)*(x1780)))+(((IkReal(0.0620002000000000))*(x1770)*(x1777)))+(((IkReal(-1.00000000000000))*(pz)*(x1781))));
evalcond[7]=((IkReal(0.270000000000000))+(((IkReal(-0.302000000000000))*(x1769)))+(((IkReal(-0.0620002000000000))*(x1776)*(x1784)))+(((IkReal(-0.0620002000000000))*(x1776)*(x1777)))+(((IkReal(-1.00000000000000))*(pz)*(x1778)))+(((IkReal(-1.00000000000000))*(x1770)*(x1771)))+(((IkReal(-1.00000000000000))*(x1781)*(x1785)))+(((IkReal(0.00151567000000000))*(x1789)))+(((x1779)*(x1780)))+(((IkReal(0.0620002000000000))*(x1772)*(x1773)))+(((IkReal(-1.00000000000000))*(x1781)*(x1783)))+(((IkReal(-0.0616850000000000))*(x1788)))+(((IkReal(0.0620002000000000))*(x1773)*(x1774)))+(((IkReal(0.0701403000000000))*(x1768))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
IkReal x1791=((cj6)*(r10));
IkReal x1792=((IkReal(1.00000000000000))*(cj6));
IkReal x1793=((cj6)*(r11));
IkReal x1794=((r01)*(sj6));
IkReal x1795=((IkReal(0.0620002000000000))*(sj11));
IkReal x1796=((IkReal(0.0620002000000000))*(cj11));
IkReal x1797=((IkReal(1.00000000000000))*(cj11));
IkReal x1798=((r00)*(sj6));
IkReal x1799=((IkReal(1.00000000000000))*(sj11));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j10)), IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(sj9)))+(((IkReal(-1.00000000000000))*(r12)*(x1792))));
evalcond[2]=((((sj11)*(x1793)))+(cj9)+(((cj11)*(x1798)))+(((IkReal(-1.00000000000000))*(x1791)*(x1797)))+(((IkReal(-1.00000000000000))*(x1794)*(x1799))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1798)*(x1799)))+(((IkReal(-1.00000000000000))*(x1794)*(x1797)))+(((cj11)*(x1793)))+(((sj11)*(x1791))));
evalcond[4]=((IkReal(-0.0100933000000000))+(((IkReal(-0.00151567000000000))*(sj9)))+(((IkReal(-0.0616850000000000))*(cj9)))+(((IkReal(-1.00000000000000))*(py)*(x1792)))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(x1793)*(x1795)))+(((x1794)*(x1795)))+(((IkReal(-1.00000000000000))*(x1796)*(x1798)))+(((x1791)*(x1796))));
if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
IkReal gconst15;
gconst15=IKsign(cj9);
dummyeval[0]=cj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
dummyeval[0]=cj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
dummyeval[0]=cj9;
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal evalcond[7];
IkReal x1800=((r01)*(sj6));
IkReal x1801=((IkReal(1.00000000000000))*(cj11));
IkReal x1802=((IkReal(1.00000000000000))*(r22));
IkReal x1803=((IkReal(1.00000000000000))*(cj6));
IkReal x1804=((IkReal(0.0620002000000000))*(sj11));
IkReal x1805=((cj6)*(r10));
IkReal x1806=((IkReal(0.0620002000000000))*(cj11));
IkReal x1807=((r00)*(sj6));
IkReal x1808=((cj6)*(r11));
IkReal x1809=((IkReal(1.00000000000000))*(sj11));
IkReal x1810=((r12)*(sj6));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j9)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r12)*(x1803)))+(((r02)*(sj6))));
evalcond[2]=((((cj11)*(x1807)))+(((IkReal(-1.00000000000000))*(x1800)*(x1809)))+(((IkReal(-1.00000000000000))*(x1801)*(x1805)))+(((sj11)*(x1808))));
evalcond[3]=((((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(sj7)*(x1802)))+(((cj7)*(x1810))));
evalcond[4]=((((sj11)*(x1805)))+(((IkReal(-1.00000000000000))*(x1807)*(x1809)))+(((IkReal(-1.00000000000000))*(x1800)*(x1801)))+(((cj11)*(x1808))));
evalcond[5]=((((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1803)))+(((IkReal(-1.00000000000000))*(cj7)*(x1802)))+(((IkReal(-1.00000000000000))*(sj7)*(x1810))));
evalcond[6]=((IkReal(-0.0116089700000000))+(((IkReal(-1.00000000000000))*(py)*(x1803)))+(((x1805)*(x1806)))+(((IkReal(-1.00000000000000))*(x1806)*(x1807)))+(((px)*(sj6)))+(((x1800)*(x1804)))+(((IkReal(-1.00000000000000))*(x1804)*(x1808))));
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 j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1811=((IkReal(1.00000000000000))*(sj11));
IkReal x1812=((sj6)*(sj7));
IkReal x1813=((cj7)*(r21));
IkReal x1814=((cj7)*(r20));
IkReal x1815=((cj6)*(sj7));
if( IKabs(((((r10)*(sj11)*(x1812)))+(((cj11)*(r01)*(x1815)))+(((r00)*(sj11)*(x1815)))+(((sj11)*(x1814)))+(((cj11)*(x1813)))+(((cj11)*(r11)*(x1812))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj11)*(x1814)))+(((IkReal(-1.00000000000000))*(r11)*(x1811)*(x1812)))+(((IkReal(-1.00000000000000))*(x1811)*(x1813)))+(((cj11)*(r10)*(x1812)))+(((cj11)*(r00)*(x1815)))+(((IkReal(-1.00000000000000))*(r01)*(x1811)*(x1815))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((r10)*(sj11)*(x1812)))+(((cj11)*(r01)*(x1815)))+(((r00)*(sj11)*(x1815)))+(((sj11)*(x1814)))+(((cj11)*(x1813)))+(((cj11)*(r11)*(x1812)))))+IKsqr(((((cj11)*(x1814)))+(((IkReal(-1.00000000000000))*(r11)*(x1811)*(x1812)))+(((IkReal(-1.00000000000000))*(x1811)*(x1813)))+(((cj11)*(r10)*(x1812)))+(((cj11)*(r00)*(x1815)))+(((IkReal(-1.00000000000000))*(r01)*(x1811)*(x1815)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((((r10)*(sj11)*(x1812)))+(((cj11)*(r01)*(x1815)))+(((r00)*(sj11)*(x1815)))+(((sj11)*(x1814)))+(((cj11)*(x1813)))+(((cj11)*(r11)*(x1812)))), ((((cj11)*(x1814)))+(((IkReal(-1.00000000000000))*(r11)*(x1811)*(x1812)))+(((IkReal(-1.00000000000000))*(x1811)*(x1813)))+(((cj11)*(r10)*(x1812)))+(((cj11)*(r00)*(x1815)))+(((IkReal(-1.00000000000000))*(r01)*(x1811)*(x1815)))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[6];
IkReal x1816=IKcos(j8);
IkReal x1817=IKsin(j8);
IkReal x1818=((cj7)*(sj11));
IkReal x1819=((IkReal(0.0620002000000000))*(r21));
IkReal x1820=((r10)*(sj6));
IkReal x1821=((cj11)*(sj7));
IkReal x1822=((cj6)*(r00));
IkReal x1823=((IkReal(1.00000000000000))*(r20));
IkReal x1824=((sj11)*(sj7));
IkReal x1825=((cj6)*(r01));
IkReal x1826=((IkReal(0.0620002000000000))*(r20));
IkReal x1827=((cj11)*(cj7));
IkReal x1828=((IkReal(1.00000000000000))*(pz));
IkReal x1829=((py)*(sj6));
IkReal x1830=((r11)*(sj6));
IkReal x1831=((IkReal(1.00000000000000))*(sj7));
IkReal x1832=((cj6)*(px));
IkReal x1833=((IkReal(1.00000000000000))*(x1817));
evalcond[0]=((((x1824)*(x1830)))+(((IkReal(-1.00000000000000))*(x1823)*(x1827)))+(x1816)+(((x1824)*(x1825)))+(((IkReal(-1.00000000000000))*(x1821)*(x1822)))+(((IkReal(-1.00000000000000))*(x1820)*(x1821)))+(((r21)*(x1818))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x1833)))+(((IkReal(-1.00000000000000))*(x1818)*(x1830)))+(((x1822)*(x1827)))+(((IkReal(-1.00000000000000))*(x1821)*(x1823)))+(((IkReal(-1.00000000000000))*(x1818)*(x1825)))+(((r21)*(x1824)))+(((x1820)*(x1827))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x1833)))+(((x1821)*(x1830)))+(((x1821)*(x1825)))+(((x1822)*(x1824)))+(((r20)*(x1818)))+(((x1820)*(x1824)))+(((r21)*(x1827))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1816)))+(((IkReal(-1.00000000000000))*(x1825)*(x1827)))+(((IkReal(-1.00000000000000))*(x1818)*(x1822)))+(((r21)*(x1821)))+(((IkReal(-1.00000000000000))*(x1818)*(x1820)))+(((IkReal(-1.00000000000000))*(x1827)*(x1830)))+(((r20)*(x1824))));
evalcond[4]=((((IkReal(-0.00845530000000000))*(x1817)))+(((IkReal(0.0620002000000000))*(x1818)*(x1825)))+(((cj7)*(x1829)))+(((IkReal(-0.0620002000000000))*(x1820)*(x1827)))+(((cj7)*(x1832)))+(((IkReal(-1.00000000000000))*(x1819)*(x1824)))+(((IkReal(-0.0620002000000000))*(x1822)*(x1827)))+(((IkReal(-1.00000000000000))*(sj7)*(x1828)))+(((IkReal(-0.302000000000000))*(x1816)))+(((x1821)*(x1826)))+(((IkReal(0.0620002000000000))*(x1818)*(x1830))));
evalcond[5]=((IkReal(0.270000000000000))+(((IkReal(-1.00000000000000))*(cj7)*(x1828)))+(((IkReal(-1.00000000000000))*(x1831)*(x1832)))+(((IkReal(-1.00000000000000))*(x1818)*(x1819)))+(((IkReal(-0.0620002000000000))*(x1824)*(x1825)))+(((IkReal(-1.00000000000000))*(x1829)*(x1831)))+(((IkReal(-0.302000000000000))*(x1817)))+(((IkReal(0.0620002000000000))*(x1820)*(x1821)))+(((IkReal(-0.0620002000000000))*(x1824)*(x1830)))+(((IkReal(0.0620002000000000))*(x1821)*(x1822)))+(((IkReal(0.00845530000000000))*(x1816)))+(((x1826)*(x1827))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
IkReal x1834=((r01)*(sj6));
IkReal x1835=((IkReal(1.00000000000000))*(cj11));
IkReal x1836=((IkReal(1.00000000000000))*(r22));
IkReal x1837=((IkReal(1.00000000000000))*(cj6));
IkReal x1838=((IkReal(0.0620002000000000))*(sj11));
IkReal x1839=((cj6)*(r10));
IkReal x1840=((IkReal(0.0620002000000000))*(cj11));
IkReal x1841=((r00)*(sj6));
IkReal x1842=((cj6)*(r11));
IkReal x1843=((IkReal(1.00000000000000))*(sj11));
IkReal x1844=((r12)*(sj6));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j9)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(1.00000000000000))+(((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1837))));
evalcond[2]=((((cj11)*(x1841)))+(((sj11)*(x1842)))+(((IkReal(-1.00000000000000))*(x1835)*(x1839)))+(((IkReal(-1.00000000000000))*(x1834)*(x1843))));
evalcond[3]=((((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(sj7)*(x1836)))+(((cj7)*(x1844))));
evalcond[4]=((((cj11)*(x1842)))+(((IkReal(-1.00000000000000))*(x1841)*(x1843)))+(((IkReal(-1.00000000000000))*(x1834)*(x1835)))+(((sj11)*(x1839))));
evalcond[5]=((((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1837)))+(((IkReal(-1.00000000000000))*(cj7)*(x1836)))+(((IkReal(-1.00000000000000))*(sj7)*(x1844))));
evalcond[6]=((IkReal(-0.00857763000000000))+(((x1834)*(x1838)))+(((IkReal(-1.00000000000000))*(x1840)*(x1841)))+(((x1839)*(x1840)))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(py)*(x1837)))+(((IkReal(-1.00000000000000))*(x1838)*(x1842))));
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 j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1845=((cj7)*(r20));
IkReal x1846=((cj7)*(r21));
IkReal x1847=((IkReal(1.00000000000000))*(cj11));
IkReal x1848=((sj11)*(sj7));
IkReal x1849=((cj6)*(r01));
IkReal x1850=((r11)*(sj6));
IkReal x1851=((cj11)*(sj7));
IkReal x1852=((cj6)*(r00));
IkReal x1853=((r10)*(sj6)*(sj7));
if( IKabs(((((x1850)*(x1851)))+(((x1849)*(x1851)))+(((sj11)*(x1845)))+(((cj11)*(x1846)))+(((r10)*(sj6)*(x1848)))+(((x1848)*(x1852))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((x1848)*(x1850)))+(((IkReal(-1.00000000000000))*(sj7)*(x1847)*(x1852)))+(((IkReal(-1.00000000000000))*(x1847)*(x1853)))+(((sj11)*(x1846)))+(((IkReal(-1.00000000000000))*(x1845)*(x1847)))+(((x1848)*(x1849))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((x1850)*(x1851)))+(((x1849)*(x1851)))+(((sj11)*(x1845)))+(((cj11)*(x1846)))+(((r10)*(sj6)*(x1848)))+(((x1848)*(x1852)))))+IKsqr(((((x1848)*(x1850)))+(((IkReal(-1.00000000000000))*(sj7)*(x1847)*(x1852)))+(((IkReal(-1.00000000000000))*(x1847)*(x1853)))+(((sj11)*(x1846)))+(((IkReal(-1.00000000000000))*(x1845)*(x1847)))+(((x1848)*(x1849)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((((x1850)*(x1851)))+(((x1849)*(x1851)))+(((sj11)*(x1845)))+(((cj11)*(x1846)))+(((r10)*(sj6)*(x1848)))+(((x1848)*(x1852)))), ((((x1848)*(x1850)))+(((IkReal(-1.00000000000000))*(sj7)*(x1847)*(x1852)))+(((IkReal(-1.00000000000000))*(x1847)*(x1853)))+(((sj11)*(x1846)))+(((IkReal(-1.00000000000000))*(x1845)*(x1847)))+(((x1848)*(x1849)))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[6];
IkReal x1854=IKsin(j8);
IkReal x1855=IKcos(j8);
IkReal x1856=((cj7)*(sj11));
IkReal x1857=((IkReal(0.0620002000000000))*(r21));
IkReal x1858=((r10)*(sj6));
IkReal x1859=((cj11)*(sj7));
IkReal x1860=((cj6)*(r00));
IkReal x1861=((IkReal(1.00000000000000))*(r20));
IkReal x1862=((sj11)*(sj7));
IkReal x1863=((cj6)*(r01));
IkReal x1864=((IkReal(0.0620002000000000))*(r20));
IkReal x1865=((cj11)*(cj7));
IkReal x1866=((IkReal(1.00000000000000))*(pz));
IkReal x1867=((py)*(sj6));
IkReal x1868=((r11)*(sj6));
IkReal x1869=((IkReal(1.00000000000000))*(sj7));
IkReal x1870=((cj6)*(px));
IkReal x1871=((IkReal(1.00000000000000))*(x1855));
evalcond[0]=((((IkReal(-1.00000000000000))*(x1861)*(x1865)))+(((IkReal(-1.00000000000000))*(x1858)*(x1859)))+(((r21)*(x1856)))+(((x1862)*(x1868)))+(((IkReal(-1.00000000000000))*(x1871)))+(((IkReal(-1.00000000000000))*(x1859)*(x1860)))+(((x1862)*(x1863))));
evalcond[1]=((((x1860)*(x1865)))+(((IkReal(-1.00000000000000))*(x1856)*(x1863)))+(x1854)+(((x1858)*(x1865)))+(((r21)*(x1862)))+(((IkReal(-1.00000000000000))*(x1859)*(x1861)))+(((IkReal(-1.00000000000000))*(x1856)*(x1868))));
evalcond[2]=((((x1858)*(x1862)))+(((x1860)*(x1862)))+(((r21)*(x1865)))+(((x1859)*(x1863)))+(((r20)*(x1856)))+(((x1859)*(x1868)))+(((IkReal(-1.00000000000000))*(x1854))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1865)*(x1868)))+(((IkReal(-1.00000000000000))*(x1863)*(x1865)))+(((IkReal(-1.00000000000000))*(x1856)*(x1858)))+(((IkReal(-1.00000000000000))*(x1856)*(x1860)))+(((r20)*(x1862)))+(((IkReal(-1.00000000000000))*(x1871)))+(((r21)*(x1859))));
evalcond[4]=((((IkReal(-1.00000000000000))*(sj7)*(x1866)))+(((IkReal(-0.302000000000000))*(x1855)))+(((IkReal(-0.0620002000000000))*(x1860)*(x1865)))+(((cj7)*(x1867)))+(((IkReal(-0.0620002000000000))*(x1858)*(x1865)))+(((IkReal(-0.131825300000000))*(x1854)))+(((IkReal(-1.00000000000000))*(x1857)*(x1862)))+(((cj7)*(x1870)))+(((IkReal(0.0620002000000000))*(x1856)*(x1868)))+(((IkReal(0.0620002000000000))*(x1856)*(x1863)))+(((x1859)*(x1864))));
evalcond[5]=((IkReal(0.270000000000000))+(((x1864)*(x1865)))+(((IkReal(0.0620002000000000))*(x1858)*(x1859)))+(((IkReal(-0.0620002000000000))*(x1862)*(x1863)))+(((IkReal(0.131825300000000))*(x1855)))+(((IkReal(-0.302000000000000))*(x1854)))+(((IkReal(-1.00000000000000))*(x1869)*(x1870)))+(((IkReal(-0.0620002000000000))*(x1862)*(x1868)))+(((IkReal(-1.00000000000000))*(x1856)*(x1857)))+(((IkReal(0.0620002000000000))*(x1859)*(x1860)))+(((IkReal(-1.00000000000000))*(cj7)*(x1866)))+(((IkReal(-1.00000000000000))*(x1867)*(x1869))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
if( 1 )
{
continue;
} else
{
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1872=((IkReal(1.00000000000000))*(cj7));
if( IKabs(((((IKabs(cj9) != 0)?((IkReal)1/(cj9)):(IkReal)1.0e30))*(((((cj7)*(r12)*(sj6)))+(((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(r22)*(sj7))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj11)*(r11)*(sj6)*(x1872)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(sj6)*(x1872)))+(((cj11)*(r21)*(sj7)))+(((r20)*(sj11)*(sj7)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj11)*(x1872)))+(((IkReal(-1.00000000000000))*(cj11)*(cj6)*(r01)*(x1872))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(cj9) != 0)?((IkReal)1/(cj9)):(IkReal)1.0e30))*(((((cj7)*(r12)*(sj6)))+(((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))))))+IKsqr(((((IkReal(-1.00000000000000))*(cj11)*(r11)*(sj6)*(x1872)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(sj6)*(x1872)))+(((cj11)*(r21)*(sj7)))+(((r20)*(sj11)*(sj7)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj11)*(x1872)))+(((IkReal(-1.00000000000000))*(cj11)*(cj6)*(r01)*(x1872)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((((IKabs(cj9) != 0)?((IkReal)1/(cj9)):(IkReal)1.0e30))*(((((cj7)*(r12)*(sj6)))+(((cj6)*(cj7)*(r02)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))))), ((((IkReal(-1.00000000000000))*(cj11)*(r11)*(sj6)*(x1872)))+(((IkReal(-1.00000000000000))*(r10)*(sj11)*(sj6)*(x1872)))+(((cj11)*(r21)*(sj7)))+(((r20)*(sj11)*(sj7)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(sj11)*(x1872)))+(((IkReal(-1.00000000000000))*(cj11)*(cj6)*(r01)*(x1872)))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[8];
IkReal x1873=IKcos(j8);
IkReal x1874=IKsin(j8);
IkReal x1875=((cj7)*(sj11));
IkReal x1876=((IkReal(0.0620002000000000))*(r21));
IkReal x1877=((r10)*(sj6));
IkReal x1878=((cj11)*(sj7));
IkReal x1879=((cj6)*(r00));
IkReal x1880=((IkReal(1.00000000000000))*(r20));
IkReal x1881=((sj11)*(sj7));
IkReal x1882=((cj6)*(r01));
IkReal x1883=((IkReal(1.00000000000000))*(cj7));
IkReal x1884=((IkReal(0.0620002000000000))*(r20));
IkReal x1885=((cj11)*(cj7));
IkReal x1886=((IkReal(1.00000000000000))*(sj7));
IkReal x1887=((cj6)*(r02));
IkReal x1888=((py)*(sj6));
IkReal x1889=((r11)*(sj6));
IkReal x1890=((cj6)*(px));
IkReal x1891=((r12)*(sj6));
IkReal x1892=((IkReal(1.00000000000000))*(x1874));
IkReal x1893=((sj9)*(x1873));
IkReal x1894=((cj9)*(x1873));
evalcond[0]=((((IkReal(-1.00000000000000))*(cj9)*(x1892)))+(((cj7)*(x1891)))+(((cj7)*(x1887)))+(((IkReal(-1.00000000000000))*(r22)*(x1886))));
evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(x1883)))+(((IkReal(-1.00000000000000))*(x1886)*(x1891)))+(((IkReal(-1.00000000000000))*(x1886)*(x1887)))+(x1894));
evalcond[2]=((((IkReal(-1.00000000000000))*(x1877)*(x1878)))+(((IkReal(-1.00000000000000))*(x1878)*(x1879)))+(((x1881)*(x1882)))+(((r21)*(x1875)))+(((IkReal(-1.00000000000000))*(x1880)*(x1885)))+(x1893)+(((x1881)*(x1889))));
evalcond[3]=((((r21)*(x1881)))+(((IkReal(-1.00000000000000))*(x1875)*(x1889)))+(((IkReal(-1.00000000000000))*(sj9)*(x1892)))+(((x1877)*(x1885)))+(((IkReal(-1.00000000000000))*(x1875)*(x1882)))+(((x1879)*(x1885)))+(((IkReal(-1.00000000000000))*(x1878)*(x1880))));
evalcond[4]=((((x1879)*(x1881)))+(((IkReal(-1.00000000000000))*(x1892)))+(((r20)*(x1875)))+(((r21)*(x1885)))+(((x1878)*(x1889)))+(((x1877)*(x1881)))+(((x1878)*(x1882))));
evalcond[5]=((((r20)*(x1881)))+(((IkReal(-1.00000000000000))*(x1875)*(x1879)))+(((IkReal(-1.00000000000000))*(x1873)))+(((IkReal(-1.00000000000000))*(cj11)*(x1882)*(x1883)))+(((IkReal(-1.00000000000000))*(x1875)*(x1877)))+(((r21)*(x1878)))+(((IkReal(-1.00000000000000))*(cj11)*(x1883)*(x1889))));
evalcond[6]=((((IkReal(-1.00000000000000))*(x1876)*(x1881)))+(((IkReal(-0.00151567000000000))*(cj9)*(x1874)))+(((IkReal(0.0616850000000000))*(sj9)*(x1874)))+(((IkReal(-0.302000000000000))*(x1873)))+(((IkReal(-0.0620002000000000))*(x1879)*(x1885)))+(((cj7)*(x1888)))+(((IkReal(0.0620002000000000))*(x1875)*(x1882)))+(((x1878)*(x1884)))+(((IkReal(-0.0701403000000000))*(x1874)))+(((IkReal(-0.0620002000000000))*(x1877)*(x1885)))+(((cj7)*(x1890)))+(((IkReal(-1.00000000000000))*(pz)*(x1886)))+(((IkReal(0.0620002000000000))*(x1875)*(x1889))));
evalcond[7]=((IkReal(0.270000000000000))+(((IkReal(-0.0620002000000000))*(x1881)*(x1889)))+(((IkReal(0.0701403000000000))*(x1873)))+(((IkReal(-1.00000000000000))*(pz)*(x1883)))+(((x1884)*(x1885)))+(((IkReal(-0.302000000000000))*(x1874)))+(((IkReal(-1.00000000000000))*(x1886)*(x1890)))+(((IkReal(-1.00000000000000))*(x1886)*(x1888)))+(((IkReal(-0.0620002000000000))*(x1881)*(x1882)))+(((IkReal(0.00151567000000000))*(x1894)))+(((IkReal(0.0620002000000000))*(x1878)*(x1879)))+(((IkReal(-0.0616850000000000))*(x1893)))+(((IkReal(0.0620002000000000))*(x1877)*(x1878)))+(((IkReal(-1.00000000000000))*(x1875)*(x1876))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1895=((sj6)*(sj7));
IkReal x1896=((cj6)*(sj7));
if( IKabs(((((cj11)*(cj7)*(r21)))+(((r00)*(sj11)*(x1896)))+(((cj11)*(r11)*(x1895)))+(((cj7)*(r20)*(sj11)))+(((r10)*(sj11)*(x1895)))+(((cj11)*(r01)*(x1896))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(cj9) != 0)?((IkReal)1/(cj9)):(IkReal)1.0e30))*(((((cj7)*(r22)))+(((r02)*(x1896)))+(((r12)*(x1895))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj11)*(cj7)*(r21)))+(((r00)*(sj11)*(x1896)))+(((cj11)*(r11)*(x1895)))+(((cj7)*(r20)*(sj11)))+(((r10)*(sj11)*(x1895)))+(((cj11)*(r01)*(x1896)))))+IKsqr(((((IKabs(cj9) != 0)?((IkReal)1/(cj9)):(IkReal)1.0e30))*(((((cj7)*(r22)))+(((r02)*(x1896)))+(((r12)*(x1895)))))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((((cj11)*(cj7)*(r21)))+(((r00)*(sj11)*(x1896)))+(((cj11)*(r11)*(x1895)))+(((cj7)*(r20)*(sj11)))+(((r10)*(sj11)*(x1895)))+(((cj11)*(r01)*(x1896)))), ((((IKabs(cj9) != 0)?((IkReal)1/(cj9)):(IkReal)1.0e30))*(((((cj7)*(r22)))+(((r02)*(x1896)))+(((r12)*(x1895)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[8];
IkReal x1897=IKcos(j8);
IkReal x1898=IKsin(j8);
IkReal x1899=((cj7)*(sj11));
IkReal x1900=((IkReal(0.0620002000000000))*(r21));
IkReal x1901=((r10)*(sj6));
IkReal x1902=((cj11)*(sj7));
IkReal x1903=((cj6)*(r00));
IkReal x1904=((IkReal(1.00000000000000))*(r20));
IkReal x1905=((sj11)*(sj7));
IkReal x1906=((cj6)*(r01));
IkReal x1907=((IkReal(1.00000000000000))*(cj7));
IkReal x1908=((IkReal(0.0620002000000000))*(r20));
IkReal x1909=((cj11)*(cj7));
IkReal x1910=((IkReal(1.00000000000000))*(sj7));
IkReal x1911=((cj6)*(r02));
IkReal x1912=((py)*(sj6));
IkReal x1913=((r11)*(sj6));
IkReal x1914=((cj6)*(px));
IkReal x1915=((r12)*(sj6));
IkReal x1916=((IkReal(1.00000000000000))*(x1898));
IkReal x1917=((sj9)*(x1897));
IkReal x1918=((cj9)*(x1897));
evalcond[0]=((((cj7)*(x1911)))+(((IkReal(-1.00000000000000))*(cj9)*(x1916)))+(((cj7)*(x1915)))+(((IkReal(-1.00000000000000))*(r22)*(x1910))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x1910)*(x1915)))+(((IkReal(-1.00000000000000))*(x1910)*(x1911)))+(((IkReal(-1.00000000000000))*(r22)*(x1907)))+(x1918));
evalcond[2]=((((x1905)*(x1906)))+(((r21)*(x1899)))+(x1917)+(((IkReal(-1.00000000000000))*(x1904)*(x1909)))+(((x1905)*(x1913)))+(((IkReal(-1.00000000000000))*(x1902)*(x1903)))+(((IkReal(-1.00000000000000))*(x1901)*(x1902))));
evalcond[3]=((((x1903)*(x1909)))+(((IkReal(-1.00000000000000))*(sj9)*(x1916)))+(((IkReal(-1.00000000000000))*(x1899)*(x1913)))+(((IkReal(-1.00000000000000))*(x1902)*(x1904)))+(((r21)*(x1905)))+(((IkReal(-1.00000000000000))*(x1899)*(x1906)))+(((x1901)*(x1909))));
evalcond[4]=((((x1902)*(x1906)))+(((x1903)*(x1905)))+(((r20)*(x1899)))+(((x1901)*(x1905)))+(((IkReal(-1.00000000000000))*(x1916)))+(((r21)*(x1909)))+(((x1902)*(x1913))));
evalcond[5]=((((IkReal(-1.00000000000000))*(cj11)*(x1906)*(x1907)))+(((IkReal(-1.00000000000000))*(x1899)*(x1901)))+(((r21)*(x1902)))+(((IkReal(-1.00000000000000))*(x1897)))+(((IkReal(-1.00000000000000))*(x1899)*(x1903)))+(((IkReal(-1.00000000000000))*(cj11)*(x1907)*(x1913)))+(((r20)*(x1905))));
evalcond[6]=((((IkReal(-0.00151567000000000))*(cj9)*(x1898)))+(((IkReal(0.0616850000000000))*(sj9)*(x1898)))+(((cj7)*(x1912)))+(((IkReal(-1.00000000000000))*(pz)*(x1910)))+(((IkReal(0.0620002000000000))*(x1899)*(x1913)))+(((IkReal(-0.0620002000000000))*(x1903)*(x1909)))+(((IkReal(0.0620002000000000))*(x1899)*(x1906)))+(((IkReal(-0.302000000000000))*(x1897)))+(((cj7)*(x1914)))+(((x1902)*(x1908)))+(((IkReal(-0.0701403000000000))*(x1898)))+(((IkReal(-0.0620002000000000))*(x1901)*(x1909)))+(((IkReal(-1.00000000000000))*(x1900)*(x1905))));
evalcond[7]=((IkReal(0.270000000000000))+(((IkReal(-0.0620002000000000))*(x1905)*(x1906)))+(((IkReal(0.00151567000000000))*(x1918)))+(((IkReal(-1.00000000000000))*(pz)*(x1907)))+(((IkReal(-0.0620002000000000))*(x1905)*(x1913)))+(((IkReal(0.0620002000000000))*(x1901)*(x1902)))+(((x1908)*(x1909)))+(((IkReal(-1.00000000000000))*(x1899)*(x1900)))+(((IkReal(0.0620002000000000))*(x1902)*(x1903)))+(((IkReal(-0.0616850000000000))*(x1917)))+(((IkReal(-0.302000000000000))*(x1898)))+(((IkReal(-1.00000000000000))*(x1910)*(x1914)))+(((IkReal(-1.00000000000000))*(x1910)*(x1912)))+(((IkReal(0.0701403000000000))*(x1897))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1919=((r12)*(sj6));
IkReal x1920=((cj6)*(r02));
if( IKabs(((gconst15)*(((((cj7)*(x1919)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(x1920))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst15)*(((((cj7)*(r22)))+(((sj7)*(x1919)))+(((sj7)*(x1920))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j8array[0]=IKatan2(((gconst15)*(((((cj7)*(x1919)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(x1920)))))), ((gconst15)*(((((cj7)*(r22)))+(((sj7)*(x1919)))+(((sj7)*(x1920)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[8];
IkReal x1921=IKcos(j8);
IkReal x1922=IKsin(j8);
IkReal x1923=((cj7)*(sj11));
IkReal x1924=((IkReal(0.0620002000000000))*(r21));
IkReal x1925=((r10)*(sj6));
IkReal x1926=((cj11)*(sj7));
IkReal x1927=((cj6)*(r00));
IkReal x1928=((IkReal(1.00000000000000))*(r20));
IkReal x1929=((sj11)*(sj7));
IkReal x1930=((cj6)*(r01));
IkReal x1931=((IkReal(1.00000000000000))*(cj7));
IkReal x1932=((IkReal(0.0620002000000000))*(r20));
IkReal x1933=((cj11)*(cj7));
IkReal x1934=((IkReal(1.00000000000000))*(sj7));
IkReal x1935=((cj6)*(r02));
IkReal x1936=((py)*(sj6));
IkReal x1937=((r11)*(sj6));
IkReal x1938=((cj6)*(px));
IkReal x1939=((r12)*(sj6));
IkReal x1940=((IkReal(1.00000000000000))*(x1922));
IkReal x1941=((sj9)*(x1921));
IkReal x1942=((cj9)*(x1921));
evalcond[0]=((((cj7)*(x1939)))+(((IkReal(-1.00000000000000))*(cj9)*(x1940)))+(((IkReal(-1.00000000000000))*(r22)*(x1934)))+(((cj7)*(x1935))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x1934)*(x1939)))+(x1942)+(((IkReal(-1.00000000000000))*(r22)*(x1931)))+(((IkReal(-1.00000000000000))*(x1934)*(x1935))));
evalcond[2]=((x1941)+(((IkReal(-1.00000000000000))*(x1926)*(x1927)))+(((IkReal(-1.00000000000000))*(x1928)*(x1933)))+(((x1929)*(x1937)))+(((x1929)*(x1930)))+(((IkReal(-1.00000000000000))*(x1925)*(x1926)))+(((r21)*(x1923))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1923)*(x1930)))+(((x1925)*(x1933)))+(((IkReal(-1.00000000000000))*(x1926)*(x1928)))+(((IkReal(-1.00000000000000))*(x1923)*(x1937)))+(((x1927)*(x1933)))+(((IkReal(-1.00000000000000))*(sj9)*(x1940)))+(((r21)*(x1929))));
evalcond[4]=((((IkReal(-1.00000000000000))*(x1940)))+(((r21)*(x1933)))+(((x1925)*(x1929)))+(((r20)*(x1923)))+(((x1927)*(x1929)))+(((x1926)*(x1930)))+(((x1926)*(x1937))));
evalcond[5]=((((IkReal(-1.00000000000000))*(x1923)*(x1927)))+(((r21)*(x1926)))+(((IkReal(-1.00000000000000))*(x1921)))+(((IkReal(-1.00000000000000))*(cj11)*(x1930)*(x1931)))+(((IkReal(-1.00000000000000))*(x1923)*(x1925)))+(((r20)*(x1929)))+(((IkReal(-1.00000000000000))*(cj11)*(x1931)*(x1937))));
evalcond[6]=((((IkReal(0.0620002000000000))*(x1923)*(x1930)))+(((IkReal(-1.00000000000000))*(x1924)*(x1929)))+(((IkReal(0.0616850000000000))*(sj9)*(x1922)))+(((IkReal(-0.302000000000000))*(x1921)))+(((IkReal(0.0620002000000000))*(x1923)*(x1937)))+(((cj7)*(x1936)))+(((IkReal(-1.00000000000000))*(pz)*(x1934)))+(((IkReal(-0.0620002000000000))*(x1925)*(x1933)))+(((IkReal(-0.0701403000000000))*(x1922)))+(((cj7)*(x1938)))+(((x1926)*(x1932)))+(((IkReal(-0.0620002000000000))*(x1927)*(x1933)))+(((IkReal(-0.00151567000000000))*(cj9)*(x1922))));
evalcond[7]=((IkReal(0.270000000000000))+(((IkReal(-1.00000000000000))*(x1934)*(x1936)))+(((x1932)*(x1933)))+(((IkReal(0.0620002000000000))*(x1925)*(x1926)))+(((IkReal(-1.00000000000000))*(x1934)*(x1938)))+(((IkReal(-1.00000000000000))*(x1923)*(x1924)))+(((IkReal(-1.00000000000000))*(pz)*(x1931)))+(((IkReal(-0.0616850000000000))*(x1941)))+(((IkReal(0.0701403000000000))*(x1921)))+(((IkReal(0.00151567000000000))*(x1942)))+(((IkReal(-0.0620002000000000))*(x1929)*(x1937)))+(((IkReal(-0.0620002000000000))*(x1929)*(x1930)))+(((IkReal(-0.302000000000000))*(x1922)))+(((IkReal(0.0620002000000000))*(x1926)*(x1927))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
IkReal x1943=((IkReal(1.00000000000000))*(cj11));
IkReal x1944=((r01)*(sj6));
IkReal x1945=((cj6)*(sj7));
IkReal x1946=((r01)*(sj11));
IkReal x1947=((IkReal(1.00000000000000))*(cj6));
IkReal x1948=((IkReal(0.0620002000000000))*(sj11));
IkReal x1949=((cj11)*(r10));
IkReal x1950=((r11)*(sj11));
IkReal x1951=((sj6)*(sj7));
IkReal x1952=((r21)*(sj11));
IkReal x1953=((r00)*(sj6));
IkReal x1954=((cj7)*(sj6));
IkReal x1955=((cj6)*(r10));
IkReal x1956=((IkReal(1.00000000000000))*(sj11));
IkReal x1957=((cj11)*(cj6));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(6.28318530717959))+(j9)), IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x1947))));
evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1943)*(x1955)))+(((cj6)*(x1950)))+(((IkReal(-1.00000000000000))*(x1944)*(x1956)))+(((cj11)*(x1953))));
evalcond[3]=((((sj11)*(x1955)))+(((r11)*(x1957)))+(((IkReal(-1.00000000000000))*(x1943)*(x1944)))+(((IkReal(-1.00000000000000))*(x1953)*(x1956))));
evalcond[4]=((IkReal(0.0515917000000000))+(((IkReal(0.0620002000000000))*(cj6)*(x1949)))+(((IkReal(-0.0620002000000000))*(cj11)*(x1953)))+(((IkReal(-1.00000000000000))*(py)*(x1947)))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x1948)))+(((x1944)*(x1948))));
evalcond[5]=((((x1945)*(x1946)))+(((x1950)*(x1951)))+(((IkReal(-1.00000000000000))*(r10)*(x1943)*(x1951)))+(((IkReal(-1.00000000000000))*(r00)*(x1943)*(x1945)))+(((cj7)*(x1952)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1943))));
evalcond[6]=((((IkReal(-1.00000000000000))*(cj7)*(x1946)*(x1947)))+(((x1949)*(x1954)))+(((IkReal(-1.00000000000000))*(r20)*(sj7)*(x1943)))+(((IkReal(-1.00000000000000))*(x1950)*(x1954)))+(((sj7)*(x1952)))+(((cj7)*(r00)*(x1957))));
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 dummyeval[1];
IkReal gconst16;
gconst16=IKsign((((cj10)*(cj10))+((sj10)*(sj10))));
dummyeval[0]=(((cj10)*(cj10))+((sj10)*(sj10)));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
{
IkReal dummyeval[1];
IkReal gconst17;
gconst17=IKsign((((cj10)*(cj10))+((sj10)*(sj10))));
dummyeval[0]=(((cj10)*(cj10))+((sj10)*(sj10)));
if( IKabs(dummyeval[0]) < 0.0000010000000000 )
{
continue;
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1958=((r20)*(sj11));
IkReal x1959=((sj6)*(sj7));
IkReal x1960=((IkReal(1.00000000000000))*(sj11));
IkReal x1961=((IkReal(1.00000000000000))*(sj10));
IkReal x1962=((cj11)*(r11));
IkReal x1963=((cj11)*(r21));
IkReal x1964=((IkReal(1.00000000000000))*(cj10));
IkReal x1965=((cj7)*(x1964));
IkReal x1966=((cj6)*(sj10)*(sj7));
IkReal x1967=((IkReal(1.00000000000000))*(cj11)*(r01));
IkReal x1968=((cj10)*(cj6)*(sj7));
if( IKabs(((gconst17)*(((((IkReal(-1.00000000000000))*(r22)*(x1965)))+(((IkReal(-1.00000000000000))*(x1959)*(x1961)*(x1962)))+(((IkReal(-1.00000000000000))*(cj7)*(x1961)*(x1963)))+(((IkReal(-1.00000000000000))*(r00)*(x1960)*(x1966)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(sj7)*(x1964)))+(((IkReal(-1.00000000000000))*(cj11)*(cj6)*(r01)*(sj7)*(x1961)))+(((IkReal(-1.00000000000000))*(r10)*(sj10)*(x1959)*(x1960)))+(((IkReal(-1.00000000000000))*(r12)*(x1959)*(x1964)))+(((IkReal(-1.00000000000000))*(cj7)*(x1958)*(x1961))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst17)*(((((r02)*(x1966)))+(((r12)*(sj10)*(x1959)))+(((IkReal(-1.00000000000000))*(cj11)*(cj6)*(r01)*(sj7)*(x1964)))+(((IkReal(-1.00000000000000))*(x1963)*(x1965)))+(((IkReal(-1.00000000000000))*(x1958)*(x1965)))+(((IkReal(-1.00000000000000))*(r00)*(x1960)*(x1968)))+(((cj7)*(r22)*(sj10)))+(((IkReal(-1.00000000000000))*(x1959)*(x1962)*(x1964)))+(((IkReal(-1.00000000000000))*(cj10)*(r10)*(x1959)*(x1960))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j8array[0]=IKatan2(((gconst17)*(((((IkReal(-1.00000000000000))*(r22)*(x1965)))+(((IkReal(-1.00000000000000))*(x1959)*(x1961)*(x1962)))+(((IkReal(-1.00000000000000))*(cj7)*(x1961)*(x1963)))+(((IkReal(-1.00000000000000))*(r00)*(x1960)*(x1966)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(sj7)*(x1964)))+(((IkReal(-1.00000000000000))*(cj11)*(cj6)*(r01)*(sj7)*(x1961)))+(((IkReal(-1.00000000000000))*(r10)*(sj10)*(x1959)*(x1960)))+(((IkReal(-1.00000000000000))*(r12)*(x1959)*(x1964)))+(((IkReal(-1.00000000000000))*(cj7)*(x1958)*(x1961)))))), ((gconst17)*(((((r02)*(x1966)))+(((r12)*(sj10)*(x1959)))+(((IkReal(-1.00000000000000))*(cj11)*(cj6)*(r01)*(sj7)*(x1964)))+(((IkReal(-1.00000000000000))*(x1963)*(x1965)))+(((IkReal(-1.00000000000000))*(x1958)*(x1965)))+(((IkReal(-1.00000000000000))*(r00)*(x1960)*(x1968)))+(((cj7)*(r22)*(sj10)))+(((IkReal(-1.00000000000000))*(x1959)*(x1962)*(x1964)))+(((IkReal(-1.00000000000000))*(cj10)*(r10)*(x1959)*(x1960)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[6];
IkReal x1969=IKcos(j8);
IkReal x1970=IKsin(j8);
IkReal x1971=((cj7)*(sj11));
IkReal x1972=((IkReal(0.0620002000000000))*(r21));
IkReal x1973=((IkReal(1.00000000000000))*(sj6));
IkReal x1974=((cj6)*(sj7));
IkReal x1975=((IkReal(0.0620002000000000))*(cj11));
IkReal x1976=((IkReal(1.00000000000000))*(cj7));
IkReal x1977=((r20)*(sj7));
IkReal x1978=((cj11)*(cj7));
IkReal x1979=((sj11)*(sj7));
IkReal x1980=((r10)*(sj6));
IkReal x1981=((cj11)*(r01));
IkReal x1982=((cj6)*(cj7));
IkReal x1983=((cj7)*(sj6));
IkReal x1984=((IkReal(1.00000000000000))*(cj10));
IkReal x1985=((IkReal(1.00000000000000))*(sj7));
IkReal x1986=((IkReal(0.0620002000000000))*(r01));
IkReal x1987=((r11)*(sj6));
IkReal x1988=((cj11)*(sj7));
IkReal x1989=((sj10)*(x1969));
IkReal x1990=((sj10)*(x1970));
IkReal x1991=((x1970)*(x1984));
evalcond[0]=((((IkReal(-1.00000000000000))*(x1969)*(x1984)))+(((IkReal(-1.00000000000000))*(x1990)))+(((IkReal(-1.00000000000000))*(r22)*(x1985)))+(((r12)*(x1983)))+(((r02)*(x1982))));
evalcond[1]=((x1989)+(((IkReal(-1.00000000000000))*(r02)*(x1974)))+(((IkReal(-1.00000000000000))*(x1991)))+(((IkReal(-1.00000000000000))*(r22)*(x1976)))+(((IkReal(-1.00000000000000))*(r12)*(sj7)*(x1973))));
evalcond[2]=((((r00)*(sj11)*(x1974)))+(((cj10)*(x1969)))+(((x1974)*(x1981)))+(((r20)*(x1971)))+(((x1979)*(x1980)))+(((r21)*(x1978)))+(((x1987)*(x1988)))+(x1990));
evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x1973)*(x1978)))+(x1989)+(((IkReal(-1.00000000000000))*(r10)*(x1971)*(x1973)))+(((IkReal(-1.00000000000000))*(x1991)))+(((r21)*(x1988)))+(((sj11)*(x1977)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(x1971)))+(((IkReal(-1.00000000000000))*(cj6)*(x1976)*(x1981))));
evalcond[4]=((((IkReal(-1.00000000000000))*(pz)*(x1985)))+(((IkReal(-0.302000000000000))*(x1969)))+(((py)*(x1983)))+(((IkReal(-1.00000000000000))*(cj7)*(x1975)*(x1980)))+(((cj6)*(x1971)*(x1986)))+(((IkReal(-1.00000000000000))*(r00)*(x1975)*(x1982)))+(((IkReal(0.0620002000000000))*(x1971)*(x1987)))+(((px)*(x1982)))+(((x1975)*(x1977)))+(((IkReal(-1.00000000000000))*(x1972)*(x1979)))+(((IkReal(-0.0686246300000000))*(x1970))));
evalcond[5]=((IkReal(0.270000000000000))+(((IkReal(0.0686246300000000))*(x1969)))+(((r00)*(x1974)*(x1975)))+(((IkReal(-1.00000000000000))*(sj11)*(x1974)*(x1986)))+(((cj7)*(r20)*(x1975)))+(((IkReal(-0.0620002000000000))*(x1979)*(x1987)))+(((sj7)*(x1975)*(x1980)))+(((IkReal(-1.00000000000000))*(px)*(x1974)))+(((IkReal(-1.00000000000000))*(pz)*(x1976)))+(((IkReal(-1.00000000000000))*(py)*(sj7)*(x1973)))+(((IkReal(-1.00000000000000))*(x1971)*(x1972)))+(((IkReal(-0.302000000000000))*(x1970))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x1992=((IkReal(1.00000000000000))*(r22));
IkReal x1993=((sj10)*(sj7));
IkReal x1994=((cj10)*(cj7));
IkReal x1995=((cj7)*(sj10));
IkReal x1996=((r12)*(sj6));
IkReal x1997=((cj6)*(r02));
IkReal x1998=((cj10)*(sj7));
if( IKabs(((gconst16)*(((((x1995)*(x1997)))+(((IkReal(-1.00000000000000))*(x1996)*(x1998)))+(((IkReal(-1.00000000000000))*(x1992)*(x1994)))+(((IkReal(-1.00000000000000))*(x1992)*(x1993)))+(((x1995)*(x1996)))+(((IkReal(-1.00000000000000))*(x1997)*(x1998))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst16)*(((((IkReal(-1.00000000000000))*(x1992)*(x1998)))+(((x1994)*(x1997)))+(((x1994)*(x1996)))+(((r22)*(x1995)))+(((x1993)*(x1997)))+(((x1993)*(x1996))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j8array[0]=IKatan2(((gconst16)*(((((x1995)*(x1997)))+(((IkReal(-1.00000000000000))*(x1996)*(x1998)))+(((IkReal(-1.00000000000000))*(x1992)*(x1994)))+(((IkReal(-1.00000000000000))*(x1992)*(x1993)))+(((x1995)*(x1996)))+(((IkReal(-1.00000000000000))*(x1997)*(x1998)))))), ((gconst16)*(((((IkReal(-1.00000000000000))*(x1992)*(x1998)))+(((x1994)*(x1997)))+(((x1994)*(x1996)))+(((r22)*(x1995)))+(((x1993)*(x1997)))+(((x1993)*(x1996)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[6];
IkReal x1999=IKcos(j8);
IkReal x2000=IKsin(j8);
IkReal x2001=((cj7)*(sj11));
IkReal x2002=((IkReal(0.0620002000000000))*(r21));
IkReal x2003=((IkReal(1.00000000000000))*(sj6));
IkReal x2004=((cj6)*(sj7));
IkReal x2005=((IkReal(0.0620002000000000))*(cj11));
IkReal x2006=((IkReal(1.00000000000000))*(cj7));
IkReal x2007=((r20)*(sj7));
IkReal x2008=((cj11)*(cj7));
IkReal x2009=((sj11)*(sj7));
IkReal x2010=((r10)*(sj6));
IkReal x2011=((cj11)*(r01));
IkReal x2012=((cj6)*(cj7));
IkReal x2013=((cj7)*(sj6));
IkReal x2014=((IkReal(1.00000000000000))*(cj10));
IkReal x2015=((IkReal(1.00000000000000))*(sj7));
IkReal x2016=((IkReal(0.0620002000000000))*(r01));
IkReal x2017=((r11)*(sj6));
IkReal x2018=((cj11)*(sj7));
IkReal x2019=((sj10)*(x1999));
IkReal x2020=((sj10)*(x2000));
IkReal x2021=((x2000)*(x2014));
evalcond[0]=((((IkReal(-1.00000000000000))*(x2020)))+(((r12)*(x2013)))+(((IkReal(-1.00000000000000))*(r22)*(x2015)))+(((r02)*(x2012)))+(((IkReal(-1.00000000000000))*(x1999)*(x2014))));
evalcond[1]=((((IkReal(-1.00000000000000))*(r02)*(x2004)))+(((IkReal(-1.00000000000000))*(x2021)))+(((IkReal(-1.00000000000000))*(r22)*(x2006)))+(((IkReal(-1.00000000000000))*(r12)*(sj7)*(x2003)))+(x2019));
evalcond[2]=((((r00)*(sj11)*(x2004)))+(x2020)+(((r20)*(x2001)))+(((x2017)*(x2018)))+(((cj10)*(x1999)))+(((x2009)*(x2010)))+(((x2004)*(x2011)))+(((r21)*(x2008))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x2003)*(x2008)))+(((IkReal(-1.00000000000000))*(x2021)))+(((sj11)*(x2007)))+(((IkReal(-1.00000000000000))*(r10)*(x2001)*(x2003)))+(((IkReal(-1.00000000000000))*(cj6)*(x2006)*(x2011)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(x2001)))+(((r21)*(x2018)))+(x2019));
evalcond[4]=((((px)*(x2012)))+(((IkReal(-1.00000000000000))*(cj7)*(x2005)*(x2010)))+(((IkReal(0.0620002000000000))*(x2001)*(x2017)))+(((IkReal(-0.0686246300000000))*(x2000)))+(((cj6)*(x2001)*(x2016)))+(((x2005)*(x2007)))+(((IkReal(-0.302000000000000))*(x1999)))+(((py)*(x2013)))+(((IkReal(-1.00000000000000))*(x2002)*(x2009)))+(((IkReal(-1.00000000000000))*(r00)*(x2005)*(x2012)))+(((IkReal(-1.00000000000000))*(pz)*(x2015))));
evalcond[5]=((IkReal(0.270000000000000))+(((IkReal(-0.0620002000000000))*(x2009)*(x2017)))+(((IkReal(-1.00000000000000))*(py)*(sj7)*(x2003)))+(((IkReal(-1.00000000000000))*(px)*(x2004)))+(((IkReal(-1.00000000000000))*(x2001)*(x2002)))+(((r00)*(x2004)*(x2005)))+(((IkReal(-1.00000000000000))*(sj11)*(x2004)*(x2016)))+(((sj7)*(x2005)*(x2010)))+(((IkReal(-0.302000000000000))*(x2000)))+(((IkReal(0.0686246300000000))*(x1999)))+(((IkReal(-1.00000000000000))*(pz)*(x2006)))+(((cj7)*(r20)*(x2005))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
IkReal x2022=((cj6)*(r10));
IkReal x2023=((IkReal(1.00000000000000))*(cj6));
IkReal x2024=((cj6)*(r11));
IkReal x2025=((r01)*(sj6));
IkReal x2026=((IkReal(0.0620002000000000))*(sj11));
IkReal x2027=((IkReal(0.0620002000000000))*(cj11));
IkReal x2028=((IkReal(1.00000000000000))*(cj11));
IkReal x2029=((r00)*(sj6));
IkReal x2030=((IkReal(1.00000000000000))*(sj11));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j10)), IkReal(6.28318530717959))));
evalcond[1]=((((r02)*(sj6)))+(((IkReal(-1.00000000000000))*(r12)*(x2023))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x2025)*(x2030)))+(((IkReal(-1.00000000000000))*(x2022)*(x2028)))+(cj9)+(((cj11)*(x2029)))+(((sj11)*(x2024))));
evalcond[3]=((sj9)+(((IkReal(-1.00000000000000))*(x2025)*(x2028)))+(((cj11)*(x2024)))+(((sj11)*(x2022)))+(((IkReal(-1.00000000000000))*(x2029)*(x2030))));
evalcond[4]=((IkReal(-0.0100933000000000))+(((x2025)*(x2026)))+(((IkReal(-0.00151567000000000))*(sj9)))+(((IkReal(-0.0616850000000000))*(cj9)))+(((IkReal(-1.00000000000000))*(x2024)*(x2026)))+(((x2022)*(x2027)))+(((px)*(sj6)))+(((IkReal(-1.00000000000000))*(py)*(x2023)))+(((IkReal(-1.00000000000000))*(x2027)*(x2029))));
if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 )
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x2031=((IkReal(1.00000000000000))*(sj7));
IkReal x2032=((r12)*(sj6));
IkReal x2033=((cj6)*(r02));
if( IKabs(((((IkReal(-1.00000000000000))*(x2031)*(x2032)))+(((IkReal(-1.00000000000000))*(x2031)*(x2033)))+(((IkReal(-1.00000000000000))*(cj7)*(r22))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r22)*(x2031)))+(((cj7)*(x2032)))+(((cj7)*(x2033))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(x2031)*(x2032)))+(((IkReal(-1.00000000000000))*(x2031)*(x2033)))+(((IkReal(-1.00000000000000))*(cj7)*(r22)))))+IKsqr(((((IkReal(-1.00000000000000))*(r22)*(x2031)))+(((cj7)*(x2032)))+(((cj7)*(x2033)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((((IkReal(-1.00000000000000))*(x2031)*(x2032)))+(((IkReal(-1.00000000000000))*(x2031)*(x2033)))+(((IkReal(-1.00000000000000))*(cj7)*(r22)))), ((((IkReal(-1.00000000000000))*(r22)*(x2031)))+(((cj7)*(x2032)))+(((cj7)*(x2033)))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[8];
IkReal x2034=IKcos(j8);
IkReal x2035=IKsin(j8);
IkReal x2036=((cj7)*(sj11));
IkReal x2037=((IkReal(0.0620002000000000))*(r21));
IkReal x2038=((r10)*(sj6));
IkReal x2039=((cj11)*(sj7));
IkReal x2040=((cj6)*(r00));
IkReal x2041=((IkReal(1.00000000000000))*(r20));
IkReal x2042=((sj11)*(sj7));
IkReal x2043=((cj6)*(r01));
IkReal x2044=((IkReal(1.00000000000000))*(cj7));
IkReal x2045=((IkReal(0.0620002000000000))*(r20));
IkReal x2046=((cj11)*(cj7));
IkReal x2047=((IkReal(1.00000000000000))*(sj7));
IkReal x2048=((cj6)*(r02));
IkReal x2049=((py)*(sj6));
IkReal x2050=((r11)*(sj6));
IkReal x2051=((cj6)*(px));
IkReal x2052=((r12)*(sj6));
IkReal x2053=((IkReal(1.00000000000000))*(x2035));
IkReal x2054=((cj9)*(x2035));
IkReal x2055=((sj9)*(x2034));
IkReal x2056=((cj9)*(x2034));
evalcond[0]=((((IkReal(-1.00000000000000))*(x2034)))+(((IkReal(-1.00000000000000))*(r22)*(x2047)))+(((cj7)*(x2048)))+(((cj7)*(x2052))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x2047)*(x2048)))+(((IkReal(-1.00000000000000))*(x2053)))+(((IkReal(-1.00000000000000))*(x2047)*(x2052)))+(((IkReal(-1.00000000000000))*(r22)*(x2044))));
evalcond[2]=((((x2042)*(x2043)))+(x2055)+(((IkReal(-1.00000000000000))*(x2039)*(x2040)))+(((IkReal(-1.00000000000000))*(x2041)*(x2046)))+(((IkReal(-1.00000000000000))*(x2038)*(x2039)))+(((r21)*(x2036)))+(((x2042)*(x2050))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x2036)*(x2043)))+(((IkReal(-1.00000000000000))*(x2039)*(x2041)))+(((IkReal(-1.00000000000000))*(x2036)*(x2050)))+(((x2038)*(x2046)))+(((IkReal(-1.00000000000000))*(sj9)*(x2053)))+(((x2040)*(x2046)))+(((r21)*(x2042))));
evalcond[4]=((((IkReal(-1.00000000000000))*(x2056)))+(((r21)*(x2046)))+(((x2040)*(x2042)))+(((x2038)*(x2042)))+(((x2039)*(x2043)))+(((r20)*(x2036)))+(((x2039)*(x2050))));
evalcond[5]=((x2054)+(((r21)*(x2039)))+(((IkReal(-1.00000000000000))*(cj11)*(x2043)*(x2044)))+(((IkReal(-1.00000000000000))*(cj11)*(x2044)*(x2050)))+(((IkReal(-1.00000000000000))*(x2036)*(x2038)))+(((IkReal(-1.00000000000000))*(x2036)*(x2040)))+(((r20)*(x2042))));
evalcond[6]=((((x2039)*(x2045)))+(((IkReal(-0.00151567000000000))*(x2054)))+(((IkReal(-1.00000000000000))*(x2037)*(x2042)))+(((IkReal(-0.0701403000000000))*(x2035)))+(((IkReal(-0.0620002000000000))*(x2038)*(x2046)))+(((cj7)*(x2051)))+(((IkReal(-0.302000000000000))*(x2034)))+(((IkReal(-0.0620002000000000))*(x2040)*(x2046)))+(((IkReal(-1.00000000000000))*(pz)*(x2047)))+(((IkReal(0.0620002000000000))*(x2036)*(x2043)))+(((IkReal(0.0616850000000000))*(sj9)*(x2035)))+(((cj7)*(x2049)))+(((IkReal(0.0620002000000000))*(x2036)*(x2050))));
evalcond[7]=((IkReal(0.270000000000000))+(((IkReal(0.0620002000000000))*(x2039)*(x2040)))+(((IkReal(-1.00000000000000))*(pz)*(x2044)))+(((IkReal(-1.00000000000000))*(x2047)*(x2049)))+(((IkReal(-0.0616850000000000))*(x2055)))+(((IkReal(0.0701403000000000))*(x2034)))+(((IkReal(0.0620002000000000))*(x2038)*(x2039)))+(((IkReal(-0.0620002000000000))*(x2042)*(x2050)))+(((x2045)*(x2046)))+(((IkReal(-0.302000000000000))*(x2035)))+(((IkReal(-1.00000000000000))*(x2047)*(x2051)))+(((IkReal(-0.0620002000000000))*(x2042)*(x2043)))+(((IkReal(-1.00000000000000))*(x2036)*(x2037)))+(((IkReal(0.00151567000000000))*(x2056))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
IkReal x2057=((cj6)*(r10));
IkReal x2058=((IkReal(1.00000000000000))*(cj6));
IkReal x2059=((cj6)*(r11));
IkReal x2060=((r01)*(sj6));
IkReal x2061=((IkReal(0.0620002000000000))*(sj11));
IkReal x2062=((IkReal(0.0620002000000000))*(cj11));
IkReal x2063=((IkReal(1.00000000000000))*(cj11));
IkReal x2064=((r00)*(sj6));
IkReal x2065=((IkReal(1.00000000000000))*(sj11));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j10)), IkReal(6.28318530717959))));
evalcond[1]=((((IkReal(-1.00000000000000))*(r12)*(x2058)))+(((r02)*(sj6))));
evalcond[2]=((cj9)+(((sj11)*(x2059)))+(((cj11)*(x2064)))+(((IkReal(-1.00000000000000))*(x2060)*(x2065)))+(((IkReal(-1.00000000000000))*(x2057)*(x2063))));
evalcond[3]=((((cj11)*(x2059)))+(((IkReal(-1.00000000000000))*(sj9)))+(((sj11)*(x2057)))+(((IkReal(-1.00000000000000))*(x2064)*(x2065)))+(((IkReal(-1.00000000000000))*(x2060)*(x2063))));
evalcond[4]=((IkReal(-0.0100933000000000))+(((x2060)*(x2061)))+(((IkReal(-1.00000000000000))*(py)*(x2058)))+(((IkReal(-1.00000000000000))*(x2059)*(x2061)))+(((IkReal(-0.00151567000000000))*(sj9)))+(((IkReal(-0.0616850000000000))*(cj9)))+(((IkReal(-1.00000000000000))*(x2062)*(x2064)))+(((px)*(sj6)))+(((x2057)*(x2062))));
if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 )
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x2066=((r12)*(sj6));
IkReal x2067=((cj6)*(r02));
IkReal x2068=((IkReal(1.00000000000000))*(cj7));
if( IKabs(((((cj7)*(r22)))+(((sj7)*(x2066)))+(((sj7)*(x2067))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(x2067)*(x2068)))+(((IkReal(-1.00000000000000))*(x2066)*(x2068)))+(((r22)*(sj7))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj7)*(r22)))+(((sj7)*(x2066)))+(((sj7)*(x2067)))))+IKsqr(((((IkReal(-1.00000000000000))*(x2067)*(x2068)))+(((IkReal(-1.00000000000000))*(x2066)*(x2068)))+(((r22)*(sj7)))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((((cj7)*(r22)))+(((sj7)*(x2066)))+(((sj7)*(x2067)))), ((((IkReal(-1.00000000000000))*(x2067)*(x2068)))+(((IkReal(-1.00000000000000))*(x2066)*(x2068)))+(((r22)*(sj7)))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[8];
IkReal x2069=IKcos(j8);
IkReal x2070=IKsin(j8);
IkReal x2071=((cj7)*(sj11));
IkReal x2072=((IkReal(0.0620002000000000))*(r21));
IkReal x2073=((r10)*(sj6));
IkReal x2074=((cj11)*(sj7));
IkReal x2075=((cj6)*(r00));
IkReal x2076=((IkReal(1.00000000000000))*(r20));
IkReal x2077=((sj11)*(sj7));
IkReal x2078=((cj6)*(r01));
IkReal x2079=((IkReal(1.00000000000000))*(cj7));
IkReal x2080=((IkReal(0.0620002000000000))*(r20));
IkReal x2081=((cj11)*(cj7));
IkReal x2082=((IkReal(1.00000000000000))*(sj7));
IkReal x2083=((cj6)*(r02));
IkReal x2084=((py)*(sj6));
IkReal x2085=((r11)*(sj6));
IkReal x2086=((cj6)*(px));
IkReal x2087=((r12)*(sj6));
IkReal x2088=((sj9)*(x2069));
IkReal x2089=((cj9)*(x2069));
IkReal x2090=((IkReal(1.00000000000000))*(x2070));
evalcond[0]=((x2069)+(((cj7)*(x2083)))+(((cj7)*(x2087)))+(((IkReal(-1.00000000000000))*(r22)*(x2082))));
evalcond[1]=((x2070)+(((IkReal(-1.00000000000000))*(r22)*(x2079)))+(((IkReal(-1.00000000000000))*(x2082)*(x2087)))+(((IkReal(-1.00000000000000))*(x2082)*(x2083))));
evalcond[2]=((((x2077)*(x2085)))+(((IkReal(-1.00000000000000))*(x2074)*(x2075)))+(((r21)*(x2071)))+(x2088)+(((IkReal(-1.00000000000000))*(x2076)*(x2081)))+(((IkReal(-1.00000000000000))*(x2073)*(x2074)))+(((x2077)*(x2078))));
evalcond[3]=((((x2073)*(x2081)))+(((IkReal(-1.00000000000000))*(x2071)*(x2078)))+(((r21)*(x2077)))+(((x2075)*(x2081)))+(((IkReal(-1.00000000000000))*(sj9)*(x2090)))+(((IkReal(-1.00000000000000))*(x2074)*(x2076)))+(((IkReal(-1.00000000000000))*(x2071)*(x2085))));
evalcond[4]=((((x2074)*(x2085)))+(((x2075)*(x2077)))+(x2089)+(((r21)*(x2081)))+(((r20)*(x2071)))+(((x2073)*(x2077)))+(((x2074)*(x2078))));
evalcond[5]=((((IkReal(-1.00000000000000))*(cj11)*(x2079)*(x2085)))+(((r20)*(x2077)))+(((r21)*(x2074)))+(((IkReal(-1.00000000000000))*(cj11)*(x2078)*(x2079)))+(((IkReal(-1.00000000000000))*(cj9)*(x2090)))+(((IkReal(-1.00000000000000))*(x2071)*(x2073)))+(((IkReal(-1.00000000000000))*(x2071)*(x2075))));
evalcond[6]=((((IkReal(0.0620002000000000))*(x2071)*(x2085)))+(((IkReal(-1.00000000000000))*(pz)*(x2082)))+(((IkReal(0.0620002000000000))*(x2071)*(x2078)))+(((IkReal(-0.00151567000000000))*(cj9)*(x2070)))+(((x2074)*(x2080)))+(((IkReal(-0.0701403000000000))*(x2070)))+(((IkReal(-0.0620002000000000))*(x2075)*(x2081)))+(((cj7)*(x2086)))+(((IkReal(-0.302000000000000))*(x2069)))+(((cj7)*(x2084)))+(((IkReal(0.0616850000000000))*(sj9)*(x2070)))+(((IkReal(-0.0620002000000000))*(x2073)*(x2081)))+(((IkReal(-1.00000000000000))*(x2072)*(x2077))));
evalcond[7]=((IkReal(0.270000000000000))+(((IkReal(0.0701403000000000))*(x2069)))+(((IkReal(-1.00000000000000))*(x2082)*(x2086)))+(((IkReal(0.0620002000000000))*(x2073)*(x2074)))+(((IkReal(-0.0620002000000000))*(x2077)*(x2085)))+(((IkReal(-0.302000000000000))*(x2070)))+(((IkReal(-0.0616850000000000))*(x2088)))+(((IkReal(-1.00000000000000))*(x2071)*(x2072)))+(((IkReal(-1.00000000000000))*(x2082)*(x2084)))+(((IkReal(0.0620002000000000))*(x2074)*(x2075)))+(((IkReal(-1.00000000000000))*(pz)*(x2079)))+(((x2080)*(x2081)))+(((IkReal(0.00151567000000000))*(x2089)))+(((IkReal(-0.0620002000000000))*(x2077)*(x2078))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
} else
{
if( 1 )
{
continue;
} else
{
}
}
}
}
}
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x2091=((IKabs(sj9) != 0)?((IkReal)1/(sj9)):(IkReal)1.0e30);
IkReal x2092=((cj6)*(cj7));
IkReal x2093=((IkReal(1.00000000000000))*(r01));
IkReal x2094=((r21)*(sj7));
IkReal x2095=((cj7)*(sj6));
IkReal x2096=((cj11)*(r10));
IkReal x2097=((cj11)*(r00));
IkReal x2098=((cj9)*(sj10));
IkReal x2099=((IkReal(1.00000000000000))*(sj7));
IkReal x2100=((cj11)*(r20));
IkReal x2101=((IkReal(1.00000000000000))*(r11));
IkReal x2102=((sj11)*(x2098));
if( IKabs(((x2091)*(((((IkReal(-1.00000000000000))*(sj11)*(x2092)*(x2093)))+(((IkReal(-1.00000000000000))*(x2099)*(x2100)))+(((x2095)*(x2096)))+(((sj11)*(x2094)))+(((IkReal(-1.00000000000000))*(sj11)*(x2095)*(x2101)))+(((x2092)*(x2097))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x2091)*(((IKabs(cj10) != 0)?((IkReal)1/(cj10)):(IkReal)1.0e30))*(((((r02)*(sj9)*(x2092)))+(((IkReal(-1.00000000000000))*(x2098)*(x2099)*(x2100)))+(((r12)*(sj9)*(x2095)))+(((IkReal(-1.00000000000000))*(x2092)*(x2093)*(x2102)))+(((x2094)*(x2102)))+(((x2095)*(x2096)*(x2098)))+(((IkReal(-1.00000000000000))*(x2095)*(x2101)*(x2102)))+(((IkReal(-1.00000000000000))*(r22)*(sj9)*(x2099)))+(((x2092)*(x2097)*(x2098))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x2091)*(((((IkReal(-1.00000000000000))*(sj11)*(x2092)*(x2093)))+(((IkReal(-1.00000000000000))*(x2099)*(x2100)))+(((x2095)*(x2096)))+(((sj11)*(x2094)))+(((IkReal(-1.00000000000000))*(sj11)*(x2095)*(x2101)))+(((x2092)*(x2097)))))))+IKsqr(((x2091)*(((IKabs(cj10) != 0)?((IkReal)1/(cj10)):(IkReal)1.0e30))*(((((r02)*(sj9)*(x2092)))+(((IkReal(-1.00000000000000))*(x2098)*(x2099)*(x2100)))+(((r12)*(sj9)*(x2095)))+(((IkReal(-1.00000000000000))*(x2092)*(x2093)*(x2102)))+(((x2094)*(x2102)))+(((x2095)*(x2096)*(x2098)))+(((IkReal(-1.00000000000000))*(x2095)*(x2101)*(x2102)))+(((IkReal(-1.00000000000000))*(r22)*(sj9)*(x2099)))+(((x2092)*(x2097)*(x2098)))))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((x2091)*(((((IkReal(-1.00000000000000))*(sj11)*(x2092)*(x2093)))+(((IkReal(-1.00000000000000))*(x2099)*(x2100)))+(((x2095)*(x2096)))+(((sj11)*(x2094)))+(((IkReal(-1.00000000000000))*(sj11)*(x2095)*(x2101)))+(((x2092)*(x2097)))))), ((x2091)*(((IKabs(cj10) != 0)?((IkReal)1/(cj10)):(IkReal)1.0e30))*(((((r02)*(sj9)*(x2092)))+(((IkReal(-1.00000000000000))*(x2098)*(x2099)*(x2100)))+(((r12)*(sj9)*(x2095)))+(((IkReal(-1.00000000000000))*(x2092)*(x2093)*(x2102)))+(((x2094)*(x2102)))+(((x2095)*(x2096)*(x2098)))+(((IkReal(-1.00000000000000))*(x2095)*(x2101)*(x2102)))+(((IkReal(-1.00000000000000))*(r22)*(sj9)*(x2099)))+(((x2092)*(x2097)*(x2098)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[8];
IkReal x2103=IKsin(j8);
IkReal x2104=IKcos(j8);
IkReal x2105=((cj7)*(sj11));
IkReal x2106=((IkReal(0.0620002000000000))*(r21));
IkReal x2107=((IkReal(1.00000000000000))*(sj6));
IkReal x2108=((cj11)*(sj7));
IkReal x2109=((cj6)*(r00));
IkReal x2110=((IkReal(1.00000000000000))*(r20));
IkReal x2111=((cj6)*(r01));
IkReal x2112=((sj11)*(sj7));
IkReal x2113=((IkReal(1.00000000000000))*(cj7));
IkReal x2114=((IkReal(0.0620002000000000))*(r20));
IkReal x2115=((cj11)*(cj7));
IkReal x2116=((IkReal(1.00000000000000))*(sj7));
IkReal x2117=((cj6)*(r02));
IkReal x2118=((r10)*(sj6));
IkReal x2119=((cj7)*(sj6));
IkReal x2120=((IkReal(1.00000000000000))*(cj10));
IkReal x2121=((r11)*(sj6));
IkReal x2122=((cj6)*(px));
IkReal x2123=((sj9)*(x2104));
IkReal x2124=((cj9)*(x2104));
IkReal x2125=((cj9)*(x2103));
IkReal x2126=((sj9)*(x2103));
evalcond[0]=((((r12)*(x2119)))+(((IkReal(-1.00000000000000))*(x2104)*(x2120)))+(((sj10)*(x2125)))+(((IkReal(-1.00000000000000))*(r22)*(x2116)))+(((cj7)*(x2117))));
evalcond[1]=((((IkReal(-1.00000000000000))*(sj10)*(x2124)))+(((IkReal(-1.00000000000000))*(r22)*(x2113)))+(((IkReal(-1.00000000000000))*(r12)*(sj7)*(x2107)))+(((IkReal(-1.00000000000000))*(x2103)*(x2120)))+(((IkReal(-1.00000000000000))*(x2116)*(x2117))));
evalcond[2]=((x2123)+(((x2111)*(x2112)))+(((x2112)*(x2121)))+(((r21)*(x2105)))+(((IkReal(-1.00000000000000))*(x2110)*(x2115)))+(((IkReal(-1.00000000000000))*(x2108)*(x2109)))+(((IkReal(-1.00000000000000))*(r10)*(x2107)*(x2108))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x2105)*(x2111)))+(((x2109)*(x2115)))+(((IkReal(-1.00000000000000))*(x2108)*(x2110)))+(((r21)*(x2112)))+(((IkReal(-1.00000000000000))*(x2126)))+(((IkReal(-1.00000000000000))*(r11)*(x2105)*(x2107)))+(((x2115)*(x2118))));
evalcond[4]=((((r20)*(x2105)))+(((x2108)*(x2111)))+(((x2112)*(x2118)))+(((sj10)*(x2103)))+(((IkReal(-1.00000000000000))*(x2120)*(x2124)))+(((x2108)*(x2121)))+(((x2109)*(x2112)))+(((r21)*(x2115))));
evalcond[5]=((((IkReal(-1.00000000000000))*(x2105)*(x2109)))+(((IkReal(-1.00000000000000))*(r10)*(x2105)*(x2107)))+(((r20)*(x2112)))+(((IkReal(-1.00000000000000))*(cj11)*(x2111)*(x2113)))+(((sj10)*(x2104)))+(((r21)*(x2108)))+(((IkReal(-1.00000000000000))*(r11)*(x2107)*(x2115)))+(((cj10)*(x2125))));
evalcond[6]=((((IkReal(-0.0620002000000000))*(x2115)*(x2118)))+(((IkReal(-0.00151567000000000))*(x2125)))+(((IkReal(0.0620002000000000))*(x2105)*(x2111)))+(((x2108)*(x2114)))+(((IkReal(-1.00000000000000))*(x2106)*(x2112)))+(((IkReal(-0.0620002000000000))*(x2109)*(x2115)))+(((IkReal(-1.00000000000000))*(pz)*(x2116)))+(((IkReal(0.0620002000000000))*(x2105)*(x2121)))+(((cj7)*(x2122)))+(((py)*(x2119)))+(((IkReal(0.0616850000000000))*(x2126)))+(((IkReal(-0.0701403000000000))*(x2103)))+(((IkReal(-0.302000000000000))*(x2104))));
evalcond[7]=((IkReal(0.270000000000000))+(((IkReal(0.0620002000000000))*(x2108)*(x2109)))+(((IkReal(0.0620002000000000))*(x2108)*(x2118)))+(((IkReal(-1.00000000000000))*(x2116)*(x2122)))+(((IkReal(-1.00000000000000))*(pz)*(x2113)))+(((IkReal(0.0701403000000000))*(x2104)))+(((IkReal(-0.0620002000000000))*(x2112)*(x2121)))+(((IkReal(-0.0620002000000000))*(x2111)*(x2112)))+(((IkReal(-0.0616850000000000))*(x2123)))+(((x2114)*(x2115)))+(((IkReal(-0.302000000000000))*(x2103)))+(((IkReal(-1.00000000000000))*(py)*(sj7)*(x2107)))+(((IkReal(-1.00000000000000))*(x2105)*(x2106)))+(((IkReal(0.00151567000000000))*(x2124))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x2127=(cj9)*(cj9);
IkReal x2128=((r22)*(sj7));
IkReal x2129=((IkReal(1.00000000000000))*(cj7));
IkReal x2130=((r12)*(sj6));
IkReal x2131=((cj6)*(r01));
IkReal x2132=((r11)*(sj6));
IkReal x2133=((cj6)*(r02));
IkReal x2134=((cj11)*(sj7));
IkReal x2135=((cj7)*(r20)*(sj11));
IkReal x2136=((cj11)*(cj7)*(r21));
IkReal x2137=((cj9)*(x2134));
IkReal x2138=((r10)*(sj11)*(sj6)*(sj7));
IkReal x2139=((cj6)*(r00)*(sj11)*(sj7));
if( IKabs(((((IKabs(((((sj10)*(x2127)))+(((IkReal(-1.00000000000000))*(sj10))))) != 0)?((IkReal)1/(((((sj10)*(x2127)))+(((IkReal(-1.00000000000000))*(sj10)))))):(IkReal)1.0e30))*(((((cj9)*(x2128)))+(((IkReal(-1.00000000000000))*(cj9)*(x2129)*(x2130)))+(x2136)+(x2135)+(x2138)+(x2139)+(((IkReal(-1.00000000000000))*(cj9)*(x2129)*(x2133)))+(((x2131)*(x2134)))+(((x2132)*(x2134))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((((IkReal(-1.00000000000000))*(cj10)))+(((cj10)*(x2127))))) != 0)?((IkReal)1/(((((IkReal(-1.00000000000000))*(cj10)))+(((cj10)*(x2127)))))):(IkReal)1.0e30))*(((x2128)+(((cj9)*(x2136)))+(((IkReal(-1.00000000000000))*(x2129)*(x2130)))+(((cj9)*(x2135)))+(((x2131)*(x2137)))+(((cj9)*(x2138)))+(((IkReal(-1.00000000000000))*(x2129)*(x2133)))+(((x2132)*(x2137)))+(((cj9)*(x2139))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(((((sj10)*(x2127)))+(((IkReal(-1.00000000000000))*(sj10))))) != 0)?((IkReal)1/(((((sj10)*(x2127)))+(((IkReal(-1.00000000000000))*(sj10)))))):(IkReal)1.0e30))*(((((cj9)*(x2128)))+(((IkReal(-1.00000000000000))*(cj9)*(x2129)*(x2130)))+(x2136)+(x2135)+(x2138)+(x2139)+(((IkReal(-1.00000000000000))*(cj9)*(x2129)*(x2133)))+(((x2131)*(x2134)))+(((x2132)*(x2134)))))))+IKsqr(((((IKabs(((((IkReal(-1.00000000000000))*(cj10)))+(((cj10)*(x2127))))) != 0)?((IkReal)1/(((((IkReal(-1.00000000000000))*(cj10)))+(((cj10)*(x2127)))))):(IkReal)1.0e30))*(((x2128)+(((cj9)*(x2136)))+(((IkReal(-1.00000000000000))*(x2129)*(x2130)))+(((cj9)*(x2135)))+(((x2131)*(x2137)))+(((cj9)*(x2138)))+(((IkReal(-1.00000000000000))*(x2129)*(x2133)))+(((x2132)*(x2137)))+(((cj9)*(x2139)))))))-1) <= IKFAST_SINCOS_THRESH )
continue;
j8array[0]=IKatan2(((((IKabs(((((sj10)*(x2127)))+(((IkReal(-1.00000000000000))*(sj10))))) != 0)?((IkReal)1/(((((sj10)*(x2127)))+(((IkReal(-1.00000000000000))*(sj10)))))):(IkReal)1.0e30))*(((((cj9)*(x2128)))+(((IkReal(-1.00000000000000))*(cj9)*(x2129)*(x2130)))+(x2136)+(x2135)+(x2138)+(x2139)+(((IkReal(-1.00000000000000))*(cj9)*(x2129)*(x2133)))+(((x2131)*(x2134)))+(((x2132)*(x2134)))))), ((((IKabs(((((IkReal(-1.00000000000000))*(cj10)))+(((cj10)*(x2127))))) != 0)?((IkReal)1/(((((IkReal(-1.00000000000000))*(cj10)))+(((cj10)*(x2127)))))):(IkReal)1.0e30))*(((x2128)+(((cj9)*(x2136)))+(((IkReal(-1.00000000000000))*(x2129)*(x2130)))+(((cj9)*(x2135)))+(((x2131)*(x2137)))+(((cj9)*(x2138)))+(((IkReal(-1.00000000000000))*(x2129)*(x2133)))+(((x2132)*(x2137)))+(((cj9)*(x2139)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[8];
IkReal x2140=IKsin(j8);
IkReal x2141=IKcos(j8);
IkReal x2142=((cj7)*(sj11));
IkReal x2143=((IkReal(0.0620002000000000))*(r21));
IkReal x2144=((IkReal(1.00000000000000))*(sj6));
IkReal x2145=((cj11)*(sj7));
IkReal x2146=((cj6)*(r00));
IkReal x2147=((IkReal(1.00000000000000))*(r20));
IkReal x2148=((cj6)*(r01));
IkReal x2149=((sj11)*(sj7));
IkReal x2150=((IkReal(1.00000000000000))*(cj7));
IkReal x2151=((IkReal(0.0620002000000000))*(r20));
IkReal x2152=((cj11)*(cj7));
IkReal x2153=((IkReal(1.00000000000000))*(sj7));
IkReal x2154=((cj6)*(r02));
IkReal x2155=((r10)*(sj6));
IkReal x2156=((cj7)*(sj6));
IkReal x2157=((IkReal(1.00000000000000))*(cj10));
IkReal x2158=((r11)*(sj6));
IkReal x2159=((cj6)*(px));
IkReal x2160=((sj9)*(x2141));
IkReal x2161=((cj9)*(x2141));
IkReal x2162=((cj9)*(x2140));
IkReal x2163=((sj9)*(x2140));
evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x2153)))+(((r12)*(x2156)))+(((sj10)*(x2162)))+(((cj7)*(x2154)))+(((IkReal(-1.00000000000000))*(x2141)*(x2157))));
evalcond[1]=((((IkReal(-1.00000000000000))*(sj10)*(x2161)))+(((IkReal(-1.00000000000000))*(x2140)*(x2157)))+(((IkReal(-1.00000000000000))*(r12)*(sj7)*(x2144)))+(((IkReal(-1.00000000000000))*(r22)*(x2150)))+(((IkReal(-1.00000000000000))*(x2153)*(x2154))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x2147)*(x2152)))+(x2160)+(((IkReal(-1.00000000000000))*(x2145)*(x2146)))+(((IkReal(-1.00000000000000))*(r10)*(x2144)*(x2145)))+(((x2149)*(x2158)))+(((r21)*(x2142)))+(((x2148)*(x2149))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x2163)))+(((IkReal(-1.00000000000000))*(r11)*(x2142)*(x2144)))+(((x2146)*(x2152)))+(((IkReal(-1.00000000000000))*(x2145)*(x2147)))+(((x2152)*(x2155)))+(((IkReal(-1.00000000000000))*(x2142)*(x2148)))+(((r21)*(x2149))));
evalcond[4]=((((r20)*(x2142)))+(((x2146)*(x2149)))+(((r21)*(x2152)))+(((x2145)*(x2148)))+(((IkReal(-1.00000000000000))*(x2157)*(x2161)))+(((x2145)*(x2158)))+(((sj10)*(x2140)))+(((x2149)*(x2155))));
evalcond[5]=((((sj10)*(x2141)))+(((IkReal(-1.00000000000000))*(r11)*(x2144)*(x2152)))+(((r20)*(x2149)))+(((r21)*(x2145)))+(((IkReal(-1.00000000000000))*(r10)*(x2142)*(x2144)))+(((IkReal(-1.00000000000000))*(cj11)*(x2148)*(x2150)))+(((IkReal(-1.00000000000000))*(x2142)*(x2146)))+(((cj10)*(x2162))));
evalcond[6]=((((x2145)*(x2151)))+(((IkReal(-0.0620002000000000))*(x2146)*(x2152)))+(((IkReal(-0.0620002000000000))*(x2152)*(x2155)))+(((py)*(x2156)))+(((IkReal(-1.00000000000000))*(pz)*(x2153)))+(((IkReal(0.0616850000000000))*(x2163)))+(((cj7)*(x2159)))+(((IkReal(0.0620002000000000))*(x2142)*(x2148)))+(((IkReal(-0.302000000000000))*(x2141)))+(((IkReal(0.0620002000000000))*(x2142)*(x2158)))+(((IkReal(-1.00000000000000))*(x2143)*(x2149)))+(((IkReal(-0.0701403000000000))*(x2140)))+(((IkReal(-0.00151567000000000))*(x2162))));
evalcond[7]=((IkReal(0.270000000000000))+(((IkReal(-1.00000000000000))*(x2142)*(x2143)))+(((IkReal(-0.0616850000000000))*(x2160)))+(((IkReal(-0.302000000000000))*(x2140)))+(((x2151)*(x2152)))+(((IkReal(-0.0620002000000000))*(x2148)*(x2149)))+(((IkReal(0.0620002000000000))*(x2145)*(x2155)))+(((IkReal(0.0620002000000000))*(x2145)*(x2146)))+(((IkReal(-1.00000000000000))*(py)*(sj7)*(x2144)))+(((IkReal(-1.00000000000000))*(pz)*(x2150)))+(((IkReal(0.00151567000000000))*(x2161)))+(((IkReal(-0.0620002000000000))*(x2149)*(x2158)))+(((IkReal(-1.00000000000000))*(x2153)*(x2159)))+(((IkReal(0.0701403000000000))*(x2141))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
} else
{
{
IkReal j8array[1], cj8array[1], sj8array[1];
bool j8valid[1]={false};
_nj8 = 1;
IkReal x2164=((IkReal(1.00000000000000))*(cj10));
IkReal x2165=((cj6)*(r02));
IkReal x2166=((cj7)*(r22));
IkReal x2167=((r12)*(sj6));
IkReal x2168=((cj10)*(cj7));
IkReal x2169=((r22)*(sj7));
IkReal x2170=((IkReal(1.00000000000000))*(cj9)*(sj10));
if( IKabs(((gconst9)*(((((IkReal(-1.00000000000000))*(cj7)*(x2165)*(x2170)))+(((cj9)*(sj10)*(x2169)))+(((IkReal(-1.00000000000000))*(cj7)*(x2167)*(x2170)))+(((IkReal(-1.00000000000000))*(x2164)*(x2166)))+(((IkReal(-1.00000000000000))*(sj7)*(x2164)*(x2167)))+(((IkReal(-1.00000000000000))*(sj7)*(x2164)*(x2165))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst9)*(((((IkReal(-1.00000000000000))*(x2166)*(x2170)))+(((IkReal(-1.00000000000000))*(x2164)*(x2169)))+(((IkReal(-1.00000000000000))*(sj7)*(x2167)*(x2170)))+(((IkReal(-1.00000000000000))*(sj7)*(x2165)*(x2170)))+(((x2167)*(x2168)))+(((x2165)*(x2168))))))) < IKFAST_ATAN2_MAGTHRESH )
continue;
j8array[0]=IKatan2(((gconst9)*(((((IkReal(-1.00000000000000))*(cj7)*(x2165)*(x2170)))+(((cj9)*(sj10)*(x2169)))+(((IkReal(-1.00000000000000))*(cj7)*(x2167)*(x2170)))+(((IkReal(-1.00000000000000))*(x2164)*(x2166)))+(((IkReal(-1.00000000000000))*(sj7)*(x2164)*(x2167)))+(((IkReal(-1.00000000000000))*(sj7)*(x2164)*(x2165)))))), ((gconst9)*(((((IkReal(-1.00000000000000))*(x2166)*(x2170)))+(((IkReal(-1.00000000000000))*(x2164)*(x2169)))+(((IkReal(-1.00000000000000))*(sj7)*(x2167)*(x2170)))+(((IkReal(-1.00000000000000))*(sj7)*(x2165)*(x2170)))+(((x2167)*(x2168)))+(((x2165)*(x2168)))))));
sj8array[0]=IKsin(j8array[0]);
cj8array[0]=IKcos(j8array[0]);
if( j8array[0] > IKPI )
{
j8array[0]-=IK2PI;
}
else if( j8array[0] < -IKPI )
{ j8array[0]+=IK2PI;
}
j8valid[0] = true;
for(int ij8 = 0; ij8 < 1; ++ij8)
{
if( !j8valid[ij8] )
{
continue;
}
_ij8[0] = ij8; _ij8[1] = -1;
for(int iij8 = ij8+1; iij8 < 1; ++iij8)
{
if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
{
j8valid[iij8]=false; _ij8[1] = iij8; break;
}
}
j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
{
IkReal evalcond[8];
IkReal x2171=IKsin(j8);
IkReal x2172=IKcos(j8);
IkReal x2173=((cj7)*(sj11));
IkReal x2174=((IkReal(0.0620002000000000))*(r21));
IkReal x2175=((IkReal(1.00000000000000))*(sj6));
IkReal x2176=((cj11)*(sj7));
IkReal x2177=((cj6)*(r00));
IkReal x2178=((IkReal(1.00000000000000))*(r20));
IkReal x2179=((cj6)*(r01));
IkReal x2180=((sj11)*(sj7));
IkReal x2181=((IkReal(1.00000000000000))*(cj7));
IkReal x2182=((IkReal(0.0620002000000000))*(r20));
IkReal x2183=((cj11)*(cj7));
IkReal x2184=((IkReal(1.00000000000000))*(sj7));
IkReal x2185=((cj6)*(r02));
IkReal x2186=((r10)*(sj6));
IkReal x2187=((cj7)*(sj6));
IkReal x2188=((IkReal(1.00000000000000))*(cj10));
IkReal x2189=((r11)*(sj6));
IkReal x2190=((cj6)*(px));
IkReal x2191=((sj9)*(x2172));
IkReal x2192=((cj9)*(x2172));
IkReal x2193=((cj9)*(x2171));
IkReal x2194=((sj9)*(x2171));
evalcond[0]=((((sj10)*(x2193)))+(((IkReal(-1.00000000000000))*(x2172)*(x2188)))+(((r12)*(x2187)))+(((IkReal(-1.00000000000000))*(r22)*(x2184)))+(((cj7)*(x2185))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x2184)*(x2185)))+(((IkReal(-1.00000000000000))*(r22)*(x2181)))+(((IkReal(-1.00000000000000))*(x2171)*(x2188)))+(((IkReal(-1.00000000000000))*(r12)*(sj7)*(x2175)))+(((IkReal(-1.00000000000000))*(sj10)*(x2192))));
evalcond[2]=((((x2180)*(x2189)))+(((IkReal(-1.00000000000000))*(r10)*(x2175)*(x2176)))+(((IkReal(-1.00000000000000))*(x2176)*(x2177)))+(((r21)*(x2173)))+(((IkReal(-1.00000000000000))*(x2178)*(x2183)))+(x2191)+(((x2179)*(x2180))));
evalcond[3]=((((x2177)*(x2183)))+(((IkReal(-1.00000000000000))*(x2173)*(x2179)))+(((IkReal(-1.00000000000000))*(x2194)))+(((r21)*(x2180)))+(((IkReal(-1.00000000000000))*(r11)*(x2173)*(x2175)))+(((x2183)*(x2186)))+(((IkReal(-1.00000000000000))*(x2176)*(x2178))));
evalcond[4]=((((x2177)*(x2180)))+(((IkReal(-1.00000000000000))*(x2188)*(x2192)))+(((sj10)*(x2171)))+(((r20)*(x2173)))+(((r21)*(x2183)))+(((x2176)*(x2189)))+(((x2176)*(x2179)))+(((x2180)*(x2186))));
evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x2173)*(x2175)))+(((cj10)*(x2193)))+(((IkReal(-1.00000000000000))*(cj11)*(x2179)*(x2181)))+(((sj10)*(x2172)))+(((IkReal(-1.00000000000000))*(r11)*(x2175)*(x2183)))+(((IkReal(-1.00000000000000))*(x2173)*(x2177)))+(((r20)*(x2180)))+(((r21)*(x2176))));
evalcond[6]=((((IkReal(0.0620002000000000))*(x2173)*(x2179)))+(((cj7)*(x2190)))+(((IkReal(-0.0620002000000000))*(x2177)*(x2183)))+(((IkReal(-1.00000000000000))*(x2174)*(x2180)))+(((IkReal(-0.00151567000000000))*(x2193)))+(((IkReal(-0.0620002000000000))*(x2183)*(x2186)))+(((IkReal(0.0620002000000000))*(x2173)*(x2189)))+(((IkReal(0.0616850000000000))*(x2194)))+(((py)*(x2187)))+(((x2176)*(x2182)))+(((IkReal(-1.00000000000000))*(pz)*(x2184)))+(((IkReal(-0.302000000000000))*(x2172)))+(((IkReal(-0.0701403000000000))*(x2171))));
evalcond[7]=((IkReal(0.270000000000000))+(((IkReal(-1.00000000000000))*(pz)*(x2181)))+(((IkReal(0.0701403000000000))*(x2172)))+(((IkReal(-0.0616850000000000))*(x2191)))+(((IkReal(-0.0620002000000000))*(x2179)*(x2180)))+(((IkReal(-0.0620002000000000))*(x2180)*(x2189)))+(((IkReal(0.00151567000000000))*(x2192)))+(((IkReal(0.0620002000000000))*(x2176)*(x2177)))+(((x2182)*(x2183)))+(((IkReal(-1.00000000000000))*(py)*(sj7)*(x2175)))+(((IkReal(-1.00000000000000))*(x2173)*(x2174)))+(((IkReal(0.0620002000000000))*(x2176)*(x2186)))+(((IkReal(-0.302000000000000))*(x2171)))+(((IkReal(-1.00000000000000))*(x2184)*(x2190))));
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;
}
}
{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j6;
vinfos[0].indices[0] = _ij6[0];
vinfos[0].indices[1] = _ij6[1];
vinfos[0].maxsolutions = _nj6;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j7;
vinfos[1].indices[0] = _ij7[0];
vinfos[1].indices[1] = _ij7[1];
vinfos[1].maxsolutions = _nj7;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j8;
vinfos[2].indices[0] = _ij8[0];
vinfos[2].indices[1] = _ij8[1];
vinfos[2].maxsolutions = _nj8;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j9;
vinfos[3].indices[0] = _ij9[0];
vinfos[3].indices[1] = _ij9[1];
vinfos[3].maxsolutions = _nj9;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j10;
vinfos[4].indices[0] = _ij10[0];
vinfos[4].indices[1] = _ij10[1];
vinfos[4].maxsolutions = _nj10;
vinfos[5].jointtype = 1;
vinfos[5].foffset = j11;
vinfos[5].indices[0] = _ij11[0];
vinfos[5].indices[1] = _ij11[1];
vinfos[5].maxsolutions = _nj11;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}
}
}
}
}
}
}
}
}
return solutions.GetNumSolutions()>0;
}
/// \brief Solve the det Ax^2+Bx+C = 0 problem using the Manocha and Canny method (1994)
///
/// matcoeffs is of length 54*3, for 3 matrices
static inline void solvedialyticpoly12qep(const IkReal* matcoeffs, IkReal* rawroots, int& numroots)
{
const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
IkReal IKFAST_ALIGNED16(M[24*24]) = {0};
IkReal IKFAST_ALIGNED16(A[12*12]);
IkReal IKFAST_ALIGNED16(work[24*24*23]);
int ipiv[12];
int info, coeffindex;
const int worksize=24*24*23;
const int matrixdim = 12;
const int matrixdim2 = 24;
numroots = 0;
// first setup M = [0 I; -C -B] and A
coeffindex = 0;
for(int j = 0; j < 6; ++j) {
for(int k = 0; k < 9; ++k) {
M[matrixdim+(j+6)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+3)] = -matcoeffs[coeffindex++];
}
}
for(int j = 0; j < 6; ++j) {
for(int k = 0; k < 9; ++k) {
M[matrixdim+(j+6)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+3)+matrixdim*2*matrixdim] = -matcoeffs[coeffindex++];
}
}
for(int j = 0; j < 6; ++j) {
for(int k = 0; k < 9; ++k) {
A[(j+6)+matrixdim*k] = A[j+matrixdim*(k+3)] = matcoeffs[coeffindex++];
}
for(int k = 0; k < 3; ++k) {
A[j+matrixdim*k] = A[(j+6)+matrixdim*(k+9)] = 0;
}
}
const IkReal lfpossibilities[4][4] = {{1,-1,1,1},{1,0,-2,1},{1,1,2,0},{1,-1,4,1}};
int lfindex = -1;
bool bsingular = true;
do {
dgetrf_(&matrixdim,&matrixdim,A,&matrixdim,&ipiv[0],&info);
if( info == 0 ) {
bsingular = false;
for(int j = 0; j < matrixdim; ++j) {
if( IKabs(A[j*matrixdim+j]) < 100*tol ) {
bsingular = true;
break;
}
}
if( !bsingular ) {
break;
}
}
if( lfindex == 3 ) {
break;
}
// transform by the linear functional
lfindex++;
const IkReal* lf = lfpossibilities[lfindex];
// have to reinitialize A
coeffindex = 0;
for(int j = 0; j < 6; ++j) {
for(int k = 0; k < 9; ++k) {
IkReal a = matcoeffs[coeffindex+108], b = matcoeffs[coeffindex+54], c = matcoeffs[coeffindex];
A[(j+6)+matrixdim*k] = A[j+matrixdim*(k+3)] = lf[0]*lf[0]*a+lf[0]*lf[2]*b+lf[2]*lf[2]*c;
M[matrixdim+(j+6)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+3)] = -(lf[1]*lf[1]*a + lf[1]*lf[3]*b + lf[3]*lf[3]*c);
M[matrixdim+(j+6)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+3)+matrixdim*2*matrixdim] = -(2*lf[0]*lf[1]*a + (lf[0]*lf[3]+lf[1]*lf[2])*b + 2*lf[2]*lf[3]*c);
coeffindex++;
}
for(int k = 0; k < 3; ++k) {
A[j+matrixdim*k] = A[(j+6)+matrixdim*(k+9)] = 0;
}
}
} while(lfindex<4);
if( bsingular ) {
return;
}
dgetrs_("No transpose", &matrixdim, &matrixdim2, A, &matrixdim, &ipiv[0], &M[matrixdim], &matrixdim2, &info);
if( info != 0 ) {
return;
}
// set identity in upper corner
for(int j = 0; j < matrixdim; ++j) {
M[matrixdim*2*matrixdim+j+matrixdim*2*j] = 1;
}
IkReal IKFAST_ALIGNED16(wr[24]);
IkReal IKFAST_ALIGNED16(wi[24]);
IkReal IKFAST_ALIGNED16(vr[24*24]);
int one=1;
dgeev_("N", "V", &matrixdim2, M, &matrixdim2, wr, wi,NULL, &one, vr, &matrixdim2, work, &worksize, &info);
if( info != 0 ) {
return;
}
IkReal Breal[matrixdim-1];
for(int i = 0; i < matrixdim2; ++i) {
if( IKabs(wi[i]) < tol*100 ) {
IkReal* ev = vr+matrixdim2*i;
if( IKabs(wr[i]) > 1 ) {
ev += matrixdim;
}
// consistency has to be checked!!
if( IKabs(ev[0]) < tol ) {
continue;
}
IkReal iconst = 1/ev[0];
for(int j = 1; j < matrixdim; ++j) {
Breal[j-1] = ev[j]*iconst;
}
if( checkconsistency12(Breal) ) {
if( lfindex >= 0 ) {
const IkReal* lf = lfpossibilities[lfindex];
rawroots[numroots++] = (wr[i]*lf[0]+lf[1])/(wr[i]*lf[2]+lf[3]);
}
else {
rawroots[numroots++] = wr[i];
}
bool bsmall0=IKabs(ev[0]) > IKabs(ev[3]);
bool bsmall1=IKabs(ev[0]) > IKabs(ev[1]);
if( bsmall0 && bsmall1 ) {
rawroots[numroots++] = ev[3]/ev[0];
rawroots[numroots++] = ev[1]/ev[0];
}
else if( bsmall0 && !bsmall1 ) {
rawroots[numroots++] = ev[5]/ev[2];
rawroots[numroots++] = ev[2]/ev[1];
}
else if( !bsmall0 && bsmall1 ) {
rawroots[numroots++] = ev[9]/ev[6];
rawroots[numroots++] = ev[10]/ev[9];
}
else if( !bsmall0 && !bsmall1 ) {
rawroots[numroots++] = ev[11]/ev[8];
rawroots[numroots++] = ev[11]/ev[10];
}
}
}
}
}
static inline bool checkconsistency12(const IkReal* Breal)
{
IkReal norm = 0.1;
for(int i = 0; i < 11; ++i) {
norm += IKabs(Breal[i]);
}
IkReal tol = 1e-6*norm; // have to increase the threshold since many computations are involved
return IKabs(Breal[0]*Breal[0]-Breal[1]) < tol && IKabs(Breal[0]*Breal[2]-Breal[3]) < tol && IKabs(Breal[1]*Breal[2]-Breal[4]) < tol && IKabs(Breal[2]*Breal[2]-Breal[5]) < tol && IKabs(Breal[0]*Breal[5]-Breal[6]) < tol && IKabs(Breal[1]*Breal[5]-Breal[7]) < tol && IKabs(Breal[2]*Breal[5]-Breal[8]) < tol && IKabs(Breal[0]*Breal[8]-Breal[9]) < tol && IKabs(Breal[1]*Breal[8]-Breal[10]) < tol;
}
};
/// solves the inverse kinematics equations.
/// \param pfree is an array specifying the free joints of the chain.
IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
IKSolver solver;
return solver.ComputeIk(eetrans,eerot,pfree,solutions);
}
IKFAST_API const char* GetKinematicsHash() { return "<robot:genericrobot - abby (5288f61bbe95d46f4c49134ed1c59e1d)>"; }
IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
#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;
}
IkSolutionList<IkReal> solutions;
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 = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
if( !bSuccess ) {
fprintf(stderr,"Failed to get ik solution\n");
return -1;
}
printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
std::vector<IkReal> solvalues(GetNumJoints());
for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
std::vector<IkReal> vsolfree(sol.GetFree().size());
sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
for( std::size_t j = 0; j < solvalues.size(); ++j)
printf("%.15f, ", solvalues[j]);
printf("\n");
}
return 0;
}
#endif
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment