void GLThread::DrawDownvecAtInd(int ind)
{
glDisable(GL_TEXTURE_2D);
glPushMatrix();
glColor3d(0.8, 0.1, 0.0);
const double radius = 0.20;
const double radius_cone_max = 0.45;
Vector3d diff_pos = _thread->vertex_at_ind(ind)-display_start_pos;
double length_factor = 8.0;
double cone_scale_factor = 0.07;
Vector3d down_vec = -Vector3d::UnitZ()*length_factor;
double pts[4][3];
double pts_cone[4][3];
double radius_cone[4];
radius_cone[0] = radius_cone_max*2;
radius_cone[1] = radius_cone_max;
radius_cone[2] = 1e-3;
radius_cone[3] = 0;
for (int i=0; i < 3; i++)
{
pts[0][i] = diff_pos(i)-down_vec(i);
pts[1][i] = diff_pos(i);
pts[2][i] = diff_pos(i) + down_vec(i);
pts[3][i] = diff_pos(i) + 2*down_vec(i);
pts_cone[0][i] = diff_pos(i) + down_vec(i)*(1.0-cone_scale_factor);
pts_cone[1][i] = diff_pos(i) + down_vec(i);
pts_cone[2][i] = diff_pos(i) + down_vec(i)*(1.0+cone_scale_factor);
pts_cone[3][i] = diff_pos(i) + down_vec(i)*(1.0+cone_scale_factor*2);
}
glePolyCylinder(4, pts, 0x0,radius);
glePolyCone(4, pts_cone, 0x0, radius_cone);
glPopMatrix();
glEnable(GL_TEXTURE_2D);
}
double footstepHeuristicStepCost(
SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph,
double first_rotation_weight,
double second_rotation_weight)
{
FootstepState::Ptr state = node->getState();
FootstepState::Ptr goal = graph->getGoal(state->getLeg());
Eigen::Vector3f goal_pos(goal->getPose().translation());
Eigen::Vector3f diff_pos(goal_pos - state->getPose().translation());
diff_pos[2] = 0.0; // Ignore z distance
Eigen::Quaternionf first_rot;
// Eigen::Affine3f::rotation is too slow because it calls SVD decomposition
first_rot.setFromTwoVectors(state->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX(),
diff_pos.normalized());
Eigen::Quaternionf second_rot;
second_rot.setFromTwoVectors(diff_pos.normalized(),
goal->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX());
// is it correct??
double first_theta = acos(first_rot.w()) * 2;
double second_theta = acos(second_rot.w()) * 2;
if (isnan(first_theta)) {
first_theta = 0;
}
if (isnan(second_theta)) {
second_theta = 0;
}
// acos := [0, M_PI]
if (first_theta > M_PI) {
first_theta = 2.0 * M_PI - first_theta;
}
if (second_theta > M_PI) {
second_theta = 2.0 * M_PI - second_theta;
}
//return (Eigen::Vector2f(diff_pos[0], diff_pos[1]).norm() / graph->maxSuccessorDistance()) + std::abs(diff_pos[2]) / 0.2 +
return (diff_pos.norm() / graph->maxSuccessorDistance()) + std::abs(diff_pos[2]) / 0.2 +
(first_theta * first_rotation_weight + second_theta * second_rotation_weight) / graph->maxSuccessorRotation();
}
/* draw the helix shape */
void DrawStuff (void)
{
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glColor3f (0.8, 0.3, 0.6);
glPushMatrix ();
/* set up some matrices so that the object spins with the mouse */
glTranslatef (0.0,0.0,-150.0);
glRotatef (rotate_frame[1], 1.0, 0.0, 0.0);
glRotatef (rotate_frame[0], 0.0, 0.0, 1.0);
//change thread, if necessary
if (move_end[0] != 0.0 || move_end[1] != 0.0 || tangent_end[0] != 0.0 || tangent_end[1] != 0.0 || tangent_rotation_end[0] != 0 || tangent_rotation_end[1] != 0)
{
GLdouble model_view[16];
glGetDoublev(GL_MODELVIEW_MATRIX, model_view);
GLdouble projection[16];
glGetDoublev(GL_PROJECTION_MATRIX, projection);
GLint viewport[4];
glGetIntegerv(GL_VIEWPORT, viewport);
double winX, winY, winZ;
//change end positions
Vector3d new_end_pos;
gluProject(positions[1](0), positions[1](1), positions[1](2), model_view, projection, viewport, &winX, &winY, &winZ);
winX += move_end[0];
winY += move_end[1];
move_end[0] = 0.0;
move_end[1] = 0.0;
gluUnProject(winX, winY, winZ, model_view, projection, viewport, &new_end_pos(0), &new_end_pos(1), &new_end_pos(2));
// std::cout << "X: " << positions[1](0) << " Y: " << positions[1](1) << " Z: " << positions[1](2) << std::endl;
//change tangents
Vector3d new_end_tan;
gluProject(positions[1](0)+tangents[1](0),positions[1](1)+tangents[1](1), positions[1](2)+tangents[1](2), model_view, projection, viewport, &winX, &winY, &winZ);
winX += tangent_end[0];
winY += tangent_end[1];
tangent_end[0] = 0.0;
tangent_end[1] = 0.0;
gluUnProject(winX, winY, winZ, model_view, projection, viewport, &new_end_tan(0), &new_end_tan(1), &new_end_tan(2));
new_end_tan -= positions[1];
new_end_tan.normalize();
positions[1] = new_end_pos;
Matrix3d rotation_new_tan;
rotate_between_tangents(tangents[1], new_end_tan, rotation_new_tan);
rotations[1] = rotation_new_tan*rotations[1];
//check rotation around tangent
Vector3d tangent_normal_rotate_around = rotations[1].col(1);
Vector3d new_end_tan_normal;
gluProject(positions[1](0)+tangent_normal_rotate_around(0), positions[1](1)+tangent_normal_rotate_around(1), positions[1](2)+tangent_normal_rotate_around(2), model_view, projection, viewport, &winX, &winY, &winZ);
winX += tangent_rotation_end[0];
winY += tangent_rotation_end[1];
tangent_rotation_end[0] = 0.0;
tangent_rotation_end[1] = 0.0;
gluUnProject(winX, winY, winZ, model_view, projection, viewport, &new_end_tan_normal(0), &new_end_tan_normal(1), &new_end_tan_normal(2));
new_end_tan_normal -= positions[1];
//project this normal onto the plane normal to X (to ensure Y stays normal to X)
new_end_tan_normal -= new_end_tan_normal.dot(rotations[1].col(0))*rotations[1].col(0);
new_end_tan_normal.normalize();
rotations[1].col(1) = new_end_tan_normal;
rotations[1].col(2) = rotations[1].col(0).cross(new_end_tan_normal);
//change thread
thread->set_constraints(positions[0], rotations[0], positions[1], rotations[1]);
//thread->set_end_constraint(positions[1], rotations[1]);
//std::cout <<"CONSTRAINT END:\n" << rotations[1] << std::endl;
thread->minimize_energy();
updateThreadPoints();
findThreadInIms();
//std::cout <<"ACTUAL END:\n" << thread->end_rot() << std::endl;
//std::cout << "objX: " <<objX << " objY: " << objY << " objZ: " << objZ << " winX: " << winX << " winY: " << winY << " winZ: " << winZ << std::endl;
}
//Draw Axes
glBegin(GL_LINES);
glEnable(GL_LINE_SMOOTH);
glColor3d(1.0, 0.0, 0.0); //red
glVertex3f(0.0f, 0.0f, 0.0f); //x
glVertex3f(10.0f, 0.0f, 0.0f);
glColor3d(0.0, 1.0, 0.0); //green
glVertex3f(0.0f, 0.0f, 0.0f); //y
glVertex3f(0.0f, 10.0f, 0.0f);
glColor3d(0.0, 0.0, 1.0); //blue
glVertex3f(0.0f, 0.0f, 0.0f); //z
glVertex3f(0.0f, 0.0f, 10.0f);
/* glColor3d(0.5, 0.5, 1.0);
glVertex3f(positions[1](0)-positions[0](0), positions[1](1)-positions[0](1), positions[1](2)-positions[0](2)); //z
glVertex3f(positions[1](0)-positions[0](0)+tangents[1](0)*4.0, positions[1](1)-positions[0](1)+tangents[1](1)*4.0, positions[1](2)-positions[0](2)+tangents[1](2)*4.0); //z
*/
//Draw Axes at End
Vector3d diff_pos = positions[1]-positions[0];
double rotation_scale_factor = 10.0;
Matrix3d rotations_project = rotations[1]*rotation_scale_factor;
glBegin(GL_LINES);
glEnable(GL_LINE_SMOOTH);
glColor3d(1.0, 0.0, 0.0); //red
glVertex3f((float)diff_pos(0), (float)diff_pos(1), (float)diff_pos(2)); //x
glVertex3f((float)(diff_pos(0)+rotations_project(0,0)), (float)(diff_pos(1)+rotations_project(1,0)), (float)(diff_pos(2)+rotations_project(2,0)));
glColor3d(0.0, 1.0, 0.0); //green
glVertex3f((float)diff_pos(0), (float)diff_pos(1), (float)diff_pos(2)); //y
glVertex3f((float)(diff_pos(0)+rotations_project(0,1)), (float)(diff_pos(1)+rotations_project(1,1)), (float)(diff_pos(2)+rotations_project(2,1)));
glColor3d(0.0, 0.0, 1.0); //blue
glVertex3f((float)diff_pos(0), (float)diff_pos(1), (float)diff_pos(2)); //z
glVertex3f((float)(diff_pos(0)+rotations_project(0,2)), (float)(diff_pos(1)+rotations_project(1,2)), (float)(diff_pos(2)+rotations_project(2,2)));
glEnd( );
//label axes
void * font = GLUT_BITMAP_HELVETICA_18;
glColor3d(1.0, 0.0, 0.0); //red
glRasterPos3i(20.0, 0.0, -1.0);
glutBitmapCharacter(font, 'X');
glColor3d(0.0, 1.0, 0.0); //red
glRasterPos3i(0.0, 20.0, -1.0);
glutBitmapCharacter(font, 'Y');
glColor3d(0.0, 0.0, 1.0); //red
glRasterPos3i(-1.0, 0.0, 20.0);
glutBitmapCharacter(font, 'Z');
addThreadDebugInfo();
//Draw Thread
glColor4f (0.5, 0.5, 0.2, 0.5);
/*
double pts_cpy[points.size()][3];
double twist_cpy[points.size()];
pts_cpy[0][0] = 0.0;
pts_cpy[0][1] = 0.0;
pts_cpy[0][2] = 0.0;
twist_cpy[0] = -(360.0/(2.0*M_PI))*twist_angles[0];
//std::cout << twist_cpy[0] << std::endl;
for (int i=1; i < points.size(); i++)
{
pts_cpy[i][0] = points[i](0)-(double)positions[0](0);
pts_cpy[i][1] = points[i](1)-(double)positions[0](1);
pts_cpy[i][2] = points[i](2)-(double)positions[0](2);
twist_cpy[i] = -(360.0/(2.0*M_PI))*twist_angles[i];
//std::cout << twist_cpy[i] << std::endl;
}
gleTwistExtrusion(20,
contour,
contour_norms,
NULL,
points.size(),
pts_cpy,
0x0,
twist_cpy);
*/
double pts_cpy[points.size()+2][3];
double twist_cpy[points.size()+2];
pts_cpy[1][0] = 0.0;
pts_cpy[1][1] = 0.0;
pts_cpy[1][2] = 0.0;
twist_cpy[1] = -(360.0/(2.0*M_PI))*twist_angles[0];
//std::cout << twist_cpy[0] << std::endl;
for (int i=1; i < points.size()-1; i++)
{
pts_cpy[i+1][0] = points[i](0)-(double)positions[0](0);
pts_cpy[i+1][1] = points[i](1)-(double)positions[0](1);
pts_cpy[i+1][2] = points[i](2)-(double)positions[0](2);
twist_cpy[i+1] = -(360.0/(2.0*M_PI))*twist_angles[i];
//std::cout << twist_cpy[i+1] - twist_cpy[i] << std::endl;
}
//add first and last point
pts_cpy[0][0] = -rotations[0](0,0);
pts_cpy[0][1] = -rotations[0](1,0);
pts_cpy[0][2] = -rotations[0](2,0);
twist_cpy[0] = -(360.0/(2.0*M_PI))*twist_angles[0];
pts_cpy[points.size()][0] = (double)positions[1](0)-(double)positions[0](0);
pts_cpy[points.size()][1] = (double)positions[1](1)-(double)positions[0](1);
pts_cpy[points.size()][2] = (double)positions[1](2)-(double)positions[0](2);
twist_cpy[points.size()] = twist_cpy[points.size()-1];
pts_cpy[points.size()+1][0] = pts_cpy[points.size()][0]+rotations[1](0,0);
pts_cpy[points.size()+1][1] = pts_cpy[points.size()][1]+rotations[1](1,0);
pts_cpy[points.size()+1][2] = pts_cpy[points.size()][2]+rotations[1](2,0);
twist_cpy[points.size()+1] = twist_cpy[points.size()];
gleTwistExtrusion_c4f(20,
contour,
contour_norms,
NULL,
points.size()+2,
pts_cpy,
0x0,
twist_cpy);
glPopMatrix ();
glutSwapBuffers ();
thread_vision.display();
}
void GLThread::DrawAxesAtPoint(const Vector3d& pt, const Matrix3d& rot_in, ColorCode color_axis, bool skipX)
{
glDisable(GL_TEXTURE_2D);
glPushMatrix();
//Matrix3d rot = Eigen::AngleAxisd(M_PI/2.0, Vector3d::UnitX())*rot_in;
Matrix3d rot = Eigen::AngleAxisd(M_PI/2.0, rot_in.col(0))*rot_in;
const double radius = 0.15;
const double radius_cone_max = 0.4;
Vector3d diff_pos = pt-display_start_pos;
double rotation_scale_factor = 6.0;
double cone_scale_factor = 0.07;
Matrix3d rotations_project = rot*rotation_scale_factor;
double pts[4][3];
double pts_cone[4][3];
double radius_cone[4];
radius_cone[0] = radius_cone_max*2;
radius_cone[1] = radius_cone_max;
radius_cone[2] = 1e-3;
radius_cone[3] = 0;
for (int i=0; i < 3; i++)
{
pts[0][i] = diff_pos(i)-rotations_project(i,0);
pts[1][i] = diff_pos(i);
}
for (int coord=0; coord < 3; coord++)
{
if (coord == 0 && skipX)
continue;
for (int i=0; i < 3; i++)
{
pts[2][i] = diff_pos(i) + rotations_project(i,coord);
pts[3][i] = diff_pos(i) + 2*rotations_project(i,coord);
pts_cone[0][i] = diff_pos(i) + rotations_project(i,coord)*(1.0-cone_scale_factor);
pts_cone[1][i] = diff_pos(i) + rotations_project(i,coord);
pts_cone[2][i] = diff_pos(i) + rotations_project(i,coord)*(1.0+cone_scale_factor);
pts_cone[3][i] = diff_pos(i) + rotations_project(i,coord)*(1.0+cone_scale_factor*2);
}
float thread_color_array[4][3];
float stripe_color_array[4][3];
for (int i=0; i < 4; i++)
{
for (int j=0; j < 3; j++)
{
thread_color_array[i][j] = thread_color_float[j];
stripe_color_array[i][j] = stripe_color_float[j];
}
}
const float alpha = 0.85;
// gleTextureMode (GLE_TEXTURE_VERTEX_MODEL_CYL);
if (color_axis == material)
{
if (coord == 0)
glColor4f(0.7, 0.2, 0, alpha);
else if (coord == 1)
glColor4f((thread_color_float[0])*0.8, (thread_color_float[1])*0.8, (thread_color_float[2])*0.8, alpha);
else
glColor4f((stripe_color_float[0])*0.8, (stripe_color_float[1])*0.8, (stripe_color_float[2])*0.8, alpha);
} else if (color_axis == bishop) {
if (coord == 0)
glColor4f(0.7, 0.2, 0, alpha);
else if (coord == 1)
glColor3d(0.5, 0.5, 0.5);
else
glColor3d(0.17, 0.17, 0.17);
} else {
if (coord == 0)
glColor3d(1.0, 0.0, 0.0);
else if (coord == 1)
glColor3d(0.0, 1.0, 0.0);
else
glColor3d(0.0, 0.0, 1.0);
}
glePolyCylinder(4, pts, 0x0,radius);
glePolyCone(4, pts_cone, 0x0, radius_cone);
}
glPopMatrix();
glEnable(GL_TEXTURE_2D);
}
void GLThread::DrawAxes()
{
//Draw Axes at Start
Vector3d diff_pos = positions[0]-display_start_pos;
double rotation_scale_factor = 10.0;
Matrix3d rotations_project = rotations[0]*rotation_scale_factor;
glBegin(GL_LINES);
glEnable(GL_LINE_SMOOTH);
glColor3d(1.0, 0.0, 0.0); //red
glVertex3f((float)diff_pos(0), (float)diff_pos(1), (float)diff_pos(2)); //x
glVertex3f((float)(diff_pos(0)+rotations_project(0,0)), (float)(diff_pos(1)+rotations_project(1,0)), (float)(diff_pos(2)+rotations_project(2,0)));
glColor3d(0.0, 1.0, 0.0); //green
glVertex3f((float)diff_pos(0), (float)diff_pos(1), (float)diff_pos(2)); //y
glVertex3f((float)(diff_pos(0)+rotations_project(0,1)), (float)(diff_pos(1)+rotations_project(1,1)), (float)(diff_pos(2)+rotations_project(2,1)));
glColor3d(0.0, 0.0, 1.0); //blue
glVertex3f((float)diff_pos(0), (float)diff_pos(1), (float)diff_pos(2)); //z
glVertex3f((float)(diff_pos(0)+rotations_project(0,2)), (float)(diff_pos(1)+rotations_project(1,2)), (float)(diff_pos(2)+rotations_project(2,2)));
glEnd();
//Draw Axes at End
diff_pos = positions[1]-display_start_pos;
rotation_scale_factor = 10.0;
rotations_project = rotations[1]*rotation_scale_factor;
glBegin(GL_LINES);
glEnable(GL_LINE_SMOOTH);
glColor3d(1.0, 0.0, 0.0); //red
glVertex3f((float)diff_pos(0), (float)diff_pos(1), (float)diff_pos(2)); //x
glVertex3f((float)(diff_pos(0)+rotations_project(0,0)), (float)(diff_pos(1)+rotations_project(1,0)), (float)(diff_pos(2)+rotations_project(2,0)));
glColor3d(0.0, 1.0, 0.0); //green
glVertex3f((float)diff_pos(0), (float)diff_pos(1), (float)diff_pos(2)); //y
glVertex3f((float)(diff_pos(0)+rotations_project(0,1)), (float)(diff_pos(1)+rotations_project(1,1)), (float)(diff_pos(2)+rotations_project(2,1)));
glColor3d(0.0, 0.0, 1.0); //blue
glVertex3f((float)diff_pos(0), (float)diff_pos(1), (float)diff_pos(2)); //z
glVertex3f((float)(diff_pos(0)+rotations_project(0,2)), (float)(diff_pos(1)+rotations_project(1,2)), (float)(diff_pos(2)+rotations_project(2,2)));
glEnd();
}
