void plot_points(std::vector<v3d> &points, const v3d &color){
glColor4d(color.r(), color.g(), color.b(), 1.0);
glBegin(GL_POINTS);
for (unsigned i=0; i<points.size(); i++){
glVertex3d(points.at(i).x(), points.at(i).y(), points.at(i).z());
}
glEnd();
SDL_GL_SwapBuffers();
}
void plot_track(std::vector<v3d> &points, const v3d &color){
glColor4d(color.r(), color.g(), color.b(), 0.4);
glBegin(GL_LINES);
unsigned i;
glVertex3d(points.at(0).x(), points.at(0).y(), points.at(0).z()+1);
for (i=1; i<points.size(); i++){
glVertex3d(points.at(i).x(), points.at(i).y(), points.at(i).z()+1);
glVertex3d(points.at(i).x(), points.at(i).y(), points.at(i).z()+1);
}
glVertex3d(points.at(i-1).x(), points.at(i-1).y(), points.at(i-1).z()+1);
glEnd();
SDL_GL_SwapBuffers();
}
