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;
}
/**
* @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;
}