public MomentumVisualizer(String name, TwistCalculator twistCalculator, YoVariableRegistry registry, YoGraphicsListRegistry graphicsRegistry, RigidBody... rigidBodies) { comCalculator = new CenterOfMassCalculator(rigidBodies, ReferenceFrame.getWorldFrame()); momentumCalculator = new MomentumCalculator(twistCalculator, rigidBodies); centerOfMass = new YoFramePoint(name + "CoM", ReferenceFrame.getWorldFrame(), registry); linearMomentum = new YoFrameVector(name + "Momentum", ReferenceFrame.getWorldFrame(), registry); YoGraphicPosition yoCoMGraphics = new YoGraphicPosition(name + "CoM", centerOfMass, 0.05, YoAppearance.Brown()); YoGraphicVector yoMomentumGraphics = new YoGraphicVector(name + "Momentum", centerOfMass, linearMomentum, 0.05, YoAppearance.Brown()); graphicsRegistry.registerYoGraphic(name, yoCoMGraphics); graphicsRegistry.registerYoGraphic(name, yoMomentumGraphics); }
double yEnd = centerY + halfSize; terrain.addBox(xStart, yStart, xEnd, yEnd, 0.8, YoAppearance.Brown()); terrain.addBox(xStart, yStart, xStart + edgeThick, yEnd, 0.8 + edgeHeight, YoAppearance.Brown()); terrain.addBox(xEnd - edgeThick, yStart, xEnd, yEnd, 0.8 + edgeHeight, YoAppearance.Brown()); terrain.addBox(xStart + edgeThick, yStart, xEnd - edgeThick, yStart + edgeThick, 0.8 + edgeHeight, YoAppearance.Brown()); terrain.addBox(xStart + edgeThick, yEnd - edgeThick, xEnd - edgeThick, yEnd, 0.8 + edgeHeight, YoAppearance.Brown());
polygonSnapperVisualizer.addPlanarRegionsList(planarRegionsList, YoAppearance.Gold(), YoAppearance.Purple(), YoAppearance.Brown(), YoAppearance.Blue(), YoAppearance.Chartreuse());