us.ihmc.euclid.tools

Class EuclidCoreTools

java.lang.Object

us.ihmc.euclid.tools.EuclidCoreTools

public class EuclidCoreTools
extends java.lang.Object

This class provides a variety of generic tools such as fast square-root algorithm fastSquareRoot(double), or also an linear interpolation algorithm interpolate(double, double, double).

Field Summary

Fields

Modifier and Type	Field and Description
static double	CLAMP_EPS Tolerance used in clamp(double, double, double) to verify the bounds are sane.
static double	EPS_ANGLE_SHIFT An epsilon that is just slightly bigger than numerical inaccuracies used in shiftAngleInRange(double, double).
static double	EPS_NORM_FAST_SQRT Magic number used as a tolerance for switching to the fast square-root formula.
static double	TwoPI This field is used to save unnecessary computation and represents the number 2.0 * Math.PI.

Constructor Summary

Constructors

Constructor and Description
EuclidCoreTools()

Method Summary

Modifier and Type	Method and Description
static double	angleDifferenceMinusPiToPi(double angleA, double angleB) Computes the angle difference: difference = angleA - angleB and shift the result to be contained in [ -pi, pi [.
static double	clamp(double value, double minMax) Clamps value to the given range, defined by -minMax and minMax, inclusive.
static double	clamp(double value, double min, double max) Clamps value to the given range, inclusive.
static boolean	containsNaN(double[] array) Tests if at least one element in the given array is equal to Double.NaN.
static boolean	containsNaN(double a, double b) Tests if at least one of the two given elements is equal to Double.NaN.
static boolean	containsNaN(double a, double b, double c) Tests if at least one of the three given elements is equal to Double.NaN.
static boolean	containsNaN(double a, double b, double c, double d) Tests if at least one of the four given elements is equal to Double.NaN.
static boolean	containsNaN(double a0, double a1, double a2, double a3, double a4, double a5, double a6, double a7, double a8) Tests if at least one of the nine given elements is equal to Double.NaN.
static boolean	epsilonEquals(double expectedValue, double actualValue, double epsilon) Tests if the two values are equal to an epsilon: |expectedValue - actualValue| ≤ epsilon
static double	fastSquareRoot(double squaredValueClosedToOne) Calculates and returns the square root of the given value.
static double	interpolate(double a, double b, double alpha) Performs a linear interpolation from a to b given the percentage alpha.
static double	max(double a, double b, double c) Find and return the argument with the maximum value.
static double	med(double a, double b, double c) Find and return the argument with the value in between the two others.
static double	min(double a, double b, double c) Find and return the argument with the minimum value.
static double	norm(double x, double y) Calculates and returns the norm squared of the given two elements.
static double	norm(double x, double y, double z) Calculates and returns the norm squared of the given three elements.
static double	norm(double x, double y, double z, double s) Calculates and returns the norm squared of the given four elements.
static double	normSquared(double x, double y) Calculates and returns the norm squared of the given two elements.
static double	normSquared(double x, double y, double z) Calculates and returns the norm squared of the given three elements.
static double	normSquared(double x, double y, double z, double s) Calculates and returns the norm squared of the given four elements.
static double	shiftAngleInRange(double angleToShift, double angleStart) Recomputes the angle value angleToShift such that the result is in [angleStart, angleStart + 2pi[ and still represent the same physical angle as angleToShift.
static double	trimAngleMinusPiToPi(double angleToShift) Recomputes the angle value angleToShift such that the result is in [ -pi, pi [ and still represent the same physical angle as angleToShift.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Detail

TwoPI

public static final double TwoPI

This field is used to save unnecessary computation and represents the number 2.0 * Math.PI.

See Also:

Constant Field Values

EPS_NORM_FAST_SQRT

public static final double EPS_NORM_FAST_SQRT

Magic number used as a tolerance for switching to the fast square-root formula.

See Also:

Constant Field Values

EPS_ANGLE_SHIFT

public static final double EPS_ANGLE_SHIFT

An epsilon that is just slightly bigger than numerical inaccuracies used in shiftAngleInRange(double, double).

See Also:

Constant Field Values

CLAMP_EPS

public static final double CLAMP_EPS

Tolerance used in clamp(double, double, double) to verify the bounds are sane.

See Also:

Constant Field Values

Constructor Detail

EuclidCoreTools

public EuclidCoreTools()

Method Detail

fastSquareRoot

public static double fastSquareRoot(double squaredValueClosedToOne)

Calculates and returns the square root of the given value.

This method is optimized when squaredValueClosedToOne is equal to 1+/-2.107342E-8 by using an approximation of the square root.

Parameters:

squaredValueClosedToOne - the value to calculates the square root of.

Returns:

the value of the square root.

containsNaN

public static boolean containsNaN(double a,
                                  double b)

Tests if at least one of the two given elements is equal to Double.NaN.

Parameters:

a - the first element.

b - the second element.

Returns:

true if at least one element is equal to Double.NaN, false otherwise.

containsNaN

public static boolean containsNaN(double a,
                                  double b,
                                  double c)

Tests if at least one of the three given elements is equal to Double.NaN.

Parameters:

a - the first element.

b - the second element.

c - the third element.

Returns:

true if at least one element is equal to Double.NaN, false otherwise.

containsNaN

public static boolean containsNaN(double a,
                                  double b,
                                  double c,
                                  double d)

Tests if at least one of the four given elements is equal to Double.NaN.

Parameters:

a - the first element.

b - the second element.

c - the third element.

d - the fourth element.

Returns:

true if at least one element is equal to Double.NaN, false otherwise.

containsNaN

public static boolean containsNaN(double a0,
                                  double a1,
                                  double a2,
                                  double a3,
                                  double a4,
                                  double a5,
                                  double a6,
                                  double a7,
                                  double a8)

Tests if at least one of the nine given elements is equal to Double.NaN.

Parameters:

a0 - the first element.

a1 - the second element.

a2 - the third element.

a3 - the fourth element.

a4 - the fifth element.

a5 - the sixth element.

a6 - the seventh element.

a7 - the eighth element.

a8 - the ninth element.

Returns:

true if at least one element is equal to Double.NaN, false otherwise.

containsNaN

public static boolean containsNaN(double[] array)

Tests if at least one element in the given array is equal to Double.NaN.

Parameters:

array - the array containing the elements to test for Double.NaN. Not modified.

Returns:

true if at least one element is equal to Double.NaN, false otherwise.

normSquared

public static double normSquared(double x,
                                 double y)

Calculates and returns the norm squared of the given two elements.

norm² = x² + y²

Parameters:

x - the first element.

y - the second element.

Returns:

the value of the square of the norm.

normSquared

public static double normSquared(double x,
                                 double y,
                                 double z)

Calculates and returns the norm squared of the given three elements.

norm² = x² + y² + z²

Parameters:

x - the first element.

y - the second element.

z - the third element.

Returns:

the value of the square of the norm.

normSquared

public static double normSquared(double x,
                                 double y,
                                 double z,
                                 double s)

Calculates and returns the norm squared of the given four elements.

norm² = x² + y² + z² + s²

Parameters:

x - the first element.

y - the second element.

z - the third element.

s - the fourth element.

Returns:

the value of the square of the norm.

norm

public static double norm(double x,
                          double y)

Calculates and returns the norm squared of the given two elements.

norm = √(x² + y²)

This method is optimized for calculating norms closed to 1 by using fastSquareRoot(double). For computing norms that are not closed to 1, prefer using the usual Math.sqrt(double) on the norm squared.

Parameters:

x - the first element.

y - the second element.

Returns:

the value of the square of the norm.

norm

public static double norm(double x,
                          double y,
                          double z)

Calculates and returns the norm squared of the given three elements.

norm = √(x² + y² + z²)

This method is optimized for calculating norms closed to 1 by using fastSquareRoot(double). For computing norms that are not closed to 1, prefer using the usual Math.sqrt(double) on the norm squared.

Parameters:

x - the first element.

y - the second element.

z - the third element.

Returns:

the value of the square of the norm.

norm

public static double norm(double x,
                          double y,
                          double z,
                          double s)

Calculates and returns the norm squared of the given four elements.

norm = √(x² + y² + z² + s²)

This method is optimized for calculating norms closed to 1 by using fastSquareRoot(double). For computing norms that are not closed to 1, prefer using the usual Math.sqrt(double) on the norm squared.

Parameters:

x - the first element.

y - the second element.

z - the third element.

s - the fourth element.

Returns:

the value of the square of the norm.

trimAngleMinusPiToPi

public static double trimAngleMinusPiToPi(double angleToShift)

Recomputes the angle value angleToShift such that the result is in [ -pi, pi [ and still represent the same physical angle as angleToShift.

Edge cases:

• if Math.abs(angleToShift + Math.PI) < EPS_ANGLE_SHIFT, the returned angle is -Math.PI.
• if Math.abs(angleToShift - Math.PI) < EPS_ANGLE_SHIFT, the returned angle is -Math.PI.

Parameters:

angleToShift - the angle to shift.

Returns:

the result that is in [ -pi, pi [

angleDifferenceMinusPiToPi

public static double angleDifferenceMinusPiToPi(double angleA,
                                                double angleB)

Computes the angle difference:
difference = angleA - angleB
and shift the result to be contained in [ -pi, pi [.

Parameters:

angleA - the first angle in the difference.

angleB - the second angle in the difference.

Returns:

the result of the subtraction contained in [ -pi, pi [.

shiftAngleInRange

public static double shiftAngleInRange(double angleToShift,
                                       double angleStart)

Recomputes the angle value angleToShift such that the result is in [angleStart, angleStart + 2pi[ and still represent the same physical angle as angleToShift.

Edge cases:

• if Math.abs(angleToShift - angleStart) < EPS_ANGLE_SHIFT, the returned angle is angleStart.
• if Math.abs(angleToShift - angleStart + 2.0 * Math.PI) < EPS_ANGLE_SHIFT, the returned angle is angleStart.

Parameters:

angleToShift - the angle to shift.

angleStart - the lowest admissible angle value.

Returns:

the result that is in [angleStart, angleStart + 2pi[

max

public static final double max(double a,
                               double b,
                               double c)

Find and return the argument with the maximum value.

Parameters:

a - the first argument to compare.

b - the second argument to compare.

c - the third argument to compare.

Returns:

the maximum value of the three arguments.

min

public static final double min(double a,
                               double b,
                               double c)

Find and return the argument with the minimum value.

Parameters:

a - the first argument to compare.

b - the second argument to compare.

c - the third argument to compare.

Returns:

the minimum value of the three arguments.

med

public static final double med(double a,
                               double b,
                               double c)

Find and return the argument with the value in between the two others.

Parameters:

a - the first argument to compare.

b - the second argument to compare.

c - the third argument to compare.

Returns:

the value in between the two other arguments.

epsilonEquals

public static boolean epsilonEquals(double expectedValue,
                                    double actualValue,
                                    double epsilon)

Tests if the two values are equal to an epsilon:
|expectedValue - actualValue| ≤ epsilon

If any of the two values is equal to Double.NaN, this method fails and returns false.

Parameters:

expectedValue - the first value to compare.

actualValue - the second value to compare.

epsilon - the tolerance to use for the test.

Returns:

true if the two values are considered to be equal, false otherwise.

clamp

public static double clamp(double value,
                           double minMax)

Clamps value to the given range, defined by -minMax and minMax, inclusive.

Parameters:

value - value

minMax - inclusive absolute boundary

Returns:

-minMax if value is less than -minMax

minMax if value is greater than minMax

value if value is between or equal to -minMax and minMax

clamp

public static double clamp(double value,
                           double min,
                           double max)

Clamps value to the given range, inclusive.

Parameters:

value - value

min - inclusive boundary start

max - inclusive boundary end

Returns:

min if value is less than min

max if value is greater than max

value if value is between or equal to min and max

interpolate

public static double interpolate(double a,
                                 double b,
                                 double alpha)

Performs a linear interpolation from a to b given the percentage alpha.

result = (1.0 - alpha) * a + alpha * b

Parameters:

a - the first value used in the interpolation.

b - the second value used in the interpolation.

alpha - the percentage to use for the interpolation. A value of 0 will return a, while a value of 1 will return b.

Returns:

the interpolated value.