add his_location

card.cpp

@@ -281,10 +281,11 @@ private:
 		cp.down_time = m_mine_tool->get_down_time();
 		cp.work_time = m_mine_tool->get_work_time();
 		cp.is_on_duty= m_mine_tool->is_on_duty();
+		log_info("on_timer here ...%d,%lld,%.2f,%.2f,%d,%d",m_id,_time,pt.x,pt.y,area_id,map_id);
 		m_his_location_card->push(_time,pt,area_id,map_id);
 		upt_card_pos(cp,pt);
 	}
-	point getSmoothPoint(uint64_t t)
+	point getSmoothPoint(uint64_t& t)
 	{
 		point pt;
 		loc_point lp = m_smo_tool->smooth_strategy();
@@ -299,6 +300,7 @@ private:
 			{
 				m_stat = 7;
 				pt = p->getPoint(t);	
+				log_info("getpoint_oncar:%d,%lld",m_id,t);
 			}
 		}
 		return pt;

his_location.h

@@ -71,10 +71,10 @@ struct location_card
 		if(!is_valid())
 		  return false;
 		point p=m_d.back().p;
-		if(p==pt)p=m_p;
+		if(p==pt)return false;
 		double arg = make_arg(pt,p);
-		log_info("his_location:line_changed:%d,%.2f,m_arg:%.2f",m_cardid,arg,m_arg);
-		return arg != m_arg;
+		log_info("his_location:line_changed:%d,%f,m_arg:%f,%s,%f",m_cardid,arg,m_arg, (arg-m_arg > -1e-10 && arg-m_arg<1e-10)?"same":"not same",m_arg-arg);
+		return !point::eq(arg,m_arg,1e-10);
 	}
 	bool is_speed_changed(const point& pt,uint64_t time)
 	{
@@ -100,9 +100,9 @@ struct location_card
 		  
 		return flag;
 	}
-	bool time_out(uint64_t time)
+	bool time_out(const point &p,uint64_t time)
 	{
-		if(time-m_timestamp>=60*1000)
+		if(time-m_timestamp>=60*1000 && !(p == m_p))
 			return true;
 		return false;
 	}
@@ -151,7 +151,7 @@ struct location_card
 		bool flag=false;
 		bool iflag=false;
 		flag=handle_message(p,timestamp);
-		if(time_out(timestamp))
+		if(time_out(p,timestamp))
 			if(!flag)iflag=true;
 		if(is_speed_changed(p,timestamp))
 			if(!flag)iflag=true;
@@ -162,8 +162,9 @@ struct location_card
 		if(iflag)
 		{
 			update(p,timestamp);
-			set_invalid();	
+			//set_invalid();	
 			set(p,timestamp);
+			insert();
 		}
 	}
 	virtual std::string getTabName()=0;
@@ -207,40 +208,50 @@ struct location_staff:location_card
 			flag=true;
 			if(timestamp-m_timestamp>120*1000)
 			{
+				log_info("his_location:line_changed_timeout %d",m_cardid);
 				update(m_d.back().p,m_d.back().time);
 				set(p,timestamp);
 			}
 			else
 			{
-				std::vector<point> rc=card_path::inst().find_path(m_p,p);
+				log_info("his_location:line_changed_find_path %d,(%.2f,%.2f)-->(%.2f,%.2f)-->(%.2f,%.2f)",m_cardid,m_p.x,m_p.y,m_d.back().p.x,m_d.back().p.y,p.x,p.y);
+				std::vector<point> rc=card_path::inst().find_path(point(m_d.back().p.x,-m_d.back().p.y),point(p.x,-p.y));
 				if(rc.empty())				
 				{
+					log_info("his_location:line_changed rc.empty() %d",m_cardid);
 					update(m_d.back().p,m_d.back().time);
 					set(m_d.back().p,m_d.back().time);
 				}
 				else
 				{
+					for(uint32_t i=0;i<rc.size();i++)
+						log_info("his_location:find_path..x=%.3lf,y=%.3lf\n",rc[i].x,rc[i].y);
 					if(rc.size()==1)
 					{
 						log_info("his_location:line_changed %d circle point(%.2f,%.2f)",m_cardid,rc[0].x,rc[0].y);
-						update(rc[0],m_d.back().time);
-						set(rc[0],m_d.back().time);
+						point tmp(rc[0].x,-rc[0].y);
+						update(tmp,m_d.back().time);
+						set(tmp,m_d.back().time);
 					}
 					else if(rc.size()==2)
 					{
-						update(rc[0],m_d.back().time);
-						set(rc[0],m_d.back().time);
+						point tmp(rc[0].x,-rc[0].y);
+						update(tmp,m_d.back().time);
+						set(tmp,m_d.back().time);
 						//
-						m_arg=make_arg(rc[1],m_p);
+						tmp.set(rc[1].x,-rc[1].y);
+						m_arg=make_arg(tmp,m_p);
 						insert();
 						//
-						update(rc[1],timestamp);
-						set(rc[1],timestamp);
+						update(tmp,timestamp);
+						set(tmp,timestamp);
 					}
 					else//更多的拐点
 					{
-						update(rc[0],m_d.back().time);
-						set(rc[1],timestamp);
+						point tmp(rc[0].x,-rc[0].y);
+						update(tmp,m_d.back().time);
+						tmp.set(rc.back().x,-rc.back().y);
+						set(tmp,timestamp);
 					}
 				}
 			}

main.cpp

@@ -133,6 +133,13 @@ void usage(char ** argv)
 	}
 
 
+}
+void test_find_path(const point&p1,const point&p2)
+{
+	printf("\nfind-path:  from=(%.3lf,%.3lf),to=(%.3lf,%.3lf)\n",p1.x,p1.y,p2.x,p2.y);
+	std::vector<point> rc=card_path::inst().find_path(p1,p2);
+	for(uint32_t i=0;i<rc.size();i++)
+		printf("x=%.3lf,y=%.3lf\n",rc[i].x,rc[i].y);
 }
 int main(int argc ,char * argv[])
 {
@@ -150,6 +157,7 @@ int main(int argc ,char * argv[])
 	atexit(&cleanup);
 
 	card_path::init();
+	test_find_path(point(4717,209.2),point(4727,206.4));
 
     module_mgr::start();
 

monkey_car/monkeycar_bus.cpp

@@ -51,10 +51,10 @@ void monkey_bus::adjust_monkeyperson_distance()
 
 bool monkey_bus::test_get_on(double speed)
 {
-	double s=speed>0?speed:-speed;
-	double ms=m_speed>0?m_speed:-m_speed;
-	log_info("monkeycar test_get_on:%.2f,%.2f",s,ms);
-	log_info("monkeycar ...........:%.2f,%.2f",ms*ON_BUS_SPEED_LIMIT,ms*ON_BUS_SPEED_MAX_LIMIT);
+	double s=fabs(speed);
+	double ms=fabs(m_speed);
+	//log_info("monkeycar test_get_on:%.2f,%.2f",s,ms);
+	//log_info("monkeycar ...........:%.2f,%.2f",ms*ON_BUS_SPEED_LIMIT,ms*ON_BUS_SPEED_MAX_LIMIT);
 	if (s >= ms*ON_BUS_SPEED_LIMIT && s < ms*ON_BUS_SPEED_MAX_LIMIT)
 	{
 		log_info("monkeycar test_get_on_true:%.2f,%.2f",speed,m_speed);
@@ -66,8 +66,8 @@ bool monkey_bus::test_get_on(double speed)
 bool monkey_bus::test_get_off(std::shared_ptr<monkey_person> mp,double speed)
 {
 	bool ret = false;
-	double s=speed>0?speed:-speed;
-	double ms=m_speed>0?m_speed:-m_speed;
+	double s=fabs(speed);
+	double ms=fabs(m_speed);
 	ret = (speed > ZERO_ ? NEGTIVE_DIRECTION:POSTIVE_DIRECTION) == m_direct ;
 	if (ret)
 	{

point.h

@@ -66,7 +66,7 @@ struct point
 
 	bool operator==(const point&r)const
 	{
-		return x==r.x && y==r.y && z==r.z;
+		return eq(x,r.x,1e-10)&&eq(y,r.y,1e-10)&&eq(z,r.z,1e-10);
 	}
 
 	double dist_direct(const point&o)const