How to use us.ihmc.robotics

@Override
public boolean epsilonEquals(Sphere3d other, double epsilon)
{
 return MathTools.epsilonEquals(radius, other.radius, epsilon);
}

public void setEpsilonToShrink(double epsilon)
{
 MathTools.checkIfNegative(epsilon);
 this.epsilon = epsilon;
}

private void computeOther(double value)
{
 k++;
 sum = sum + value;
 
 double mKLast = mK;
 double kDouble = (double)k;
 mK = mKLast + (value - mKLast) / kDouble;
 qK = qK + (kDouble - 1.0) * MathTools.square(value - mKLast) / kDouble;
}

private static double getCosineAngleWithCosineLaw(double l_neighbour1, double l_neighbour2, double l_opposite)
{
 checkIfInRange(l_neighbour1, 0.0, Double.POSITIVE_INFINITY);
 checkIfInRange(l_neighbour2, 0.0, Double.POSITIVE_INFINITY);
 checkIfInRange(l_opposite, 0.0, Double.POSITIVE_INFINITY);
 double cosAngle = MathTools.clipToMinMax((square(l_neighbour1) + square(l_neighbour2) - square(l_opposite)) / (2.0 * l_neighbour1 * l_neighbour2), -1.0,
                      1.0);
 return cosAngle;
}

public HandCollisionDetectedPacket(RobotSide robotSide, int collisionSeverityLevelZeroToThree)
{
 this.robotSide = robotSide;
 this.collisionSeverityLevelOneToThree = MathTools.clipToMinMax(collisionSeverityLevelZeroToThree, 1, 3);
}

public static double getCosineAngleDotWithCosineLaw(double l_neighbour1, double l_neighbour2, double lDot_neighbour2, double l_opposite,
   double lDot_opposite)
{
 checkIfInRange(l_neighbour1, 0.0, Double.POSITIVE_INFINITY);
 checkIfInRange(l_neighbour2, 0.0, Double.POSITIVE_INFINITY);
 checkIfInRange(l_opposite, 0.0, Double.POSITIVE_INFINITY);
 double cosAngleDot = (square(l_neighbour2) * lDot_neighbour2 - 2.0 * l_neighbour2 * l_opposite * lDot_opposite - lDot_neighbour2 * square(l_neighbour1)
    + lDot_neighbour2 * square(l_opposite)) / (2.0 * square(l_neighbour2) * l_neighbour1);
 return cosAngleDot;
}

public void enableWeightMethod(double maxWeight, double minWeight)
{
 MathTools.checkIfInRange(minWeight, 1.0-3, 100.0);
 MathTools.checkIfInRange(maxWeight, 1.0-3, 100.0);
 this.useWeightMethod = true;
 double weightRatio = maxWeight / minWeight;
 this.minWeight = 1.0;
 this.maxWeight = weightRatio;
}

  public static PlanarRegionsList importPlanarRegionData(File dataFolder)
  {
   return PlanarRegionFileTools.importPlanarRegionData(dataFolder);
  }
}

/**
* Generates a default timestamped name that can be used to generate automated and unique
* folders.
*
* @return a {@code String} of the form: "20171201_163422_PlanarRegion".
*/
public static String createDefaultTimeStampedFolderName()
{
 return getDate() + "_PlanarRegion";
}

public void setEpsilonToGrow(double epsilon)
{
 MathTools.checkIfPositive(epsilon);
 this.epsilon = epsilon;
}

public static double getCosineAngleWithCosineLaw(double l_neighbour1, double l_neighbour2, double l_opposite)
{
 checkIfInRange(l_neighbour1, 0.0, Double.POSITIVE_INFINITY);
 checkIfInRange(l_neighbour2, 0.0, Double.POSITIVE_INFINITY);
 checkIfInRange(l_opposite, 0.0, Double.POSITIVE_INFINITY);
 double cosAngle = MathTools
    .clipToMinMax((square(l_neighbour1) + square(l_neighbour2) - square(l_opposite)) / (2.0 * l_neighbour1 * l_neighbour2), -1.0, 1.0);
 return cosAngle;
}

@Override
public boolean epsilonEquals(LegCompliancePacket other, double epsilon)
{
 if (maxVelocityDeltas.length != other.maxVelocityDeltas.length)
   return false;
 for (int i = 0; i < maxVelocityDeltas.length; i++)
   if (!MathTools.epsilonEquals(maxVelocityDeltas[i], other.maxVelocityDeltas[i], epsilon))
    return false;
 return true;
}

public void setTrajectoryTime(double trajectoryTime)
{
 //      this.trajectoryTime = trajectoryTime;
 // limiting motor speed for safe joint speed. if the arms exceed (700 rad / sec) / (100 gear ratio) = 7 rad/sec we are in trouble
 this.trajectoryTime = MathTools.clipToMinMax(trajectoryTime, 2.0, Double.MAX_VALUE);
}

/**
* Generates a default timestamped name that can be used to generate automated and unique
* folders.
*
* @return a {@code String} of the form: "20171201_163422_VizGraphs".
*/
public static String createDefaultTimeStampedDatasetFolderName()
{
 return PlanarRegionFileTools.getDate() + "_" + VIZ_GRAPHS_DATA_FOLDER_SUFFIX;
}

@Override
public boolean epsilonEquals(PilotAlarmPacket other, double epsilon)
{
 return MathTools.epsilonEquals(beepRate, other.beepRate, epsilon);
}

public static double computeAlphaGivenBreakFrequency(double breakFrequencyInHetrz, double dt)
{
 double alpha = 1.0 - 4.0 * dt * breakFrequencyInHetrz;
 alpha = MathTools.clipToMinMax(alpha, 0.0, 1.0);
 return alpha;
}

@Override
public boolean epsilonEquals(WristSensorNoisePacket other, double epsilon)
{
  return
     MathTools.epsilonEquals(leftWristNoise,  other.leftWristNoise, epsilon ) &&
     MathTools.epsilonEquals(rightWristNoise, other.rightWristNoise, epsilon) ;
}

@Override
public boolean epsilonEquals(PelvisPoseErrorPacket other, double epsilon)
{
 return (MathTools.epsilonEquals(residualError, other.residualError, epsilon)
   && MathTools.epsilonEquals(totalError, other.totalError, epsilon)
   && other.hasMapBeenReset == hasMapBeenReset);
}

@Override
public boolean epsilonEquals(Cylinder3d other, double epsilon)
{
 return MathTools.epsilonEquals(height, other.height, epsilon) && MathTools.epsilonEquals(radius, other.radius, epsilon);
}

@Override
public boolean epsilonEquals(Torus3d other, double epsilon)
{
 return MathTools.epsilonEquals(radius, other.radius, epsilon) && MathTools.epsilonEquals(tubeRadius, other.tubeRadius, epsilon);
}