objects.cpp

robocup3d源化码 Nexus3D.tar.gz

/*
Copyright (C) 2005 Ferdowsi University
*/

#include "objects.h"
#include "global.h"
#include "math.h"
#include "logunit.h"



FieldObject::FieldObject()
{
	pos = Vector3f();
	distance=0;
}

MobileObject::MobileObject() : FieldObject()
{
	is_seen=false;
}

Line::Line(float A1,float B1,float C1)
{
	A=A1;
	B=B1;
	C=C1;
}

float Line::Angle()
{
	return ATan(-A/B);
}

float Line::Slope()
{
	 return -A/B;
}

void MobileObject::Update(Vector3f current_pos, float dist)
{
	int i;
	float ln_tim;
	is_seen=true;
	i=index_pos;
	Vector3f current_vel;
	index_pos=(index_pos+1)%5;

	last_info[index_pos].is_seen=true;
	last_info[index_pos].pos=current_pos;

	ln_tim=(CurrentTime - last_info[i].time);
	last_info[index_pos].time=CurrentTime;

	vel=(current_pos-last_info[i].pos)/ln_tim;

	last_info[index_pos].vel=vel;
	current_vel=vel;
	acc=(current_vel-last_info[i].vel)/ln_tim;

	env_conf=0;
	int temp;
	for(int i=0;i<5;i++)
	{
		temp=(i+index_pos)%5;
		if(last_info[temp].is_seen)
			env_conf = 100 / pow (2.0,(i+1)) ;
	}
	distance=dist;
	pos=current_pos;
}

void MobileObject::Update()
{
	int i;
	float ln_tim;
	is_seen=false;
	Vector3f current_pos;
	Vector3f current_vel;

	i = index_pos;
	index_pos=(index_pos+1)%5;

	ln_tim=(CurrentTime -last_info[i].time);
	last_info[index_pos].is_seen=false;

	//last_info[index_pos].pos = 0.5f * acc * pow( ln_tim ,2)+ last_info[i].vel *( ln_tim ) + last_info[i].pos;

	last_info[index_pos].pos = last_info[i].pos;;

	//current_pos=last_info[i].pos;
	//last_info[index_pos].pos=current_pos;
	//vel=(current_pos-last_info[i].pos)/ln_tim;
	vel=Vector3f(0,0,0);

	last_info[index_pos].vel=vel;
	//acc=(current_vel-last_info[i].vel)/ln_tim;
	acc=Vector3f(0,0,0);

	env_conf=0;
	int temp;
	for(int i=0;i<5;i++)
	{
		temp=(i+index_pos)%5;
		if(last_info[temp].is_seen)
			env_conf = 100 / pow (2.0,(i+1)) ;
	}

	pos=last_info[index_pos].pos;
}