world.cpp

robocuo相关资料robocuo相关资料

#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;
}