/////////////////////////////////////////////////

/////////////////////////////////////////////////

#include<stdlib.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <GL/glut.h>
#include<iostream>
#include<string>
#include<sstream> 
#include<fstream>
using namespace std;


#include "geometory.h"
//#include "viewgui.h"
#include "opencv_inc.h"
#include "collision_detec_3d.h"



//unsigned int urg_data_num=510;
//double urg_range_deg=240.0;
//double urg_leng_max=5.0;
//double urg_leng_min=0.2;

double urg_range_deg=240.0;
double urg_leng_max=30.0;
double urg_leng_min=0.2;

double noise_odometory_v_gain=-0.10;
double noise_odometory_w_gain=M_PI*0.1;
double noise_urg=0.00;





double Uniform( void ){
	return ((double)rand()+1.0)/((double)RAND_MAX+2.0);
}

double rand_normal( double mu, double sigma ){
	double z=sqrt( -2.0*log(Uniform()) ) * sin( 2.0*M_PI*Uniform() );
	return mu + sigma*z;
}


void urg_noise( URG &urg_data){

	for(int i=0;i<urg_data.data_num;i++){
		if(urg_data.leng[i]==urg_data.leng_max){
				urg_data.leng[i]=NAN;
		}
		else{
		urg_data.leng[i] = urg_data.leng[i] +urg_data.leng[i]*rand_normal(0,1.0)*noise_urg;
		}
	}


}



//strをdoubleに変換する
double str_double(string str){	
	istringstream is;     
	is.str(str);           
	double x;              
	is >> x;                 
	return x;
}



//時間を計測する
/*double get_dtime(void){
	double freq = 1000.0/cv::getTickFrequency();
	static int64 old_time = cv::getTickCount();
	int64 time = cv::getTickCount();
	double dt_msec=(time-old_time)*freq;
	old_time=time;
	return dt_msec;
}
*/
 static double get_dtime()
{
    struct timeval tv;
    gettimeofday(&tv, NULL);
	double n_time =tv.tv_sec + tv.tv_usec * 1e-6;
	static double o_time= n_time;
	double dt_msec=(n_time-o_time);

	o_time= n_time;

    return dt_msec;
}




void normal_calc(double *p1, double *p2, double *p3, double *v_nl){
    double v1[3],v2[3];
    double length_nl;
    //ベクトル計算
    v1[0]=p2[0] - p1[0];
    v1[1]=p2[1] - p1[1];
    v1[2]=p2[2] - p1[2];
    v2[0]=p3[0] - p1[0];
    v2[1]=p3[1] - p1[1];
    v2[2]=p3[2] - p1[2];
    //外積
    v_nl[0] = v1[1] * v2[2] - v1[2] * v2[1];
    v_nl[1] = v1[2] * v2[0] - v1[0] * v2[2];
    v_nl[2] = v1[0] * v2[1] - v1[1] * v2[0];
    //正規化


    length_nl = sqrt(v_nl[0]*v_nl[0]+v_nl[1]*v_nl[1]+v_nl[2]*v_nl[2]);
    if(length_nl != 0){
	v_nl[0] = v_nl[0]/length_nl;
	v_nl[1] = v_nl[1]/length_nl;
	v_nl[2] = v_nl[2]/length_nl;
    }
}


//線分の交差判定関数
//但し，a=bだと交差判定受けるので注意．
bool cross_check(double a1_x,double a1_y,double a2_x,double a2_y,double b1_x,double b1_y,double b2_x,double b2_y){

	// 外積:axb = ax*by - ay*bx
	// 外積と使用して交差判定を行なう
	double v1 = (a2_x - a1_x) * (b1_y - a1_y) - (a2_y - a1_y) * (b1_x - a1_x);
	double v2 = (a2_x - a1_x) * (b2_y - a1_y) - (a2_y - a1_y) * (b2_x - a1_x);
	double m1 = (b2_x - b1_x) * (a1_y - b1_y) - (b2_y - b1_y) * (a1_x - b1_x);
	double m2 = (b2_x - b1_x) * (a2_y - b1_y) - (b2_y - b1_y) * (a2_x - b1_x);

	// +-,-+だったら-値になるのでそれぞれを掛けて確認
	if((v1*v2<= 0) && (m1*m2 <= 0)){
		return true; // ２つとも左右にあった
	}else{
		//cout<<"checkerror"<<endl;
		return false;
	}
}

//
bool cross_xy(double a1_x,double a1_y,double a2_x,double a2_y,double b1_x,double b1_y,double b2_x,double b2_y,double &cro_x,double &cro_y){

	double aa_a=(a2_y-a1_y)/(a2_x-a1_x+1/1000000.0);
	double aa_b=a1_y-aa_a*a1_x;

	double bb_a=(b2_y-b1_y)/(b2_x-b1_x+1/1000000.0);
	double bb_b=b1_y-bb_a*b1_x;

	if(a2_x!=a1_x){
		aa_a=(a2_y-a1_y)/(a2_x-a1_x);
		aa_b=a1_y-aa_a*a1_x;
	}
	if(b2_x!=b1_x){
		bb_a=(b2_y-b1_y)/(b2_x-b1_x);
		bb_b=b1_y-bb_a*b1_x;
	}


	if(aa_a!=bb_a){
		cro_x=-(bb_b-aa_b)/(bb_a-aa_a);
		cro_y=cro_x*aa_a+aa_b;
		//cout<<cro_x<<"_"<<cro_y<<endl;
		return true;
	}
	return false;
}


//仮想距離センサ
void urg_calcurate( ROBOT &robot, URG urg_area, OBSTACLE obst, MOVE_OBSTACLE move_obst, URG &urg_data){

	//URG_Dataの初期化
	for(int i=0;i<urg_data.data_num;i++){
		urg_data.x[i]=robot.x;
		urg_data.y[i]=robot.y;
		urg_data.leng[i]=0.0;
	}

	//URG_レーザ
	for(int i=0;i<urg_area.data_num;i++){
		urg_area.x[i]=(-1)*urg_area.leng[i]*sin(i*urg_data.reso+urg_data.start_rad+robot.theta)+robot.x;
		urg_area.y[i]=urg_area.leng[i]*cos(i*urg_data.reso+urg_data.start_rad+robot.theta)+robot.y;
	}

	for(int j=0;j<obst.n;j++){
		for(int i=0;i<urg_area.data_num;i++){
			double a1_x=obst.x1[j];
			double a1_y=obst.y1[j];
			double a2_x=obst.x2[j];
			double a2_y=obst.y2[j];
			double b2_x=urg_area.x[i];
			double b2_y=urg_area.y[i];
			double b1_x=robot.x;
			double b1_y=robot.y;
			//線分の交差判定
			if(cross_check(a1_x,a1_y,a2_x,a2_y,b1_x,b1_y,b2_x,b2_y)==true){

				//	cout<<"交差:"<<a1_x<<endl;
				//attach_flg=1;
				double ata_x=0.0;
				double ata_y=0.0;
				//線分の交点の算出
				if(cross_xy(a1_x,a1_y,a2_x,a2_y,b1_x,b1_y,b2_x,b2_y,ata_x,ata_y)==true){//交点のある場合

					if(urg_data.leng[i]==0){	//データが入っていないなら
						//	cout<<"n"<<(ata_y-robot.y)<<"_"<<(ata_x-robot.x)<<endl;
						//	cout<<"n"<<((ata_y-robot.y)*(ata_y-robot.y)+(ata_x-robot.x)*(ata_x-robot.x))<<endl;
						urg_data.leng[i]=sqrt((ata_y-robot.y)*(ata_y-robot.y)+(ata_x-robot.x)*(ata_x-robot.x));
					}

					else{						//データが入っていたら
						double oldleng=urg_data.leng[i];
						double temp_leng=sqrt((ata_y-robot.y)*(ata_y-robot.y)+(ata_x-robot.x)*(ata_x-robot.x));
						if(temp_leng<oldleng){//元より小さければ格納
							urg_data.leng[i]=sqrt((ata_y-robot.y)*(ata_y-robot.y)+(ata_x-robot.x)*(ata_x-robot.x));

						}
					}

				}
				//線分の交点算出はここまで
			}
			//線分の交差判定はここまで
		}
	}

	for(int j=0;j<move_obst.n;j++){
		for(int k=0;k<move_obst.m;k++){
			for(int i=0;i<urg_area.data_num;i++){
				double a1_x=move_obst.obst_x[j][k];
				double a1_y=move_obst.obst_y[j][k];
				double a2_x=move_obst.obst_x[j][k+1];
				double a2_y=move_obst.obst_y[j][k+1];
				double b2_x=urg_area.x[i];
				double b2_y=urg_area.y[i];
				double b1_x=robot.x;
				double b1_y=robot.y;
				//線分の交差判定
			//	if((a1_x!=a2_x)&&(a1_y!=a2_y)){
				if(cross_check(a1_x,a1_y,a2_x,a2_y,b1_x,b1_y,b2_x,b2_y)==true){

					//	cout<<"交差:"<<a1_x<<endl;
					//attach_flg=1;
					double ata_x=0.0;
					double ata_y=0.0;
					//線分の交点の算出
					if(cross_xy(a1_x,a1_y,a2_x,a2_y,b1_x,b1_y,b2_x,b2_y,ata_x,ata_y)==true){//交点のある場合

						if(urg_data.leng[i]==0){	//データが入っていないなら
							//	cout<<"n"<<(ata_y-robot.y)<<"_"<<(ata_x-robot.x)<<endl;
							//	cout<<"n"<<((ata_y-robot.y)*(ata_y-robot.y)+(ata_x-robot.x)*(ata_x-robot.x))<<endl;
							urg_data.leng[i]=sqrt((ata_y-robot.y)*(ata_y-robot.y)+(ata_x-robot.x)*(ata_x-robot.x));
						}

						else{						//データが入っていたら
							double oldleng=urg_data.leng[i];
							double temp_leng=sqrt((ata_y-robot.y)*(ata_y-robot.y)+(ata_x-robot.x)*(ata_x-robot.x));
							if(temp_leng<oldleng){//元より小さければ格納
								urg_data.leng[i]=sqrt((ata_y-robot.y)*(ata_y-robot.y)+(ata_x-robot.x)*(ata_x-robot.x));

							}
						}

					}
				}
					//線分の交点算出はここまで
			//	}
				//線分の交差判定はここまで
			}
		}
	}

	
}
//障害物情報の読み込み
void read_obstacle(OBSTACLE &obst,string fname){
	cout << "Obstacle File reading" << endl;

	string filename(fname.c_str());
	string str_line;
	ifstream ifs( filename.c_str() );

	if( !ifs ) {
		cout << "Error:Input data file not found" << endl;
		return;
	}

	string obst_in[4096][4]={};

	int line=0;
	int width=0;
	getline( ifs, str_line );
	while( getline( ifs, str_line ) ){
		string token;
		istringstream stream( str_line );

		while( getline( stream, token, ',' ) ) {
			obst_in[line][width]=token;
			//		cout << obst_in[line][width] << ",";
			width++;
		}
		line++;
		width=0;

		//	cout << endl;
	}

	obst.n=line;
	//cout<<"obst:" <<obst.n<< endl;

	for(int i=0;i<obst.n;i++){
		obst.x1[i]=str_double(obst_in[i][0]);
		obst.y1[i]=str_double(obst_in[i][1]);
		obst.x2[i]=str_double(obst_in[i][2]);
		obst.y2[i]=str_double(obst_in[i][3]);
		obst.z[i]=obstacle_z;
	}
//	for(int i=0;i<obst.n;i++){
//		cout << obst.x1[i] << ",";
//		cout << obst.x2[i] << ",";
//		cout << obst.y1[i] << ",";
//		cout << obst.y2[i] << ",";
//		cout << endl;
//	}

	return;
}

//障害物情報の読み込み
void write_obstacle(OBSTACLE &obst,string fname){
	cout << "Obstacle File Write" << endl;
	for(int i=0;i<obst.n;i++){
		cout << obst.x1[i] << ",";
		cout << obst.x2[i] << ",";
		cout << obst.y1[i] << ",";
		cout << obst.y2[i] << ",";
		cout << endl;
	}


	std::ofstream writing_file;
	writing_file.open(fname.c_str(), std::ios::out);

	std::cout << "writing " << fname << "..." << std::endl;

	writing_file << "x1,y1,x2,y2" << endl;

	for(int i=0;i<obst.n;i++){
		writing_file << obst.x1[i] << ",";
		writing_file << obst.y1[i] << ",";
		writing_file << obst.x2[i] << ",";
		writing_file << obst.y2[i] << ",";
		writing_file << endl;
	}
	return;
}







void To_goal_velocity(double _goal_x,double _goal_y,double robot_x,double robot_y,double robot_theta,double &speed,double &turn,double goal_reach_dis,double turn_limit){

	double goal_x=_goal_x-robot_x;
	double goal_y=_goal_y-robot_y;
	double goal_l=sqrt(goal_x*goal_x+goal_y*goal_y);
	double goal_rad=(-1)*atan2(goal_x,goal_y);
	//ロボット角度の正規化(＋2PIに抑制)
	double robot_theta_fmod=atan2(sin(robot_theta),cos(robot_theta));
	//角度の差分
	double sub_rad=goal_rad-robot_theta_fmod;

	//Tanが±πの範囲のため、右回りと左回りを評価
	if(fabs(goal_rad-robot_theta_fmod)>1.0*M_PI){
		if(fabs(goal_rad-robot_theta_fmod+4.0*M_PI)<fabs(goal_rad-robot_theta_fmod)){
			sub_rad=goal_rad-robot_theta_fmod+2.0*M_PI;
		}
		else if(fabs(goal_rad-robot_theta_fmod-4.0*M_PI)<fabs(goal_rad-robot_theta_fmod)){
			sub_rad=goal_rad-robot_theta_fmod-2.0*M_PI;
		}
		else if(fabs(goal_rad-robot_theta_fmod+2.0*M_PI)<fabs(goal_rad-robot_theta_fmod)){
			sub_rad=goal_rad-robot_theta_fmod+2.0*M_PI;
		}
		else if(fabs(goal_rad-robot_theta_fmod-2.0*M_PI)<fabs(goal_rad-robot_theta_fmod)){
			sub_rad=goal_rad-robot_theta_fmod-2.0*M_PI;
		}
	}

	double speed_gain=2.0;
	double turn_gain=speed_gain*1.0;
	//速度の評価関数は実機にあわせて修正すること。
	double x=goal_l;
	//speed = (0.0246*pow(x,4) - 0.1987*pow(x,3) + 0.169*pow(x,2) + 1.7482*pow(x,1)-1.4 );
	//	speed = (x );

	speed=fabs(x)*speed_gain;


	//	double turn_speed=fabs(sub_rad);
	double r=sub_rad;
	//	double turn_speed = fabs(r);
	double turn_speed = -0.0278*pow(r,5) + 0.3025*pow(r,4) - 1.1232*pow(r,3) + 1.4103*pow(r,2) + 0.4428*pow(r,1) + 0.0;

	if(sub_rad>=0) turn=-1*turn_speed*turn_gain;
	else if((sub_rad<=0)) turn=1*turn_speed*turn_gain;

	//角度のかい離大きいたらその場で旋回
	if(turn_speed>=M_PI/2.0)	speed=0.0;
	if(turn_speed<=-M_PI/2.0)	speed=0.0;


	//最大速度の規定
	if(speed>2.0)	speed=2.0;
	if(goal_l<goal_reach_dis) speed=0.0;	//到達したら停止

	//cout<<"robo"<<robot_theta_fmod<<"\t";
	//cout<<"atan"<<sub_rad<<"\t";
	//cout<<"sub"<<turn<<endl;
	return ;
}

//仮想距離センサ
void urg_calcurate_3d( ROBOT &robot, OBSTACLE obst, MOVE_OBSTACLE move_obst, URG &urg_data,double pan,double tilt,double robot_z){
	//URG urg_data;

	double length[2500];
	double area_x[2500];
	double area_y[2500];
	double area_z[2500];	
	
	//URG_Dataの初期化
	for(int i=0;i<urg_data.data_num;i++){
		//urg_data.x[i]=robot.x;
		//urg_data.y[i]=robot.y;
		//urg_data.leng[i]=0.0;
		length[i]=0.0;
	}

	//double robot_z=2.0;
	//URG_レーザ
	for(int i=0;i<urg_data.data_num;i++){
		length[i]=urg_data.leng_max;

		double xx=(-1)*length[i]*sin(i*urg_data.reso+urg_data.start_rad);
		double yy=length[i]*cos(i*urg_data.reso+urg_data.start_rad);
		double zz=0.0;

		double xx_r=xx;
		double yy_r=yy*cos(tilt)-zz*sin(tilt);
		double zz_r=yy*sin(tilt)+zz*cos(tilt);

		area_x[i]=xx_r*cos(pan+robot.theta)-yy_r*sin(pan+robot.theta)+robot.x;
		area_y[i]=xx_r*sin(pan+robot.theta)+yy_r*cos(pan+robot.theta)+robot.y;
		area_z[i]=zz_r+robot_z;
	}


	//底面との衝突判定
	for(int i=0;i<urg_data.data_num;i++){
		xyz line[2];
		xyz tri[3];
		xyz out[2];

		//床の3Dポリゴン
		tri[0].x=-100;tri[0].y=-100;tri[0].z=0.001;
		tri[1].x=-100;tri[1].y=+100;tri[1].z=0.001;
		tri[2].x=+100;tri[2].y=+100;tri[2].z=0.001;
		line[0].x=area_x[i];line[0].y=area_y[i];line[0].z=area_z[i];
		line[1].x=robot.x;line[1].y=robot.y;line[1].z=robot_z;
		bool flag1=collision_tri_line(tri,line,out[0]);

		//床の3Dポリゴン
		tri[0].x=-100;tri[0].y=-100;tri[0].z=0.001;
		tri[1].x=+100;tri[1].y=-100;tri[1].z=0.001;
		tri[2].x=+100;tri[2].y=+100;tri[2].z=0.001;
		line[0].x=area_x[i];line[0].y=area_y[i];line[0].z=area_z[i];
		line[1].x=robot.x;line[1].y=robot.y;line[1].z=robot_z;
		bool flag2=collision_tri_line(tri,line,out[1]);

		if(flag1||flag2){
			double dis_min=0.0;
			double dis1=sqrt((line[1].x-out[0].x)*(line[1].x-out[0].x)+(line[1].y-out[0].y)*(line[1].y-out[0].y)+(line[1].z-out[0].z)*(line[1].z-out[0].z));
			double dis2=sqrt((line[1].x-out[1].x)*(line[1].x-out[1].x)+(line[1].y-out[1].y)*(line[1].y-out[1].y)+(line[1].z-out[1].z)*(line[1].z-out[1].z));
			if(dis1<dis2)	dis_min=dis1;
			else			dis_min=dis2;

			if(length[i]==0){	//データが入っていないなら
				length[i]=dis_min;
			}
			else{						//データが入っていたら
				double old_dis=length[i];
				if(dis_min<old_dis){//元より小さければ格納
					length[i]=dis_min;
				}
			}

		}
	}



	//壁面とのあたり判定
	for(int j=0;j<obst.n;j++){
		for(int i=0;i<urg_data.data_num;i++){
			xyz line[2];
			xyz tri[3];
			xyz out[2];

			//上半分
			tri[0].x=obst.x1[j];tri[0].y=obst.y1[j];tri[0].z=0.0;
			tri[1].x=obst.x2[j];tri[1].y=obst.y2[j];tri[1].z=0.0;
			tri[2].x=obst.x2[j];tri[2].y=obst.y2[j];tri[2].z=obst.z[j];
			
			line[0].x=area_x[i];line[0].y=area_y[i];line[0].z=area_z[i];
			line[1].x=robot.x;line[1].y=robot.y;line[1].z=robot_z;
			
			bool flag1=collision_tri_line(tri,line,out[0]);

			//下半分
			tri[0].x=obst.x1[j];tri[0].y=obst.y1[j];tri[0].z=0.0;
			tri[1].x=obst.x1[j];tri[1].y=obst.y1[j];tri[1].z=obst.z[j];
			tri[2].x=obst.x2[j];tri[2].y=obst.y2[j];tri[2].z=obst.z[j];

			line[0].x=area_x[i];line[0].y=area_y[i];line[0].z=area_z[i];
			line[1].x=robot.x;line[1].y=robot.y;line[1].z=robot_z;
			
			bool flag2=collision_tri_line(tri,line,out[1]);
			
			if(flag1||flag2){

				double dis_min=0.0;
				double dis1=sqrt((line[1].x-out[0].x)*(line[1].x-out[0].x)+(line[1].y-out[0].y)*(line[1].y-out[0].y)+(line[1].z-out[0].z)*(line[1].z-out[0].z));
				double dis2=sqrt((line[1].x-out[1].x)*(line[1].x-out[1].x)+(line[1].y-out[1].y)*(line[1].y-out[1].y)+(line[1].z-out[1].z)*(line[1].z-out[1].z));
				if(dis1<dis2)	dis_min=dis1;
				else			dis_min=dis2;

				if(length[i]==0){	//データが入っていないなら
					length[i]=dis_min;
				}
				else{						//データが入っていたら
					double old_dis=length[i];
					if(dis_min<old_dis){//元より小さければ格納
						length[i]=dis_min;
					}
				}

			}
			else{
				;
			}
		}
	}


	//移動障害物
	//高さ方向は考慮しない。
	for(int j=0;j<move_obst.n;j++){
		for(int k=0;k<move_obst.m;k++){
			for(int i=0;i<urg_data.data_num;i++){
				double a1_x=move_obst.obst_x[j][k];
				double a1_y=move_obst.obst_y[j][k];
				double a2_x=move_obst.obst_x[j][k+1];
				double a2_y=move_obst.obst_y[j][k+1];
				double b2_x=area_x[i];
				double b2_y=area_y[i];
				double b1_x=robot.x;
				double b1_y=robot.y;
				//線分の交差判定
				if(cross_check(a1_x,a1_y,a2_x,a2_y,b1_x,b1_y,b2_x,b2_y)==true){
					double ata_x=0.0;
					double ata_y=0.0;
					//線分の交点の算出
					if(cross_xy(a1_x,a1_y,a2_x,a2_y,b1_x,b1_y,b2_x,b2_y,ata_x,ata_y)==true){//交点のある場合

						if(length[i]==0){	//データが入っていないなら
							//	cout<<"n"<<(ata_y-robot.y)<<"_"<<(ata_x-robot.x)<<endl;
							//	cout<<"n"<<((ata_y-robot.y)*(ata_y-robot.y)+(ata_x-robot.x)*(ata_x-robot.x))<<endl;
							length[i]=sqrt((ata_y-robot.y)*(ata_y-robot.y)+(ata_x-robot.x)*(ata_x-robot.x));
						}

						else{						//データが入っていたら
							double oldleng=length[i];
							double temp_leng=sqrt((ata_y-robot.y)*(ata_y-robot.y)+(ata_x-robot.x)*(ata_x-robot.x));
							if(temp_leng<oldleng){//元より小さければ格納
								length[i]=sqrt((ata_y-robot.y)*(ata_y-robot.y)+(ata_x-robot.x)*(ata_x-robot.x));
							}
						}

					}
				}
			}
		}
	}
				//線分の交差判定はここまで

	for(int i=0;i<urg_data.data_num;i++){
		urg_data.leng[i]=length[i];
	}
}

void robot_move(ROBOT &myrobot,double dt){
	double dtt=dt+1.0/100000.0;						//0除算を回避
	double ac_v=fabs(myrobot.v-myrobot.real_v)/dtt;
	double ac_w=fabs(myrobot.w-myrobot.real_w)/dtt;
	if(ac_v>=myrobot.ac_trans_limit)	ac_v=myrobot.ac_trans_limit;
	if(ac_w>=myrobot.ac_rot_limit)		ac_w=myrobot.ac_rot_limit;
	
	if((myrobot.v-myrobot.real_v)>=0)		myrobot.real_v+=ac_v*dtt;
	else if((myrobot.v-myrobot.real_v)<0)	myrobot.real_v-=ac_v*dtt;

	if((myrobot.w-myrobot.real_w)>=0)		myrobot.real_w+=ac_w*dtt;
	else if((myrobot.w-myrobot.real_w)<0)	myrobot.real_w-=ac_w*dtt;

	//real_v+=real_v*rand_normal(0,1.0)*noise_odometory_v;
	//real_w+=real_w*rand_normal(0,1.0)*noise_odometory_w;

	myrobot.x+=myrobot.real_v*sin(-myrobot.theta)*dt;
	myrobot.y+=myrobot.real_v*cos(-myrobot.theta)*dt;
	myrobot.theta-=myrobot.real_w*dt;


	const double odometory_noise_liner=0.0;
	const double odometory_noise_liner_sigma=1.0;
	const double odometory_noise_angle=0.0;
	const double odometory_noise_angle_sigma=1.0;

	myrobot.x_dummy+=myrobot.real_v*sin(-myrobot.theta_dummy)*dt*(1+fabs(rand_normal(odometory_noise_liner,odometory_noise_liner_sigma))*noise_odometory_v_gain);
	myrobot.y_dummy+=myrobot.real_v*cos(-myrobot.theta_dummy)*dt*(1+fabs(rand_normal(odometory_noise_liner,odometory_noise_liner_sigma))*noise_odometory_v_gain);
	myrobot.theta_dummy-=myrobot.real_w*dt*(1+fabs(rand_normal(odometory_noise_angle,odometory_noise_angle_sigma))*noise_odometory_w_gain);

}

// rad角度を正規化（-pi〜PI）する
double normalize_theta_rad(double theta) 
{
	double multiplier;

	if (theta >= -M_PI && theta < M_PI)
		return theta;

	multiplier = floor(theta / (2*M_PI));
	theta = theta - multiplier*2*M_PI;
	if (theta >= M_PI)
		theta -= 2*M_PI;
	if (theta < -M_PI)
		theta += 2*M_PI;

	return theta;
}