server-whatever/monkey_car/fp_path.h

#ifndef _FP_PATH_HPP_
#define _FP_PATH_HPP_

#include <math.h>
#include <assert.h>
#include <vector>
#include "base_data.h"
#include "../point.h"

struct fixed_point:point_2
{
	fixed_point(double x,double y)
		:point_2(x,y)
		,cos_(0),sin_(0)
		,dist_(0)
	{}

	fixed_point(const point_2&pt)
		:point_2(pt)
		,cos_(0),sin_(0)
		,dist_(0)
		{}

	fixed_point(const fixed_point&pt)
		:point_2(pt)
		,cos_(pt.cos_),sin_(pt.sin_)
		,dist_(pt.dist_)
		{}
	double cos_;
	double sin_;
	double dist_;
	double set_next_point(const point_2&p)
	{
		dist_ = dist_to(p);

		assert(dist_>0);

		double deta_x = p.x_-x_;
		cos_ = deta_x/dist_;

		double deta_y = p.y_-y_;
		sin_ = deta_y/dist_;
		return dist_;
	}
};
struct fp_path
{
private:
	std::vector<fixed_point> path_;
	fp_path(const fp_path&);
	double m_total_length;
private:
	static inline bool eq(double x1,double x2)
	{
		return fabs(x1-x2)<0.1;
	}
public:
	fp_path()
		:m_total_length(0)
	{
		path_.reserve(8);
	}

	template<typename iterator>
	fp_path(iterator begin,iterator end)
		:m_total_length(0)
	{
		for (;begin != end; ++ begin)
		{
			add_point(*begin);
		}
	}
	void add_point(double x,double y)
	{
		add_point(point_2(x,y));
	}
	void add_point(const point &pt)
	{
		point_2 p(pt.x,pt.y);
		add_point(p);
	}
	void add_point(const point_2&pt)
	{
		if(!path_.empty()) 
		{
			 double d = path_.back().set_next_point(pt); 
			 m_total_length += d;
		}

		path_.push_back(pt);
	}
	double map(double x,double y)
	{
		
		if(path_.size()<2)
			return -1.;

		double rc=0.;

		auto it1=path_.begin(); 
		double l1=it1->dist_to(x,y);
		
		auto it2=path_.begin(); ++it2;
		for(; it2!=path_.end();++it2)
		{
			double l2=it2->dist_to(x,y);

			if(eq(l1+l2, it1->dist_))
			{
				return rc+l1;
			}

			rc+=it1->dist_;

			it1=it2;
			l1=l2;
		}

		return -1.;
	}

	double  map(const point_2& p)
	{
		return map(p.x_, p.y_);
	}

	point_2  map(double dist)
	{
		if(path_.size()>1)
		{
			for(auto it=path_.begin(), _e=path_.end()-1; it!=_e;++it)
			{
				if(dist<=it->dist_)
				{
					return point_2(it->x_+it->cos_*dist,it->y_+it->sin_*dist);
				}

				dist-=it->dist_;

				if(dist<0) 
					break;
			}
		}

		return point_2::invalid_point();
	}
	double get_total_length()
	{
		return m_total_length;
	}
};
#endif