bool OuterCornerPerceptor::searchForBigLAndTL(OuterCorner& outerCorner) const
{
std::vector<const FieldLineIntersections::Intersection*> useBigLIntersections;
for(const FieldLineIntersections::Intersection& intersection : theFieldLineIntersections.intersections)
if(intersection.type == FieldLineIntersections::Intersection::L && intersection.additionalType == FieldLineIntersections::Intersection::big)
useBigLIntersections.push_back(&intersection);
if(useBigLIntersections.empty())
return false;
std::vector<const FieldLineIntersections::Intersection*> useNormalLIntersections;
for(const FieldLineIntersections::Intersection& intersection : theFieldLineIntersections.intersections)
if(intersection.type == FieldLineIntersections::Intersection::L && intersection.additionalType == FieldLineIntersections::Intersection::none)
useNormalLIntersections.push_back(&intersection);
const static float fieldDistance = theFieldDimensions.yPosLeftSideline - theFieldDimensions.yPosLeftPenaltyArea;
for(const FieldLineIntersections::Intersection* intersectionBigL : useBigLIntersections)
{
const Pose2f poseBigL(intersectionBigL->dir1.angle(), intersectionBigL->pos);
const Pose2f inversePoseBigL = poseBigL.inverse();
for(const FieldLineIntersections::Intersection* intersectionSmallL : useNormalLIntersections)
{
if(std::abs((intersectionBigL->pos - intersectionSmallL->pos).norm() - fieldDistance) < thresholdLTIntersections)
{
const Pose2f smallLInBigL = inversePoseBigL * intersectionSmallL->pos;
if(std::abs(smallLInBigL.translation.x()) < allowedDisplacement && smallLInBigL.translation.y() > 0.f
&& std::abs(Angle(intersectionBigL->dir1.angle() - intersectionSmallL->dir2.angle()).normalize()) < allowedAngleDisplacement
&& std::abs(Angle(pi + intersectionBigL->dir2.angle() - intersectionSmallL->dir1.angle()).normalize()) < allowedAngleDisplacement)
{
outerCorner.translation = intersectionBigL->pos;
outerCorner.rotation = intersectionBigL->dir2.angle();
outerCorner.isRightCorner = false;
// outerCorner.markedIntersections.push_back(MarkedIntersection(*intersectionSmallL, MarkedIntersection::STL));
theIntersectionRelations.propagateMarkedIntersection(MarkedIntersection(intersectionBigL->ownIndex, MarkedIntersection::BLL),
theFieldLineIntersections, theFieldLines, outerCorner);
return true;
}
else if(std::abs(smallLInBigL.translation.y()) < allowedDisplacement && smallLInBigL.translation.x() > 0.f
&& std::abs(Angle(pi + intersectionBigL->dir1.angle() - intersectionSmallL->dir2.angle()).normalize()) < allowedAngleDisplacement
&& std::abs(Angle(intersectionBigL->dir2.angle() - intersectionSmallL->dir1.angle()).normalize()) < allowedAngleDisplacement)
{
outerCorner.translation = intersectionBigL->pos;
outerCorner.rotation = intersectionBigL->dir1.angle();
outerCorner.isRightCorner = true;
// outerCorner.markedIntersections.push_back(MarkedIntersection(*intersectionSmallL, MarkedIntersection::STR));
theIntersectionRelations.propagateMarkedIntersection(MarkedIntersection(intersectionBigL->ownIndex, MarkedIntersection::BLR),
theFieldLineIntersections, theFieldLines, outerCorner);
return true;
}
}
}
}
return false;
}
bool GoalFramePerceptor::calcGoalFrame(const Pose2f& prePose, const float yTranslation, GoalFrame& goalFrame) const
{
static const float borderToGroundLineDistance = theFieldDimensions.xPosOpponentFieldBorder - theFieldDimensions.xPosOpponentGroundline;
const Pose2f prePoseInverse(prePose.inverse());
int positive = 0, negative = 0;
for(const Vector2f& p : theFieldBoundary.boundaryOnField)
{
Vector2f pInField = prePoseInverse * p;
if(std::abs(std::abs(pInField.x()) - borderToGroundLineDistance) < allowedFieldBoundaryDivergence)
{
if(pInField.x() > 0)
positive++;
else
negative++;
}
}
if(neededConvexBoundaryPoints > positive && neededConvexBoundaryPoints > negative)
return false;
const float sign = (positive < negative) ? -1.f : 1.f;
goalFrame = Pose2f(prePose).rotate(-pi_2 + sign * pi_2);
goalFrame.translation = goalFrame * Vector2f(0.f, (goalFrame.inverse().translation.y() > 0.f ? 1.f : -1.f)).normalized(yTranslation);
return true;
}