public SingleFootstepVisualizer(RobotSide robotSide, int maxContactPoints, YoVariableRegistry registry, YoGraphicsListRegistry yoGraphicsListRegistry)
{
Integer index = indices.get(robotSide);
String namePrefix = robotSide.getLowerCaseName() + "Foot" + index;
YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix);
this.robotSide = robotSide;
ArrayList<Point2d> polyPoints = new ArrayList<Point2d>();
yoContactPoints = new YoFramePoint[maxContactPoints];
for (int i = 0; i < maxContactPoints; i++)
{
yoContactPoints[i] = new YoFramePoint(namePrefix + "ContactPoint" + i, ReferenceFrame.getWorldFrame(), registry);
yoContactPoints[i].set(0.0, 0.0, -1.0);
YoGraphicPosition baseControlPointViz = new YoGraphicPosition(namePrefix + "Point" + i, yoContactPoints[i], 0.01, YoAppearance.Blue());
yoGraphicsList.add(baseControlPointViz);
polyPoints.add(new Point2d());
}
footPolygon = new YoFrameConvexPolygon2d(namePrefix + "yoPolygon", "", ReferenceFrame.getWorldFrame(), maxContactPoints, registry);
footPolygon.setConvexPolygon2d(new ConvexPolygon2d(polyPoints));
soleFramePose = new YoFramePose(namePrefix + "polygonPose", "", ReferenceFrame.getWorldFrame(), registry);
soleFramePose.setXYZ(0.0, 0.0, -1.0);
footPolygonViz = new YoGraphicPolygon(namePrefix + "graphicPolygon", footPolygon, soleFramePose, 1.0, footPolygonAppearances.get(robotSide));
yoGraphicsList.add(footPolygonViz);
if (yoGraphicsListRegistry != null)
{
yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList);
yoGraphicsListRegistry.registerGraphicsUpdatableToUpdateInAPlaybackListener(footPolygonViz);
}
index++;
indices.set(robotSide, index);
}