void renderIK() {
//Eigen::Vector3d goal = interpolateGoal();
for (int i = 0; i < appendages.size(); i++) {
test.solveIK(appendages[i], goals[i]);
if (octopus) {
glPushMatrix();
glTranslatef(cos((2*PI/goals.size())*i), sin((2*PI/goals.size())*i), 0);
}
glColor3f(0, 1, 1);
renderCylinder_convenient(0, 0, 0, appendages[i][0].currPos[0], appendages[i][0].currPos[1], appendages[i][0].currPos[2], 0.04, 10);
glPushMatrix();
glTranslatef(appendages[i][0].currPos[0], appendages[i][0].currPos[1], appendages[i][0].currPos[2]);
glutSolidSphere(0.1, 10, 10);
glPopMatrix();
for (int j=1; j<appendages[i].size(); j++) {
if (j%2==0) {
glColor3f(0, 1, 1);
} else {
glColor3f(1, 0, 1);
}
renderCylinder_convenient(appendages[i][j-1].currPos[0], appendages[i][j-1].currPos[1], appendages[i][j-1].currPos[2], appendages[i][j].currPos[0], appendages[i][j].currPos[1], appendages[i][j].currPos[2], 0.04, 5);
glPushMatrix();
glTranslatef(appendages[i][j].currPos[0], appendages[i][j].currPos[1], appendages[i][j].currPos[2]);
glutSolidSphere(0.1, 10, 10);
glPopMatrix();
}
glBegin(GL_LINES);
glColor3f(1, 1, 0);
glVertex3f(0,0,0);
glVertex3f(goals[i][0],goals[i][1],goals[i][2]);
glEnd();
if (octopus) glPopMatrix();
}
}
