/*! @brief Calculates the difference between the current state and the stationary object
@param theObject the stationary field object you want the relative coordinates
@return the difference between the current field state and theObject, otherwise known as the relative location of theObject [distance, bearing]
*/
std::vector<float> Self::CalculateDifferenceFromStationaryObject(const StationaryObject& theObject)
{
std::vector<float> fieldlocation(2,0);
fieldlocation[0] = theObject.X();
fieldlocation[1] = theObject.Y();
return CalculateDifferenceFromFieldLocation(fieldlocation);
}
/*! @brief Calculate the distance to a stationary object */
float Self::CalculateDistanceToStationaryObject(const StationaryObject& theObject)
{
float selfX = WorldModelLocation[0];
float selfY = WorldModelLocation[1];
float diffX = theObject.X() - selfX;
float diffY = theObject.Y() - selfY;
float distance = sqrt( diffX * diffX + diffY * diffY );
return distance;
}
/*! @brief Calculate the bearing to a stationary object */
float Self::CalculateBearingToStationaryObject(const StationaryObject& theObject)
{
float selfX = WorldModelLocation[0];
float selfY = WorldModelLocation[1];
float selfHeading = WorldModelLocation[2];
float diffX = theObject.X() - selfX;
float diffY = theObject.Y() - selfY;
float positionHeading = atan2(diffY, diffX);
float bearing = normaliseAngle(positionHeading - selfHeading);
return bearing;
}
/*! @brief Calculates the angular width (in radians) of the goal from the mobile object */
float Self::CalculateAngularWidthOfGoalFromMobileObject(const StationaryObject& goalpost, const MobileObject& mobileobject)
{
float mobileX = mobileobject.X();
float mobileY = mobileobject.Y();
float goalX = goalpost.X();
float goalY = 0;
float diffX = goalX - mobileX;
float diffY = goalY - mobileY;
if( (diffX == 0) && (diffY == 0)) diffY = 0.0001;
float distance = sqrt( diffX * diffX + diffY * diffY );
float bearing = atan2(diffY, diffX); // bearing FROM the mobile object to the goal (from the field x-axis)
float width = 2*fabs(goalpost.Y());
// python: angularwidth = numpy.arctan2(width*numpy.cos(bearing), 2*distance + width*numpy.sin(bearing)) + numpy.arctan2(width*numpy.cos(bearing), 2*distance - width*numpy.sin(bearing))
float angularwidth = atan2(width*cos(bearing), 2*distance + width*sin(bearing)) + atan2(width*cos(bearing), 2*distance - width*sin(bearing));
return angularwidth;
}
/*! @brief Calculates the position to stand in to protect the goal from a mobile object
@return [x,y] of the position relative to the current state
*/
std::vector<float> Self::CalculatePositionToProtectGoalFromMobileObject(const MobileObject& mobileobject, const StationaryObject& goalpost, float blockingwidth)
{
vector<float> prediction = CalculateClosestInterceptToMobileObject(mobileobject);
if (prediction[0] < 4 and mobileobject.estimatedDistance() > 30)
{ // if the ball is moving go to where the ball will be!
vector<float> prediction_position(2,0);
prediction_position[0] = prediction[1];
prediction_position[1] = prediction[2];
#if DEBUG_BEHAVIOUR_VERBOSITY > 1
debug << "protectGoal Predicated x:" << prediction_position[0] << " y: " << prediction_position[1] << " ballx: " << ball.estimatedDistance()*cos(heading) << " bally: " << ball.estimatedDistance()*sin(heading) << endl;
#endif
return prediction_position;
}
else
{
float goal_angular_width = fabs(CalculateAngularWidthOfGoalFromMobileObject(goalpost, mobileobject));
float goal_width = 2*fabs(goalpost.Y());
float distancebetween = sqrt(pow(mobileobject.X() - goalpost.X(), 2) + pow(mobileobject.Y(),2));
float distancefrommobile = (0.5*blockingwidth)/tan(0.5*goal_angular_width);
vector<float> position(2,0);
if (distancebetween < 0.7*goal_width)
{ // if the mobile object is inside the goal then the calculation will break --- just go to the mobile object
float b_r = mobileobject.estimatedDistance()*cos(mobileobject.estimatedElevation()); // get the flat distance!
float b_b = mobileobject.estimatedBearing();
position[0] = b_r*cos(b_b);
position[1] = b_r*sin(b_b);
return position;
}
// clip the distancefrommobile so that we don't go back into the goal.
if (distancebetween - distancefrommobile < 0.7*goal_width)
distancefrommobile = distancebetween - 0.7*goal_width;
return CalculatePositionBetweenMobileObjectAndGoal(mobileobject, goalpost, distancefrommobile);
}
}
void locWmGlDisplay::drawStationaryObjectLabel(const StationaryObject& object)
{
QString displayString("(%1,%2)");
renderText(object.X(), object.Y(),1,displayString.arg(object.measuredDistance(),0,'f',1).arg(object.measuredBearing(),0,'f',3));
}
/*! @brief Calculates the difference from a goal
@return the difference between the current position and the goal [distance, bearing] */
std::vector<float> Self::CalculateDifferenceFromGoal(const StationaryObject& goalpost)
{
std::vector<float> fieldlocation(2,0);
fieldlocation[0] = goalpost.X();
return CalculateDifferenceFromFieldLocation(fieldlocation);
}
