示例#1
0
void pathPlan::goForGoal(btVector3 start, btVector3 end)
{
	memset(&m_startPoint,0,sizeof(rankPoint));
	m_startPoint.point = start;
	memset(&m_goalPoint,0,sizeof(rankPoint));
	m_goalPoint.point = end;
	
	m_state = PS_SEARCHING;
	m_straightDistance = start.distance(end);
	m_progressLimit = m_straightDistance / m_efficiencyLimit;
	m_spinDirection = 0;
	
	displayCurrentSearch(true);								// turn on drawing the yellow search lines
	displayBuildPath(true);
	
	if(m_view) m_view->overlayString(QString("Searching Range %1").arg(m_range));
	
	m_time.start();											// start time of path calculation
	
	if(m_range == 0) {
		m_state = this->AStarSearch();						// if infinite range find the shortest path
	}
	else {
		m_state = this->cycleToGoal();						// step forward on path until the goal is in range

		m_GP.points = m_trailPath + m_GP.points;			// prepend the step trail point list
		m_trailPath.clear();
		m_GP.length = 0;
		for(int i=0; i<m_GP.points.size()-1; i++) 
			m_GP.length += m_GP.points[i].point.distance(m_GP.points[i+1].point);	// calculate the total length
		m_startPoint = m_GP.points[0];						// set the starting point for drawing
	}
	
	if(m_goalOccluded && m_state == PS_COMPLETE) 			// path is complete but the goal is occluded
		m_state = PS_GOALOCCLUDED;							// set the state
	
	m_GP.time = m_time.elapsed();							// get the elapsed time for the path generation
	
	if(m_CS) delete m_CS;									// delete the C Space to free up memory since it is not needed
	m_CS = 0;
	m_pointPath.clear();									// clear out the construction point path
	
	displayCurrentSearch(false);							// turn off search path drawing
	displayBuildPath(false);
	displayPath(true);
			
	if(m_GP.length != 0) m_GP.efficiency = m_straightDistance/m_GP.length;
}
示例#2
0
/**
 * @brief Test collision object if it is in the collision distance from the robot
 *
 * @param point Position of the object center - in the world coordinates
 * @param extent Bounding sphere of the collision object
 * @return
 */
bool srs_env_model::CCMapPlugin::isNearRobot( const btVector3 & point, double extent )
{
	btScalar s( point.distance( m_robotBasePosition ) );

	return s - extent < m_collisionMapLimitRadius;
}