polygon_set transform_figure(polygon_set ps, float A[4][4]) {
size_t s = ps.size();
polygon_set to_return(s);
for (int i = 0 ; i < s ; i++) {
to_return[i] = POLYGON(transform(std::get<0>(ps.at(i)), A),
transform(std::get<1>(ps.at(i)), A),
transform(std::get<2>(ps.at(i)), A));
}
return to_return;
}
void RobotPose::draw(bool teamRed)
{
DECLARE_DEBUG_DRAWING("representation:RobotPose", "drawingOnField");
Vector2<> bodyPoints[4] = {Vector2<>(55, 90),
Vector2<>(-55, 90),
Vector2<>(-55, -90),
Vector2<>(55, -90)
};
for(int i = 0; i < 4; i++)
bodyPoints[i] = *this * bodyPoints[i];
Vector2<> dirVec(200, 0);
dirVec = *this * dirVec;
const ColorRGBA ownTeamColorForDrawing = teamRed ? ColorRGBA(255, 0, 0) : ColorRGBA(0, 0, 255);
LINE("representation:RobotPose", translation.x, translation.y, dirVec.x, dirVec.y,
20, Drawings::ps_solid, ColorClasses::white);
POLYGON("representation:RobotPose", 4, bodyPoints, 20, Drawings::ps_solid,
ownTeamColorForDrawing, Drawings::bs_solid, ColorClasses::white);
CIRCLE("representation:RobotPose", translation.x, translation.y, 42, 0,
Drawings::ps_solid, ownTeamColorForDrawing, Drawings::bs_solid, ownTeamColorForDrawing);
DECLARE_DEBUG_DRAWING("representation:RobotPose:deviation", "drawingOnField");
if(deviation < 100000.f)
DRAWTEXT("representation:RobotPose:deviation", -3000, -2300, 10, ColorRGBA(0xff, 0xff, 0xff), "pose deviation: " << deviation);
else
DRAWTEXT("representation:RobotPose:deviation", -3000, -2300, 10, ColorRGBA(0xff, 0xff, 0xff), "pose deviation: unknown");
DECLARE_DEBUG_DRAWING3D("representation:RobotPose", "field",
{
LINE3D("representation:RobotPose", translation.x, translation.y, 10,
dirVec.x, dirVec.y, 10, 1, ownTeamColorForDrawing);
for(int i = 0; i < 4; ++i)
{
const Vector2<> p1 = bodyPoints[i];
const Vector2<> p2 = bodyPoints[(i + 1) & 3];
LINE3D("representation:RobotPose", p1.x, p1.y, 10,
p2.x, p2.y, 10, 1, ownTeamColorForDrawing);
}
});
polygon transform(polygon p, float A[4][4]) {
return POLYGON(transform(GET_X(p), A),
transform(GET_Y(p), A),
transform(GET_Z(p), A));
}
