/*! @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 Calculates the angular width of the goal (in radians) from the current state */ float Self::CalculateAngularWidthOfGoal(const StationaryObject& goalpost) { std::vector<float> location = CalculateDifferenceFromGoal(goalpost); float distance = location[0]; float bearing = normaliseAngle(location[1] + Heading()); // bearing 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 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)); }