Buggy code:
public class QuadrupedSupportPolygon implements Serializable { private static final long serialVersionUID = 4247638266737494462L ; private final QuadrupedSupportPolygon . QuadrantDependentFootstepList footsteps = new QuadrupedSupportPolygon . QuadrantDependentFootstepList ( ) ; private final FrameConvexPolygon2d frameConvexPolygon2d = new FrameConvexPolygon2d ( ) ; private final FramePoint temporaryFramePoint = new FramePoint ( ) ; private final FramePoint temporaryFramePoint2 = new FramePoint ( ) ; private final FrameVector tempPlaneNormalInWorld = new FrameVector ( ) ; private final FrameVector [ ] tempVectorsForInCirclePoint = new FrameVector [ ] { new FrameVector ( ) , new FrameVector ( ) , new FrameVector ( ) , new FrameVector ( ) } ; private final FramePoint [ ] tempPointListForInCirclePoint = new FramePoint [ 4 ] ; private final FramePoint tempInCircleCenter = new FramePoint ( ) ; private final FrameVector [ ] tempVectorsForCommonSupportPolygon = new FrameVector [ ] { new FrameVector ( ) , new FrameVector ( ) } ; public QuadrupedSupportPolygon ( ) { } public QuadrupedSupportPolygon ( QuadrantDependentList < FramePoint > footsteps ) { } public QuadrupedSupportPolygon ( QuadrupedSupportPolygon polygon ) { } private class QuadrantDependentFootstepList extends QuadrantDependentList < FramePoint > { private final FramePoint [ ] framePointsForStorageWhenNull = new FramePoint [ 4 ] ; public QuadrantDependentFootstepList ( ) { } @ Override public void set ( RobotQuadrant robotQuadrant , FramePoint element ) { } @ Override public FramePoint remove ( RobotQuadrant robotQuadrant ) { } @ Override public void clear ( ) { } } public void printOutPolygon ( String string ) { } @ Override public String toString ( ) { } public ReferenceFrame getReferenceFrame ( ) { } public void changeFrame ( ReferenceFrame referenceFrame ) { } public RobotQuadrant [ ] [ ] getLegPairs ( ) { } public int size ( ) { } public RobotQuadrant [ ] getSupportingQuadrantsInOrder ( ) { } public RobotQuadrant getFirstSupportingQuadrant ( ) { } public RobotQuadrant getFirstNonSupportingQuadrant ( ) { } public RobotQuadrant getLastSupportingQuadrant ( ) { } public RobotQuadrant getLastNonSupportingQuadrant ( ) { } public RobotQuadrant getNextClockwiseSupportingQuadrant ( RobotQuadrant robotQuadrant ) { } public RobotQuadrant getNextCounterClockwiseSupportingQuadrant ( RobotQuadrant robotQuadrant ) { } public FramePoint getFootstep ( RobotQuadrant robotQuadrant ) { } public FramePoint getFootstepOrCreateIfNonSupporting ( RobotQuadrant robotQuadrant ) { } public void set ( QuadrupedSupportPolygon polygon ) { } public void setFootstep ( RobotQuadrant robotQuadrant , FramePoint footstep ) { } public QuadrupedSupportPolygon replaceFootstepCopy ( RobotQuadrant quadrant , FramePoint footstep ) { } public void packPolygonWithoutLeg ( RobotQuadrant legToDelete , QuadrupedSupportPolygon quadrupedSupportPolygonToPack ) { } public QuadrupedSupportPolygon swapSameSideFeetCopy ( RobotQuadrant quadrant ) { } public QuadrupedSupportPolygon deleteLegCopy ( RobotQuadrant legName ) { } public void removeFootstep ( RobotQuadrant robotQuadrant ) { } public void translate ( Vector3d translateBy ) { } public void yawAboutCentroid ( double yaw ) { } private boolean containsFootstep ( FramePoint footstep ) { } public boolean containsFootstep ( RobotQuadrant robotQuadrant ) { } public void clear ( ) { } private void computeFramePolygonConvex2d ( ) { } public void packYoFrameConvexPolygon2d ( YoFrameConvexPolygon2d yoFrameConvexPolygon2d ) { } public RobotQuadrant getLowestFootstep ( ) { } public RobotQuadrant getHighestFootstep ( ) { } public RobotQuadrant getClosestFootstep ( FramePoint midPoint ) { } public void getEdgeNormals2d ( FrameVector2d [ ] normalsToPack ) { } public void getCentroid2d ( FramePoint centroidToPack ) { <START_BUG> centroidToPack . setToZero ( ) ; <END_BUG> for ( RobotQuadrant robotQuadrant : getSupportingQuadrantsInOrder ( ) ) { centroidToPack . add ( getFootstep ( robotQuadrant ) ) ; } centroidToPack . scale ( ( 1.0 // ( size ( ) ) ) ) ; } public void getCentroid2d ( FramePoint2d centroidToPack2d ) { } public QuadrupedSupportPolygon getShrunkenPolygon2d ( double shrinkDistance ) { } public double getLowestFootStepZHeight ( ) { } public void getBounds ( Point2d min , Point2d max ) { } public boolean isInside ( FramePoint point ) { } public boolean isInside ( FramePoint2d point ) { } public double distanceInside2d ( FramePoint point ) { } private double computeDistanceToSideOfSegment ( FramePoint point , FramePoint pointOne , FramePoint pointTwo ) { } public double getInCircleRadius2d ( ) { } public double getInCircle2d ( FramePoint inCircleCenterToPack ) { } public void getInCirclePoint2d ( FramePoint intersectionToPack ) { } public int getNumberOfFootstepsOnTopOfEachOther ( ) { } public double distanceInsideInCircle ( FramePoint point ) { } public double getNominalYaw ( ) { } public double getNominalYawHindLegs ( ) { } public double getNominalPitch ( ) { } public double getNominalRoll ( ) { } public double getPitchInRadians ( ) { } public void getAverageFrontFootPosition ( FramePoint framePointToPack ) { } public void getAverageHindFootPosition ( FramePoint framePointToPack ) { } public double [ ] getDistancesToSupportLines ( Point2d point ) { } public boolean hasSameFootsteps ( QuadrupedSupportPolygon polyTwo ) { } public FramePoint getBisectorPoint ( ) { } public double getStanceLength ( RobotSide robotSide ) { } public double getStanceWidthFrontLegs ( ) { } public double getStanceWidthHindLegs ( ) { } public void deleteLegCopy ( RobotQuadrant legName , QuadrupedSupportPolygon quadrupedSupportPolygonToPack ) { } public void getCommonSupportPolygon ( QuadrupedSupportPolygon polygonToCompare , QuadrupedSupportPolygon commonPolygonToPack , RobotQuadrant quadrantForIntersection ) { } public int getNumberOfMatchinfFootsteps ( QuadrupedSupportPolygon polygon ) { } public void getShrunkenCommonSupportPolygon ( QuadrupedSupportPolygon nextSupportPolygon , QuadrupedSupportPolygon shrunkenCommonPolygonToPack , RobotQuadrant quadrantForIntersection , double frontDistance , double sideDistance , double hindDistance ) { } public void getShrunkenPolygon2d ( QuadrupedSupportPolygon shrunkenPolygonToPack , RobotQuadrant sideToShrink , double distance ) { } public RobotQuadrant getSwingLegFromHereToNextPolygon ( QuadrupedSupportPolygon nextSupportPolygon ) { } public double distanceInsideTrotLine ( FramePoint framePoint ) { } public void getTrotLineMiddle ( FramePoint framePointToPack ) { } public void get2DVectorPerpendicularToTrotLinePointingInside ( FrameVector frameVectorToPack ) { } public boolean areLegsCrossing ( ) { } public void replaceFootstepCopy ( RobotQuadrant quadrant , FramePoint footstep , QuadrupedSupportPolygon supportPolygonToPack ) { } public void getDiagonalIntersection ( FramePoint framePointToPack ) { } public boolean isValidTrotPolygon ( ) { } public RobotQuadrant getLeftTrotLeg ( ) { } public RobotQuadrant getRightTrotLeg ( ) { } public boolean getTangentTangentRadiusCircleCenter ( RobotQuadrant robotQuadrantToAnchorTo , double radius , FramePoint2d centerToPack ) { } public void getIntersectionFromPointAcrossTrotLinePerpindicularToOutsideFeet ( RobotQuadrant swingLeg , FramePoint pointAcrossTrotLine , FrameVector planeNormal , FramePoint pointToPack ) { }

Fix:
centroidToPack . setToZero ( ReferenceFrame . getWorldFrame ( ) ) ;