double Trajectory2d::calculateCurvature(const Position2d& p1,
const Position2d& p2,
const Position2d& p3,
bool backward) const
{
// |b-a| |c-a| |b-c|
// r = ------------------
// | bx-ax by-ay |
// 2 | cx-ax cy-ay |
const double numerator = std::sqrt((p2-p1).squaredNorm()
* (p3-p1).squaredNorm()
* (p2-p3).squaredNorm());
const double denominator = 2.0 * (((p2.x()-p1.x())*(p3.y()-p1.y()))
- ((p3.x()-p1.x())*(p2.y()-p1.y())));
const double curvature = denominator / numerator;
return backward ? -curvature : curvature;
}
double Trajectory2d::squaredLineDistance(const Position2d &p1, const Position2d &p2, const Position2d &p3)
{
//inspired by the pSimpl lib http://psimpl.sourceforge.net/
//make a vector of the line
Position2d line = p2-p1;
//make a vector to the test point from p1
Position2d testPoint = p3-p1;
//projection on the line
double dotLineTestPoint = line.x()*testPoint.x()+line.y()*testPoint.y();
//line is before p1
if(dotLineTestPoint<=0 )
{
return (p1.x()-p3.x())*(p1.x()-p3.x())+(p1.y()-p3.y())*(p1.y()-p3.y());
}
//calculate squared length
double lengthLine = line.x()*line.x()+line.y()*line.y();
// dotLineTestPoint/lengthLine = relative point between p1 and p2
//projection is outside the line defined by p1 and p2 (after p2)
if(lengthLine<dotLineTestPoint)
{
//return distance to p2
return (p2.x()-p3.x())*(p2.x()-p3.x())+(p2.y()-p3.y())*(p2.y()-p3.y());
}
//the projection is between p1 and p2
//calculate projection point
Position2d projectionPoint = (line*(dotLineTestPoint/lengthLine));
//return distance between projection point and test point
return (projectionPoint.x()-testPoint.x())*(projectionPoint.x()-testPoint.x())+(projectionPoint.y()-testPoint.y())*(projectionPoint.y()-testPoint.y());
}
void Trajectory2d::toGnuplot(const std::string& filename_without_suffix) const
{
const std::string gnuplot_filename = filename_without_suffix + ".gpl";
const std::string pose_filename = filename_without_suffix + ".gpldata";
const std::string curvature_filename = filename_without_suffix + "_curve.gpldata";
// -- data formatting --
std::ofstream pose_file(pose_filename.c_str());
pose_file << "# Trajectory data set containing "<< m_trajectory.size() << " sets of position and orientation information." << std::endl;
pose_file << "# To draw the poses the orientation is coded as two vectors in gnuplot from-delta style." << std::endl;
pose_file << "# from: x y | delta: x y" << std::endl;
pose_file << "# "<< std::endl;
pose_file << "# "<< std::endl;
pose_file << "# The trajectory is for "
<< (this->isForwardTrajectory() ? "FORWARD" : "BACKWARD")
<< " motion." << std::endl;
for (std::size_t i=0; i<m_trajectory.size(); ++i)
{
const Position2d from(m_trajectory[i].getPosition());
const Position2d delta = m_trajectory[i].getOrientation() * Position2d::UnitX() * 0.02;
pose_file << from.x() << " " << from.y() << " ";
if (velocityAvailable())
{
pose_file << m_trajectory[i].getVelocity() << " ";
}
pose_file << delta.x() << " " << delta.y() << " ";
if (velocityAvailable())
{
pose_file << 0.;
}
pose_file << std::endl;
}
pose_file << std::endl;
// curvature plots
if (curvatureAvailable())
{
std::ofstream curve_file(curvature_filename.c_str());
for (std::size_t i=0; i<m_trajectory.size(); ++i)
{
// start for curvature lines
const Position2d from(m_trajectory[i].getPosition());
// end point for curvature lines
const double curvature = m_trajectory[i].getCurvature() * 0.5; // apply scale for better visualization
const double yaw = m_trajectory[i].getYaw();
Position2d to;
to.x() = from.x() - std::sin(yaw) * curvature;
to.y() = from.y() + std::cos(yaw) * curvature;
curve_file << from.x() << " " << from.y();
if (velocityAvailable())
{
curve_file << " 0.";
}
curve_file << std::endl;
curve_file << to.x() << " " << to.y();
if (velocityAvailable())
{
curve_file << " 0.";
}
curve_file << std::endl << std::endl << std::endl; // apply empty line to separate plot
}
curve_file.close();
}
pose_file.close();
// -- the plot itself --
std::ofstream gnuplot_file(gnuplot_filename.c_str(), std::ios::out);
std::string plot_type = "plot";
velocityAvailable() ? plot_type = "splot" : plot_type = "plot";
gnuplot_file << "# Gnuplot file. Draw with gnuplot -p "<< pose_filename << std::endl;
gnuplot_file << "set mapping cartesian" << std::endl;
gnuplot_file << "set mouse" << std::endl;
gnuplot_file << "set size ratio -1" << std::endl;
gnuplot_file << plot_type + " '"<< pose_filename << "' using 1:2:3:4";
if (velocityAvailable())
{
gnuplot_file << ":5:6";
}
gnuplot_file << " with vector";
if (curvatureAvailable())
{
gnuplot_file << ", " << "'" << curvature_filename << "' using 1:2";
if (velocityAvailable())
{
gnuplot_file << ":3";
}
gnuplot_file << " w l";
}
gnuplot_file << std::endl;
gnuplot_file.close();
std::cout << "GnuPlot file written to " << gnuplot_filename << std::endl;
}
void Graphics::onPositionChanged( const Position2d& newPosition )
{
m_sprite.setPosition( sf::Vector2f( newPosition.toPositionXY().toVector2i() ) );
}
