How to use

RecyclingArrayList

in

us.ihmc.robotics.lists

public void setPredictedContactPoints(RecyclingArrayList<Point2d> predictedContactPoints)
{
 this.predictedContactPoints.clear();
 for(int i = 0; i < predictedContactPoints.size(); i++)
   this.predictedContactPoints.add().set(predictedContactPoints.get(i));
}

public FrameTrajectoryPointList(Class<F> frameTrajectoryPointClass)
{
 trajectoryPoints = new RecyclingArrayList<>(frameTrajectoryPointClass);
}

/**
* Returns the last element of this list.
* If the list is empty, it returns {@code null}.
* @return the last element of this list
*/
public T getLast()
{
 if (isEmpty())
   return null;
 else
   return unsafeGet(size - 1);
}

@Override
public void addTimeOffset(double timeOffsetToAdd)
{
 for (int i = 0; i < trajectoryPoints.size(); i++)
   trajectoryPoints.get(i).addTimeOffset(timeOffsetToAdd);
}

@Override
public void addTrajectoryPoint(P trajectoryPoint)
{
 trajectoryPoints.add().set(trajectoryPoint);
}

@Override
public void setWaypoints(ArrayList<FramePoint> waypointPositions)
{
 if (waypointPositions.size() > maxWaypoints)
   throw new RuntimeException("Too many waypoints");
 this.waypointPositions.clear();
 coefficients.clear();
 coefficients.add();
 for (int i = 0; i < waypointPositions.size(); i++)
 {
   waypointPosition.setIncludingFrame(waypointPositions.get(i));
   waypointPosition.changeFrame(swingFrame);
   TDoubleArrayList waypoint = this.waypointPositions.add();
   waypoint.set(0, this.waypointPosition.getX());
   waypoint.set(1, this.waypointPosition.getZ());
   coefficients.add();
 }
 optimizer.setWaypoints(this.waypointPositions);
 segments.set(waypointPositions.size() + 1);
}

@Override
public void clear()
{
 trajectoryPoints.clear();
}

public boolean constructFromInteriorOfRays(List<Line2d> rays, ConvexPolygon2d polygonToPack)
 removeRay.clear();
   removeRay.getAndGrowIfNeeded(i).setValue(false);
   if (removeRay.get(rayIndex).booleanValue())
    if (removeRay.get(previousIndexToCheck).booleanValue())
      continue;    // That ray was removed already. Check a previous ray.
      removeRay.get(previousIndexToCheck).setValue(true);
      removeIntersectionPoint(intersectionPoints, previousIndexToCheck);
      if (removeRay.get(evenMorePreviousIndexToCheck).booleanValue())
       continue;    // That ray was removed already. Check a previous ray.
      Line2d evenMorePreviousRayToCheck = rays.get(evenMorePreviousIndexToCheck);
       removeRay.get(previousIndexToCheck).setValue(true);
       removeIntersectionPoint(intersectionPoints, previousIndexToCheck);

@Override
public F getTrajectoryPoint(int trajectoryPointIndex)
{
 return trajectoryPoints.get(trajectoryPointIndex);
}

if (footstepData.getPredictedContactPoints().isEmpty())
  contactPoints = null;
else
  for (int i = 0; i < footstepData.getPredictedContactPoints().size(); i++)
   contactPoints.add(footstepData.getPredictedContactPoints().get(i));

@Override
public int getNumberOfTrajectoryPoints()
{
 return trajectoryPoints.size();
}

/**
* Add a new element at the end of this list.
* @return the new element.
*/
public T add()
{
 return getAndGrowIfNeeded(size);
}

  @Override
  public boolean isCommandValid()
  {
   return timestamp != -1L && !scan.isEmpty();
  }
}

@Override
public P getLastTrajectoryPoint()
{
 return trajectoryPoints.getLast();
}

@Override
public void subtractTimeOffset(double timeOffsetToSubtract)
{
 for (int i = 0; i < trajectoryPoints.size(); i++)
   trajectoryPoints.get(i).subtractTimeOffset(timeOffsetToSubtract);
}

protected F addAndInitializeTrajectoryPoint()
{
 F newTrajectoryPoint = trajectoryPoints.add();
 newTrajectoryPoint.setToZero(referenceFrame);
 return newTrajectoryPoint;
}

/**
* Resets the optimizer and removes all waypoints as well as previous start and end conditions.
*/
public void reset()
{
 initialPosition.setToNaN(trajectoryFrame);
 initialVelocity.setToNaN(trajectoryFrame);
 finalPosition.setToNaN(trajectoryFrame);
 finalVelocity.setToNaN(trajectoryFrame);
 segments.set(1);
 this.waypointPositions.clear();
 optimizer.setWaypoints(waypointPositions);
 coefficients.clear();
 coefficients.add();
}

public void clear(RobotSide robotSide)
{
 commandId = Packet.VALID_MESSAGE_DEFAULT_ID;
 jointTrajectoryInputs.clear();
 this.robotSide = robotSide;
 executionMode = null;
 previousCommandId = Packet.INVALID_MESSAGE_ID;
}

@Override
public P getTrajectoryPoint(int trajectoryPointIndex)
{
 return trajectoryPoints.get(trajectoryPointIndex);
}

@Override
public int getNumberOfTrajectoryPoints()
{
 return trajectoryPoints.size();
}