#include "world.h"#include <iostream>#include "body.h"using namespace std;world::world(){	cycle = 0;	uniform_number = 0;	team_name = "Parsian";	field_length = 12.0;	field_width = 8.0;	goal_width = 7.32;	ready = false;}void world::set_flags(){	flags[F1L].global_position = Vector3d( -field_length / 2.0,  field_width / 2.0, 0.0 );	flags[F2L].global_position = Vector3d( -field_length / 2.0, -field_width / 2.0, 0.0 );	flags[F1R].global_position = Vector3d(  field_length / 2.0,  field_width / 2.0, 0.0 );	flags[F2R].global_position = Vector3d(  field_length / 2.0, -field_width / 2.0, 0.0 );	flags[G1L].global_position = Vector3d( -field_length / 2.0, -goal_width / 2.0, 0.0 );	flags[G2L].global_position = Vector3d( -field_length / 2.0,  goal_width / 2.0, 0.0 );	flags[G1R].global_position = Vector3d(  field_length / 2.0, -goal_width / 2.0, 0.0 );	flags[G2R].global_position = Vector3d(  field_length / 2.0,  goal_width / 2.0, 0.0 );	if( team_side == "right" )		for( int i = 0; i < 8; i++ )		{			flags[i].global_position.X() *= -1.0;			flags[i].global_position.Y() *= -1.0;		}	ready =true;}void world::set_teammate(int id, Vector3d p){	for(int i=0;i<teammates.size();i++)		if(teammates[i].id==id)		{			teammates[i].vision_position = p;			return;		}	teammates.push_back(agent(id,p));}void world::set_opponent(int id, Vector3d p){	for(int i=0;i<opponents.size();i++)		if(opponents[i].id==id)		{			opponents[i].vision_position = p;			return;		}	opponents.push_back(agent(id,p));}void world::analyze(){	update_torso_position();	update_torso_rotation_matrix();	update_objects_positions();	update_situation();}void world::update_torso_position(){	double dist1 = flags[F1R].vision_position.magnitude();	double dist2 = flags[F1L].vision_position.magnitude();	double dist3 = flags[F2R].vision_position.magnitude();	double l = field_length;	double w = field_width;	double x = -( pow( dist1, 2 ) - pow( dist2, 2 ) ) / l / 2;	double y = -( -pow( dist3, 2 ) + pow( dist1, 2 ) ) / w / 2;	double z;	double tmp=-sqr( w ) * pow( dist1, 4 ) +	2 * sqr( w ) * sqr( dist1 ) * sqr( dist2 ) - sqr( w ) * pow( dist2, 4 ) +	2 * sqr( w ) * sqr( l ) * sqr( dist2 ) - sqr( w ) * pow( l, 4 ) -	pow( w, 4 ) * sqr( l ) + 2 * sqr( l ) * sqr( dist3 ) * sqr( dist1 ) +	2 * sqr( dist3 ) * sqr( w ) * sqr( l ) - sqr( l ) * pow( dist1, 4 ) -	sqr( l ) * pow( dist3, 4 );	if( tmp < 0.00001 )	{		z = 0.1 / 2;		//	cerr << "world::update_torso_position z < 0: " << z <<endl;	}	else	{		z = sqrt(tmp) / l / w / 2;		if(z<0.045)		{			//	cerr << "world::update_torso_position z < 0.045: " << z <<endl;			z=0.05;		}	}	if( team_side == "right" )	{		x *= -1.0;		y *= -1.0;	}	my_position = Vector3d(x,y,z);}void world::update_torso_rotation_matrix(){	Matrix r;	for( int i = 0; i < 4; i++ )		flags[i].relative_position = flags[i].global_position - my_position;	Matrix m;	m.m[0][0] = flags[0].relative_position.X() / flags[0].relative_position.magnitude();	m.m[0][1] = flags[0].relative_position.Y() / flags[0].relative_position.magnitude();	m.m[0][2] = flags[0].relative_position.Z() / flags[0].relative_position.magnitude();	m.m[1][0] = flags[1].relative_position.X() / flags[1].relative_position.magnitude();	m.m[1][1] = flags[1].relative_position.Y() / flags[1].relative_position.magnitude();	m.m[1][2] = flags[1].relative_position.Z() / flags[1].relative_position.magnitude();	m.m[2][0] = flags[2].relative_position.X() / flags[2].relative_position.magnitude();	m.m[2][1] = flags[2].relative_position.Y() / flags[2].relative_position.magnitude();	m.m[2][2] = flags[2].relative_position.Z() / flags[2].relative_position.magnitude();	m.m[3][3] = 1;/*	logger[123] << " rposaa : " <<endl;	for( int i = 0; i < 4; i++ )	{		logger[123] << flags[i].r_pos << " " << flags[i].r_pos.magnitude() << endl;		logger[123] << "gpos: " << flags[i].global_position << endl;	}	logger[123] << " m : " << endl;*/	//	m.print();	m = -m;	Vector3d c, row0, row1, row2;	c.X() = flags[0].vision_position.X() / flags[0].vision_position.magnitude();	c.Y() = flags[1].vision_position.X() / flags[1].vision_position.magnitude();	c.Z() = flags[2].vision_position.X() / flags[2].vision_position.magnitude();	row0 = m * c;	c.X() = flags[0].vision_position.Y() / flags[0].vision_position.magnitude();	c.Y() = flags[1].vision_position.Y() / flags[1].vision_position.magnitude();	c.Z() = flags[2].vision_position.Y() / flags[2].vision_position.magnitude();	row1 = m * c;	c.X() = flags[0].vision_position.Z() / flags[0].vision_position.magnitude();	c.Y() = flags[1].vision_position.Z() / flags[1].vision_position.magnitude();	c.Z() = flags[2].vision_position.Z() / flags[2].vision_position.magnitude();	row2 = m * c;	r.m[0][0] = row0.X();	r.m[0][1] = row0.Y();	r.m[0][2] = row0.Z();	r.m[1][0] = row1.X();	r.m[1][1] = row1.Y();	r.m[1][2] = row1.Z();	r.m[2][0] = row2.X();	r.m[2][1] = row2.Y();	r.m[2][2] = row2.Z();	r.m[3][3] = 1;	for(int i=0;i<4;i++)	{		double sum=0;		for(int j=0;j<4;j++)			sum+=r.m[i][j]*r.m[i][j];		sum=sqrt(sum);		for(int j=0;j<4;j++)			r.m[i][j]/=sum;	}	torso_direction_matrix = -r;	bd->l[Torso].mat=Matrix(my_position)*torso_direction_matrix;}void world::update_objects_positions(){	for(int i=0;i<teammates.size();i++)		teammates[i].global_position = (Matrix(my_position) * torso_direction_matrix *		Matrix(teammates[i].vision_position)).getVector3d();	for(int i=0;i<opponents.size();i++)		opponents[i].global_position = (Matrix(my_position) * torso_direction_matrix *		Matrix(opponents[i].vision_position)).getVector3d();	ball_position = (Matrix(my_position) * torso_direction_matrix * 	Matrix(ball_vision_position)).getVector3d();}void world::update_situation(){	Vector2d tv = (torso_direction_matrix*Matrix(Vector3d(1,0,0))).getVector2d();	if(tv.magnitude()>0.001)		theta_angle_on_field =Angle::Angle::normalizeAngle2 ( tv.angle() );//	cerr<<my_position.to_str()<<endl;//	torso_direction_matrix.print();	if(my_position.Z() > 2.5)	{		situation = SARPAA;			}else	{		Vector3d v = (torso_direction_matrix * Matrix(Vector3d(1,0,0))).getVector3d();//		cerr<<v.to_str()<<"  ";		if(v.Z()>.8)		{			situation = TAVAZ;		}		else if(v.Z()>-.8)		{			situation = YEVAR;					}		else		{			situation = DAMAR;					}	}}void world::SetGyro(double x,double y,double z){	Gyroscope=Vector3d(x,y,z);}Vector3d world::GetGyro(){	return Gyroscope;}void world::SetHearMsg(double Time,string Name,string Msg){	HearTime=Time;	HearName=Name;	HearMsg=Msg;}double world::GetHearTime(){	return HearTime;}string world::GetHearName(){	return HearName;}string world::GetHearMsg(){	return HearMsg;}