void IOpenGLDrawable.Draw()
{
// Draw the tool location as a cone
Vector3 position = robot.GetPosition();
GL.Color3(Color.Silver);
Polyhedra.DrawCone(position + new Vector3(0, 0, router.ToolDiameter), position, router.ToolDiameter / 2.0f);
Vector3 physicalPosition = robot.GetPhysicalPosition();
GL.Color3(Color.Black);
Polyhedra.DrawCone(physicalPosition + new Vector3(0, 0, router.ToolDiameter), physicalPosition, router.ToolDiameter / 2.0f);
//// Draw the past positions & velocity graph
//float lastTime = 0;
//Vector3 lastPos = new Vector3(0, 0, 0);
//float lastVel = 0;
//bool lastIsGood = false;
//GL.Disable(EnableCap.Lighting);
//lock (previousPoints)
//{
// Vector3 lastpoint = new Vector3(0, 0, 0);
// for (int i = 0; i < previousPoints.Count(); i++)
// {
// PreviousPoint point = previousPoints[i];
// float age_delta = point.createTime - lastTime;
// float time = age_delta / 1000.0f; // Age is microseconds, time is seconds
// float pos_delta = (point.location - lastPos).Length;
// float vel = pos_delta / time; // Inches per second
// Vector3 atpoint = new Vector3(point.location.X * 1000, point.location.Y * 1000, point.location.Z * 1000);
// if (lastIsGood)
// {
// GL.LineWidth(1);
// GL.Begin(PrimitiveType.Lines);
// GL.Color3(Color.LightGray);
// for (int j = 0; j < 5; j++)
// {
// GL.Vertex3(lastpoint + new Vector3(0, 0, j * 10));
// GL.Vertex3(lastpoint + new Vector3(0, 0, j * 10 + 10));
//
// GL.Vertex3(lastpoint + new Vector3(0, 0, j * 10));
// GL.Vertex3(atpoint + new Vector3(0, 0, j * 10));
// }
// GL.End();
// GL.LineWidth(2);
// GL.Begin(PrimitiveType.Lines);
// GL.Color3(Color.Orange);
// GL.Vertex3(lastpoint + new Vector3(0, 0, lastVel * lastVel * 200));
// GL.Vertex3(atpoint + new Vector3(0, 0, vel * vel * 200));
// GL.End();
// }
// lastVel = vel;
// lastpoint = atpoint;
//
// lastPos = point.location;
// lastTime = point.createTime;
// lastIsGood = true;
// }
//}
//GL.Enable(EnableCap.Lighting);
//GL.LineWidth(1);
}
