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 OuterCornerPerceptor::searchForBigLAndT(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*> useNormalTIntersections;
for(const FieldLineIntersections::Intersection& intersection : theFieldLineIntersections.intersections)
if(intersection.type == FieldLineIntersections::Intersection::T && intersection.additionalType == FieldLineIntersections::Intersection::none)
useNormalTIntersections.push_back(&intersection);
const float fieldDistance = theFieldDimensions.yPosLeftSideline - theFieldDimensions.yPosLeftPenaltyArea;
for(const FieldLineIntersections::Intersection* intersectionL : useBigLIntersections)
for(const FieldLineIntersections::Intersection* intersectionT : useNormalTIntersections)
{
if(std::abs((intersectionL->pos - intersectionT->pos).norm() - fieldDistance) < thresholdLTIntersections)
{
if(std::abs(Angle(intersectionT->dir2.angle() - intersectionL->dir1.angle()).normalize()) < thesholdAngleDisForLTIntersections
|| std::abs(Angle(intersectionT->dir2.angle() + pi - intersectionL->dir1.angle()).normalize()) < thesholdAngleDisForLTIntersections)
{
outerCorner.translation = intersectionL->pos;
outerCorner.rotation = intersectionL->dir1.angle();
outerCorner.isRightCorner = Angle(intersectionL->dir2.angle() - intersectionL->dir1.angle()).normalize() > 0;
// outerCorner.markedIntersections.push_back(MarkedIntersection(*intersectionT, outerCorner.isRightCorner ? MarkedIntersection::STR : MarkedIntersection::STL));
theIntersectionRelations.propagateMarkedIntersection(MarkedIntersection(intersectionL->ownIndex, outerCorner.isRightCorner ? MarkedIntersection::BLR : MarkedIntersection::BLL),
theFieldLineIntersections, theFieldLines, outerCorner);
return true;
}
else if(std::abs(Angle(intersectionT->dir2.angle() - intersectionL->dir2.angle()).normalize()) < thesholdAngleDisForLTIntersections
|| std::abs(Angle(intersectionT->dir2.angle() + pi - intersectionL->dir2.angle()).normalize()) < thesholdAngleDisForLTIntersections)
{
outerCorner.translation = intersectionL->pos;
outerCorner.rotation = intersectionL->dir2.angle();
outerCorner.isRightCorner = Angle(intersectionL->dir1.angle() - intersectionL->dir2.angle()).normalize() > 0;
// outerCorner.markedIntersections.push_back(MarkedIntersection(*intersectionT, outerCorner.isRightCorner ? MarkedIntersection::STR : MarkedIntersection::STL));
theIntersectionRelations.propagateMarkedIntersection(MarkedIntersection(intersectionL->ownIndex, outerCorner.isRightCorner ? MarkedIntersection::BLR : MarkedIntersection::BLL),
theFieldLineIntersections, theFieldLines, outerCorner);
return true;
}
}
}
return false;
}
bool MidCirclePerceptor::searchWithSXAndT(MidCircle& midCircle) const
{
std::vector<const FieldLineIntersections::Intersection*> useSmallXIntersections;
for(const FieldLineIntersections::Intersection& intersection : theFieldLineIntersections.intersections)
if(intersection.type == FieldLineIntersections::Intersection::X && intersection.additionalType == FieldLineIntersections::Intersection::none)
useSmallXIntersections.push_back(&intersection);
if(useSmallXIntersections.empty())
return false;
static const float distance = theFieldDimensions.yPosLeftSideline - theFieldDimensions.centerCircleRadius ;
std::vector<const FieldLineIntersections::Intersection*> useTIntersections;
for(const FieldLineIntersections::Intersection& intersection : theFieldLineIntersections.intersections)
if(intersection.type == FieldLineIntersections::Intersection::T)
useTIntersections.push_back(&intersection);
for(const FieldLineIntersections::Intersection* intersectionSX : useSmallXIntersections)
for(const FieldLineIntersections::Intersection* intersectionT : useTIntersections)
if(intersectionT->indexDir1 == intersectionSX->indexDir1 || intersectionT->indexDir1 == intersectionSX->indexDir2)
if(std::abs((intersectionT->pos - intersectionSX->pos).norm() - distance) < allowedTsXVariance)
{
const Vector2f dirTToX = intersectionSX->pos - intersectionT->pos;
midCircle.translation = intersectionSX->pos + dirTToX.normalized(theFieldDimensions.centerCircleRadius);
midCircle.rotation = Angle(dirTToX.angle() + pi_2).normalize();// OR intersectionT->dir2.angle();
midCircle.markedPoints.emplace_back(midCircle.translation, MarkedPoint::midCircle);
theIntersectionRelations.propagateMarkedIntersection(MarkedIntersection(intersectionT->ownIndex, MarkedIntersection::BT),
theFieldLineIntersections, theFieldLines, midCircle);
return true;
}
return false;
}
bool OuterCornerPerceptor::searchForLAndPA(OuterCorner& outerCorner) const
{
if(!thePenaltyArea.isValid)
return false;
std::vector<const FieldLineIntersections::Intersection*> useLIntersections;
for(const FieldLineIntersections::Intersection& intersection : theFieldLineIntersections.intersections)
if(intersection.type == FieldLineIntersections::Intersection::L)
useLIntersections.push_back(&intersection);
if(useLIntersections.empty())
return false;
for(const FieldLineIntersections::Intersection* lIntersection : useLIntersections)
{
const Vector2f intersectionInPA = thePenaltyArea.inverse() * lIntersection->pos;
if(intersectionInPA.x() < 0.f)
continue;
const static float xOffsetPA = (theFieldDimensions.xPosOpponentGroundline - theFieldDimensions.xPosOpponentPenaltyArea) / 2.f;
if(intersectionInPA.y() < 0) //check for right corner
{
const static Vector2f rightCornerInPa = Vector2f(xOffsetPA, theFieldDimensions.yPosRightSideline);
const Vector2f displacement = intersectionInPA - rightCornerInPa;
if((displacement).squaredNorm() > squaredThresholdDisOffsetToPA ||
std::abs(displacement.x()) > allowedDisplacement || std::abs(displacement.y()) > allowedDisplacement)
continue;
const Angle postulatedCornerAngle = thePenaltyArea.rotation + 90_deg;
Angle rotation;
if(std::abs((rotation = lIntersection->dir1.angle()) - postulatedCornerAngle) < thresholdPostulatedCornerAngleOffset)
{
if(Angle(lIntersection->dir2.angle() - rotation).normalize() < 0.f)
continue;
outerCorner.translation = lIntersection->pos;
outerCorner.rotation = rotation;
outerCorner.isRightCorner = true;
theIntersectionRelations.propagateMarkedIntersection(MarkedIntersection(lIntersection->ownIndex, MarkedIntersection::BLR),
theFieldLineIntersections, theFieldLines, outerCorner);
return true;
}
else if(std::abs((rotation = lIntersection->dir2.angle()) - postulatedCornerAngle) < thresholdPostulatedCornerAngleOffset)
{
if(Angle(lIntersection->dir1.angle() - rotation).normalize() < 0.f)
continue;
outerCorner.translation = lIntersection->pos;
outerCorner.rotation = rotation;
outerCorner.isRightCorner = true;
theIntersectionRelations.propagateMarkedIntersection(MarkedIntersection(lIntersection->ownIndex, MarkedIntersection::BLR),
theFieldLineIntersections, theFieldLines, outerCorner);
return true;
}
}
else //check for left corner
{
const static Vector2f leftCornerInPa = Vector2f(xOffsetPA, theFieldDimensions.yPosLeftSideline);
const Vector2f displacement = intersectionInPA - leftCornerInPa;
if((displacement).squaredNorm() > squaredThresholdDisOffsetToPA ||
std::abs(displacement.x()) > allowedDisplacement || std::abs(displacement.y()) > allowedDisplacement)
continue;
const float postulatedCornerAngle = thePenaltyArea.rotation - 90_deg;
Angle rotation;
if(std::abs((rotation = lIntersection->dir1.angle()) - postulatedCornerAngle) < thresholdPostulatedCornerAngleOffset)
{
if(Angle(lIntersection->dir2.angle() - rotation).normalize() > 0.f)
continue;
outerCorner.translation = lIntersection->pos;
outerCorner.rotation = rotation;
outerCorner.isRightCorner = false;
theIntersectionRelations.propagateMarkedIntersection(MarkedIntersection(lIntersection->ownIndex, MarkedIntersection::BLL),
theFieldLineIntersections, theFieldLines, outerCorner);
return true;
}
else if(std::abs((rotation = lIntersection->dir2.angle()) - postulatedCornerAngle) < thresholdPostulatedCornerAngleOffset)
{
if(Angle(lIntersection->dir1.angle() - rotation).normalize() > 0.f)
continue;
outerCorner.translation = lIntersection->pos;
outerCorner.rotation = rotation;
outerCorner.isRightCorner = false;
theIntersectionRelations.propagateMarkedIntersection(MarkedIntersection(lIntersection->ownIndex, MarkedIntersection::BLL),
theFieldLineIntersections, theFieldLines, outerCorner);
return true;
}
}
}
return false;
}
