How to use

sort

method

in

us.ihmc.tools.lists.PairList

private void concatenatePathWaypoints()
{
 if (completedPathWaypoints.isEmpty())
 {
   waypointPlan.set(null);
   return;
 }
 PairList<Integer, List<Point3D>> completedPathWaypoints = this.completedPathWaypoints.getCopyForReading();
 completedPathWaypoints.sort(Comparator.comparingInt(ImmutablePair<Integer, List<Point3D>>::getLeft));
 List<Point3D> allWaypoints = new ArrayList<>();
 for (ImmutablePair<Integer, List<Point3D>> completedWaypoints : completedPathWaypoints)
 {
   allWaypoints.addAll(completedWaypoints.getRight());
 }
 waypointPlan.set(allWaypoints);
}

private void concatenateFootstepPlans()
{
 if (completedStepPlans.isEmpty())
 {
   footstepPlan.set(null);
   return;
 }
 PairList<Integer, FootstepPlan> completedStepPlans = this.completedStepPlans.getCopyForReading();
 completedStepPlans.sort(Comparator.comparingInt(ImmutablePair<Integer, FootstepPlan>::getLeft));
 int firstPlan = Integer.MAX_VALUE;
 FootstepPlan totalFootstepPlan = new FootstepPlan();
 for (ImmutablePair<Integer, FootstepPlan> footstepPlanPairs : completedStepPlans)
 {
   FootstepPlan footstepPlan = footstepPlanPairs.getRight();
   int number = footstepPlanPairs.getLeft();
   if (footstepPlan.hasLowLevelPlanGoal() && number < firstPlan)
   {
    totalFootstepPlan.setLowLevelPlanGoal(footstepPlan.getLowLevelPlanGoal());
    firstPlan = number;
   }
   totalFootstepPlan.addFootstepPlan(footstepPlan);
 }
 footstepPlan.set(totalFootstepPlan);
}