cminesweeper.cpp

一种动态环境下的基于神经网络的机器人路径搜索

#include "CMinesweeper.h"

//-----------------------------------constructor-------------------------
//
//-----------------------------------------------------------------------
CMinesweeper::CMinesweeper():
                             m_dRotation(RandFloat() * CParams::dTwoPi),
                             m_lTrack(0.16),
                             m_rTrack(0.16),
							               m_dScale(CParams::iSweeperScale),
                             m_bCollided(false),
                             m_dSpinBonus(0),
                             m_dCollisionBonus(0),
							 m_distance(1000),
							 m_costtime(0)
                             
			 
{
  //create a static start position
  m_vPosition = SVector2D(40, 40);

   //create the sensors
  CreateSensors(m_Sensors, CParams::iNumSensors, CParams::dSensorRange); 
}


//-------------------------------- CreateSensors ------------------------
//
//	This function returns a vector of points which make up the segments of
//  the sweepers sensors.
//------------------------------------------------------------------------
void CMinesweeper::CreateSensors(vector<SPoint> &sensors,
                                 int            NumSensors,
                                 double         range)
{
  //确认触觉器开始是空的

  sensors.clear();

  double SegmentAngle = CParams::dPi / (NumSensors-1);

	//顺时针生成各个触觉器(不包括起点)

	for (int i=0; i<CParams::iNumSensors; i++)
	{
		//计算顶点位置

		SPoint point;

    point.x = -sin(i * SegmentAngle - CParams::dHalfPi) * range;

	  point.y = cos(i * SegmentAngle - CParams::dHalfPi) * range;
		
		sensors.push_back(point);
		
	}//继续下一个

}
//-----------------------------Reset()------------------------------------
//
//	Resets the sweepers position, energy level and rotation
//
//------------------------------------------------------------------------
void CMinesweeper::Reset()
{
	//reset the sweepers positions
	m_vPosition = SVector2D(40, 40);
	
	//and the energy level
	m_dFitness = 0;
m_trail.clear();
  //and the rotation
  m_dRotation = RandFloat() * CParams::dTwoPi;

  //and the bonuses
  m_dSpinBonus        = 0;
  m_dCollisionBonus   = 0;
  m_distance=1000;
  m_costtime=0;

}

//------------------------- RenderMemory ---------------------------------
//
//------------------------------------------------------------------------
void CMinesweeper::RenderPenalties(HDC surface)
{
  string s = ftos(sqrt(m_distance)-24);
  s = "距离: " + s	;
  TextOut(surface, 200,0,s.c_str(), s.size());

  s = ftos(m_costtime);
  s = "消耗帧数: " + s;
  TextOut(surface, 300, 0,s.c_str(), s.size());

}
//---------------------WorldTransform--------------------------------
//
//	sets up a translation matrix for the sweeper according to its
//  scale, rotation and position. Returns the transformed vertices.
//-------------------------------------------------------------------
void CMinesweeper::WorldTransform(vector<SPoint> &sweeper, double scale)
{
	//create the world transformation matrix
	C2DMatrix matTransform;
	
	//scale
	matTransform.Scale(scale, scale);
	
	//rotate
	matTransform.Rotate(m_dRotation);
	
	//and translate
	matTransform.Translate(m_vPosition.x, m_vPosition.y);
	
	//now transform the ships vertices
	matTransform.TransformSPoints(sweeper);
}

//-------------------------------Update()--------------------------------
//
//	First we take sensor readings and feed these into the sweepers brain.
//
//	The inputs are:
//	
//  The readings from the minesweepers sensors
//
//	We receive two outputs from the brain.. lTrack & rTrack.
//	So given a force for each track we calculate the resultant rotation 
//	and acceleration and apply to current velocity vector.
//
//-----------------------------------------------------------------------
bool CMinesweeper::Update(vector<SPoint> &objects)
{
	
	//this will store all the inputs for the NN
	vector<double> inputs;	

  //grab sensor readings
  TestSensors(objects);


  //input sensors into net
  for (int sr=0; sr<m_vecdSensors.size(); ++sr)
  {
    inputs.push_back(m_vecdSensors[sr]);
    
  }
for (sr=0; sr<m_vecdSensors.size(); ++sr)
{inputs.push_back(m_vecFeelers[sr]);}	
 SVector2D goal(goalx,goaly) ;
Vec2DNormalize(goal);
double dot=Vec2DDot(m_vLookAt,goal);
int sign = Vec2DSign(m_vLookAt,goal);
inputs.push_back(dot*sign);
	//update the brain and get feedback
	vector<double> output = m_ItsBrain.Update(inputs);

	//make sure there were no errors in calculating the 
	//output
	if (output.size() < CParams::iNumOutputs) 
  {
    return false;
  }

  m_trail.push_back(SPoint(m_vPosition.x,m_vPosition.y));
	//assign the outputs to the sweepers left & right tracks
	m_lTrack = output[0];
	m_rTrack = output[1];

	//calculate steering forces
	double RotForce = m_lTrack - m_rTrack;

  //clamp rotation
	Clamp(RotForce, -CParams::dMaxTurnRate, CParams::dMaxTurnRate);
	
	m_dRotation += RotForce;

	//update Look At 
	m_vLookAt.x = -sin(m_dRotation);
	m_vLookAt.y = cos(m_dRotation);
m_distance=(goalx-m_vPosition.x)*(goalx-m_vPosition.x)+(goaly-m_vPosition.y)*(goaly-m_vPosition.y);

//if the sweepers haven't collided with an obstacle
  //update their position
if(m_distance>625)
   m_costtime++;
  if (!m_bCollided )
  {
    if(m_distance>625)
	{
    m_dSpeed = (m_lTrack + m_rTrack);
   //if(965>m_vPosition.x || 545>m_vPosition.y)
   //update position
	
	m_vPosition += (m_vLookAt * m_dSpeed);}
  }
  

  //-----------------------adjust the fitness bonuses

  //reward for not turning too quickly
  const float RotationTolerance = 0.03f;

  if (fabs(RotForce) < RotationTolerance)
  {
    m_dSpinBonus += 1;
  }

  //reward for not colliding with an object
  if (!m_bCollided)
  {
    m_dCollisionBonus += 1;
  }

  
	return true;
}
//----------------------- TestSensors ------------------------------------
//
//  This function checks for any intersections between the sweeper's 
//  sensors and the objects in its environment
//------------------------------------------------------------------------
void CMinesweeper::TestSensors(vector<SPoint> &objects)
{
  m_bCollided = false;  
  
  //首先将触觉器的相对坐标变换为世界坐标
  m_tranSensors = m_Sensors;

  WorldTransform(m_tranSensors, 1);

  //清空上代的触觉器的缓冲区s
  m_vecdSensors.clear();
  m_vecFeelers.clear();
  double k[5];
  //检测每个处决器与其他对象的关系
  for (int sr=0; sr<m_tranSensors.size(); ++sr)
  {
    bool bHit = false;
    //将这一位置的标识置位
    double dist = 0;
      
    for (int seg=0; seg<objects.size(); seg+=2)
    {
      if (LineIntersection2D(SPoint(m_vPosition.x, m_vPosition.y),
                             m_tranSensors[sr],
                             objects[seg],
                             objects[seg+1],
                             dist))
      {
        bHit = true;

        break;
      }
    }
      
    if (bHit)
    {
      m_vecdSensors.push_back(dist);

      //实现非常简单的碰撞检测
      if (dist < CParams::dCollisionDist)
      {
        m_bCollided = true;
      }
    }
    
    else
    {
      m_vecdSensors.push_back(-1);
    } 
	int a=m_tranSensors[sr].x;
	int b=m_tranSensors[sr].y;
	k[sr]=((goalx-a)*(goalx-a)+(goaly-b)*(goaly-b));
  }//下一个触觉器
  int u[5]={1,1,1,1,1};
  for(int o=0;o<5;o++)
  {for(int p=0;p<5;p++)
  {if(k[o]>k[p])
  u[o]-=0.2;
  }
  }
for(int q=0;q<5;q++)
m_vecFeelers.push_back(u[q]);
}

//------------------------- EndOfRunCalculations() -----------------------
//
//------------------------------------------------------------------------
void CMinesweeper::EndOfRunCalculations()
{
  m_dFitness += (1200-sqrt(m_distance));
  m_dFitness += 7000-m_costtime;
  m_dFitness += min((1000-sqrt(m_distance)),m_dCollisionBonus)/30;
  m_dFitness +=min((1000-sqrt(m_distance)),m_dSpinBonus)/20;
}