int FieldModel::getIndexOfAssociatedLine(const Pose2D& robotPose, const Vector2<>& start, const Vector2<>& end) const
{
Vector2<> startOnField = robotPose * start;
Vector2<> endOnField = robotPose * end;
Vector2<> dirOnField = endOnField - startOnField;
dirOnField.normalize();
Vector2<> orthogonalOnField(dirOnField.y, -dirOnField.x);
float sqrLineAssociationCorridor = sqr(parameters.lineAssociationCorridor);
Vector2<> intersection, orthogonalProjection;
int index = -1;
for(unsigned int i=0; i<fieldLines.size(); ++i)
{
const FieldLine& fieldLine = fieldLines[i];
if(getSqrDistanceToLine(fieldLine.start, fieldLine.dir, fieldLine.length, startOnField) > sqrLineAssociationCorridor ||
getSqrDistanceToLine(fieldLine.start, fieldLine.dir, fieldLine.length, endOnField) > sqrLineAssociationCorridor)
continue;
if(!intersectLineWithLine(startOnField, orthogonalOnField, fieldLine.start, fieldLine.dir, intersection))
continue;
if(getSqrDistanceToLine(startOnField, dirOnField, intersection) > sqrLineAssociationCorridor)
continue;
if(!intersectLineWithLine(endOnField, orthogonalOnField, fieldLine.start, fieldLine.dir, intersection))
continue;
if(getSqrDistanceToLine(startOnField, dirOnField, intersection) > sqrLineAssociationCorridor)
continue;
if(index != -1) // ambiguous?
{
index = -1;
break;
}
index = i;
}
return index;
}
void BSWalkTo::walkTo(Pose2D target, float speed, bool rough)
{
if(target.translation == Vector2<>())
{
generateMotionRequest(target, speed);
lastAvoidanceAngleOffset = 0;
return;
}
//
RobotPose robotPoseInv = theRobotPose.invert();
obstacleCount = 0;
// the field border is an obstacle
if(theRobotPose.translation.y > leftOppCorner.y - p.fieldBorderAvoidanceDistance)
{
Vector2<> obstaclePos(theRobotPose.translation.x, leftOppCorner.y);
if(obstaclePos.y < theRobotPose.translation.y + 10.f)
obstaclePos.y = theRobotPose.translation.y + 10.f;
obstaclePos = robotPoseInv * obstaclePos;
addObstacle2(obstaclePos, 300.f, p.fieldBorderAvoidanceMinRadius, p.fieldBorderAvoidanceMaxRadius, p.fieldBorderAvoidanceDistance);
}
if(theRobotPose.translation.y < rightOppCorner.y + p.fieldBorderAvoidanceDistance)
{
Vector2<> obstaclePos(theRobotPose.translation.x, rightOppCorner.y);
if(obstaclePos.y > theRobotPose.translation.y - 10.f)
obstaclePos.y = theRobotPose.translation.y - 10.f;
obstaclePos = robotPoseInv * obstaclePos;
addObstacle2(obstaclePos, 300.f, p.fieldBorderAvoidanceMinRadius, p.fieldBorderAvoidanceMaxRadius, p.fieldBorderAvoidanceDistance);
}
if(theRobotPose.translation.x > leftOppCorner.x - p.fieldBorderAvoidanceDistance)
{
Vector2<> obstaclePos(leftOppCorner.x, theRobotPose.translation.y);
if(obstaclePos.x < theRobotPose.translation.x + 10.f)
obstaclePos.x = theRobotPose.translation.x + 10.f;
obstaclePos = robotPoseInv * obstaclePos;
addObstacle2(obstaclePos, 300.f, p.fieldBorderAvoidanceMinRadius, p.fieldBorderAvoidanceMaxRadius, p.fieldBorderAvoidanceDistance);
}
if(theRobotPose.translation.x < leftOwnCorner.x + p.fieldBorderAvoidanceDistance)
{
Vector2<> obstaclePos(leftOwnCorner.x, theRobotPose.translation.y);
if(obstaclePos.x > theRobotPose.translation.x - 10.f)
obstaclePos.x = theRobotPose.translation.x - 10.f;
obstaclePos = robotPoseInv * obstaclePos;
addObstacle2(obstaclePos, 300.f, p.fieldBorderAvoidanceMinRadius, p.fieldBorderAvoidanceMaxRadius, p.fieldBorderAvoidanceDistance);
}
// goal triangles are obstacles
float sqrFieldBorderAvoidanceDistance = sqr(p.fieldBorderAvoidanceDistance);
Vector2<> closestPointOnLine;
if(getSqrDistanceToLine(goalPosts[2], Vector2<>(-1.f, 0.f), 1000.f, theRobotPose.translation, closestPointOnLine) < sqrFieldBorderAvoidanceDistance)
addObstacle2(closestPointOnLine, 300.f, p.fieldBorderAvoidanceMinRadius, p.fieldBorderAvoidanceMaxRadius, p.fieldBorderAvoidanceDistance);
if(getSqrDistanceToLine(goalPosts[3], Vector2<>(-1.f, 0.f), 1000.f, theRobotPose.translation, closestPointOnLine) < sqrFieldBorderAvoidanceDistance)
addObstacle2(closestPointOnLine, 300.f, p.fieldBorderAvoidanceMinRadius, p.fieldBorderAvoidanceMaxRadius, p.fieldBorderAvoidanceDistance);
// goal posts
float sqrGoalPostAvoidanceDistance = p.goalPostAvoidanceDistance * p.goalPostAvoidanceDistance;
for(int i = 0; i < 4; ++i)
if((goalPosts[i] - theRobotPose.translation).squareAbs() < sqrGoalPostAvoidanceDistance)
addObstacle2(robotPoseInv * goalPosts[i], 50.f, p.goalPostAvoidanceMinRadius, p.goalPostAvoidanceMaxRadius, p.goalPostAvoidanceDistance);
// the own penalty area is an obstacle
if(theRobotInfo.number != 1 && theGameInfo.state == STATE_PLAYING)
{
if(theRobotPose.translation.x < leftPenCorner.x + p.penaltyAreaAvoidanceDistance &&
theRobotPose.translation.y < leftPenCorner.y + p.penaltyAreaAvoidanceDistance &&
theRobotPose.translation.y > rightPenCorner.y - p.penaltyAreaAvoidanceDistance) // is near penalty area?
{
// compute "obstacle" pos
const Vector2<> supportPoint(leftPenCorner.x - leftPenCorner.y, 0.f);
Vector2<> obstacleDir = supportPoint - theRobotPose.translation;
obstacleDir.normalize(p.penaltyAreaAvoidanceDistance);
Vector2<> obstaclePos = theRobotPose.translation + obstacleDir;
if(obstaclePos.x < leftPenCorner.x &&
obstaclePos.y < leftPenCorner.y &&
obstaclePos.y > rightPenCorner.y)
{
float xoff = (leftPenCorner.x - obstaclePos.x);
float yoff = (obstaclePos.y > 0.f ? (leftPenCorner.y - obstaclePos.y) : (obstaclePos.y - rightPenCorner.y));
float a = std::min(xoff, yoff);
obstacleDir *= std::max(p.penaltyAreaAvoidanceDistance - a, 1.f) * (1.f / p.penaltyAreaAvoidanceDistance);
if(yoff < xoff)
obstacleDir = Vector2<>(0.f, obstaclePos.y > 0.f ? -obstacleDir.abs() : obstacleDir.abs());
else
obstacleDir = Vector2<>(-obstacleDir.abs(), 0.f);
obstaclePos = theRobotPose.translation + obstacleDir;
}
obstaclePos = robotPoseInv * obstaclePos;
addObstacle2(obstaclePos, 300.f, p.penaltyAreaAvoidanceMinRadius, p.penaltyAreaAvoidanceMaxRadius, p.penaltyAreaAvoidanceDistance);
}
}
// obstacles from ObstacleModel
if(true)//!disableObstacleAvoidance)
for(std::vector<ObstacleModel::Obstacle>::const_iterator it = theObstacleModel.obstacles.begin(), end = theObstacleModel.obstacles.end(); it != end; ++it)
{
const ObstacleModel::Obstacle& obstacle = *it;
const float sqrAbs = obstacle.center.squareAbs();
if(sqrAbs < sqr(p.obstacleAvoidanceDistance))
addObstacle2(obstacle.leftCorner, obstacle.rightCorner, obstacle.center, sqrt(sqrAbs), p.obstacleAvoidanceMinRadius, p.obstacleAvoidanceMaxRadius, p.obstacleAvoidanceDistance);
}
// obstacles from vision
if(true)//!disableObstacleAvoidance)
for(std::vector<RobotsModel::Robot>::const_iterator it = theRobotsModel.robots.begin(), end = theRobotsModel.robots.end(); it != end; ++it)
{
const RobotsModel::Robot& robot = *it;
if(theFrameInfo.getTimeSince(robot.timeStamp) < p.visionObstacleTimeout)
{
const float sqrAbs = robot.relPosOnField.squareAbs();
const float expandLength = robot.standing ? 150.f : 300.f;
float obstacleAvoidanceDistance = p.obstacleAvoidanceDistance/* + expandLength*/;
if(sqrAbs < sqr(obstacleAvoidanceDistance))
{
//const float obstacleDistance = std::max(sqrt(sqrAbs) - expandLength, 1.f); // reducing the distance causes strange avoidance when an opponent robot is close by
Vector2<> obstaclePos = robot.relPosOnField;
//obstaclePos.normalize(obstacleDistance);
addObstacle2(obstaclePos, expandLength, p.obstacleAvoidanceMinRadius, p.obstacleAvoidanceMaxRadius, p.obstacleAvoidanceDistance);
}
}
}
// and... the ball is an obstacle
if(theFrameInfo.getTimeSince(theBallHypotheses.timeWhenDisappeared) < p.ballTimeout)
{
const Vector2<>& ballPosition = theBallModel.estimate.position;
if(ballPosition.squareAbs() < sqr(p.ballAvoidanceDistance))
{
// calculate angle between robot and score position (from ball)
//float alpha = acos((target.translation - ballPosition).normalize() * (-ballPosition).normalize());
//float radius = p.ballAvoidanceMaxRadius * (alpha / pi);
addObstacle2(ballPosition, 35.f, p.ballAvoidanceMinRadius, p.ballAvoidanceMaxRadius, p.ballAvoidanceDistance);
}
}
// find out which obstacles disturb our plan
if(rough)
{
float targetSqrDistance = target.translation.squareAbs();
for(unsigned int i = 0; i < obstacleCount; ++i)
{
Obstacle& obstacle = obstacles[i];
float factor = getOrthogonalProjectionFactor(Vector2<>(), target.translation, obstacle.centerPosition);
if(factor < 0.f || factor > 1.f || (obstacle.leftPosition.squareAbs() > targetSqrDistance && obstacle.rightPosition.squareAbs() > targetSqrDistance))
obstacle.active = false;
}
}
#ifndef TARGET_TOOL
COMPLEX_DRAWING("module:BH2011BehaviorControl:BSWalkTo:Field",
{
for(unsigned int i = 0; i < obstacleCount; ++i)
{
const Obstacle& obstacle = obstacles[i];
if(!obstacle.active)
continue;
Vector2<> base(obstacle.centerPosition.abs(), 0.f);
base.rotate(obstacle.centerAngle);
LINE("module:BH2011BehaviorControl:BSWalkTo:Field", 0, 0, base.x, base.y, 0, Drawings::ps_solid, ColorRGBA(0xff, 0, 0));
for(float j = 0.1f; j < obstacle.openingAngle * 0.5f; j += 0.1f)
{
Vector2<> line = base;
line.rotate(j);
LINE("module:BH2011BehaviorControl:BSWalkTo:Field", 0, 0, line.x, line.y, 0, Drawings::ps_solid, ColorRGBA(0xaa, 0xaa, 0xaa));
line = base;
line.rotate(-j);
LINE("module:BH2011BehaviorControl:BSWalkTo:Field", 0, 0, line.x, line.y, 0, Drawings::ps_solid, ColorRGBA(0xaa, 0xaa, 0xaa));
}
Vector2<> line = base;
line.rotate(obstacle.openingAngle * 0.5f);
LINE("module:BH2011BehaviorControl:BSWalkTo:Field", 0, 0, line.x, line.y, 0, Drawings::ps_solid, ColorRGBA(0, 0, 0));
line = base;
line.rotate(-obstacle.openingAngle * 0.5f);
LINE("module:BH2011BehaviorControl:BSWalkTo:Field", 0, 0, line.x, line.y, 0, Drawings::ps_solid, ColorRGBA(0, 0, 0));
}
});
