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)); }