Class AttitudeUtils


  • public class AttitudeUtils
    extends java.lang.Object
    Class with static utility methods to deal with B-splines.
    • Nested Class Summary

      Nested Classes 
      Modifier and Type Class Description
      static class  AttitudeUtils.KnotsAndSplines
      Utility class for manipulating knots and splines together
    • Constructor Summary

      Constructors 
      Constructor Description
      AttitudeUtils()  
    • Method Summary

      All Methods Static Methods Concrete Methods Deprecated Methods 
      Modifier and Type Method Description
      static void calcBsplines​(long x, long[] tau, int splineOrder, int leftIndex, double[] b0, double[] b1)
      Returns the values and first derivatives of the four non-zero cubic B-splines in the interval tau(left) <= x < tau(left+1)
      static int findLeftIndex​(long x, long[] xa, int splineOrder)
      In the non-decreasing sequence xa[0:n-1], finds the left index such that xa[left] <= x < xa[left+1] If x < xa[0] the method returns left = -1.
      static int findLeftIndexBisection​(int x, int[] xa)
      In the non-decreasing sequence xa[0:n-1], finds the left index such that xa[left] <= x < xa[left+1] If x < xa[0] the method returns left = -1.
      static int findLeftIndexBisection​(long x, long[] xa)
      In the non-decreasing sequence xa[0:n-1], finds the left index such that xa[left] <= x < xa[left+1] If x < xa[0] the method returns left = -1.
      static int findLeftIndexBisectionVar​(int x, int[] xa)
      Variant of findLeftIndexBisection which returns left = xa.length - 2 if x == xa[xa.length-1].
      static int findLeftIndexBisectionVar​(long x, long[] xa)
      Variant of findLeftIndexBisection which returns left = xa.length - 2 if x == xa[xa.length-1].
      static int findLeftIndexBSpline​(long x, long[] xa)
      Variant of findLeftIndex to be used for BSpline interpolation.
      static int findLeftIndexVar​(long x, long[] xa, int splineOrder)
      Variant of findLeftIndex which returns left = xa.length - 2 if x == xa[xa.length-1].
      static double[] insertElements​(double[] oldElements, long[] knots, long[] tInsert, int multiplicity, int splineOrder)
      Insert one or more elements in an array with a certain multiplicity This will redefine a larger array of the same duration.
      static long[] insertKnots​(long[] oldKnots, long[] tInsert, int multiplicity, int splineOrder)
      Insert one or more knots in a knot sequence with a certain multiplicity This will redefine a larger knot sequence of the same duration
      static AttitudeUtils.KnotsAndSplines insertKnotsAndSplines​(AttitudeUtils.KnotsAndSplines old, long[] tInsert, int multiplicity, int splineOrder)
      Insert one or more knots in a knot sequence with a certain multiplicity This will redefine a larger knot sequence of the same duration Additionally, corresponding spline coefficients are inserted with zero as their starting values.
      static double[] smallAngularDifferences​(double[] q0, double[] q1)
      Deprecated.
      • Methods inherited from class java.lang.Object

        clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
    • Constructor Detail

      • AttitudeUtils

        public AttitudeUtils()
    • Method Detail

      • findLeftIndex

        public static int findLeftIndex​(long x,
                                        long[] xa,
                                        int splineOrder)
        In the non-decreasing sequence xa[0:n-1], finds the left index such that xa[left] <= x < xa[left+1] If x < xa[0] the method returns left = -1. If x >= xa[n-1] the method returns left = n-1 (the last valid index to the array). These are the logical extensions of the basic condition when x is outside the interval defined by the array - formally they could be derived by putting xa[-1] = -inf and xa[n] = +inf. This implementation of findLeftIndex uses an estimation method which assumes that the intervals have the same or similar length to speed up the search, this method is faster than the bisection one at least when there aren't many gaps and the time intervals are identical or similar.
        Parameters:
        x - value to find the left index for
        xa - array of spline knot times
        splineOrder - order of the spline
        Returns:
        The left index
      • findLeftIndexVar

        public static int findLeftIndexVar​(long x,
                                           long[] xa,
                                           int splineOrder)
        Variant of findLeftIndex which returns left = xa.length - 2 if x == xa[xa.length-1]. This allows linear or cubic Hermite interpolation between xa[left] and xa[left+1] also in the case when x is exactly equal to the last point in the array.
        Parameters:
        x - value to find the left index for
        xa - array of spline knot times
        splineOrder - order of the spline
        Returns:
        left index
      • findLeftIndexBSpline

        public static int findLeftIndexBSpline​(long x,
                                               long[] xa)
        Variant of findLeftIndex to be used for BSpline interpolation. In general the value of left is determined such that * xa[left] and xa[left+1] exist * xa[left+1] - xa[left] > 0 * xa[left] <= x < xa[left+1] More specific, this means that if x > xa[xa.length - 1], then the BSpline cannot be evaluated so -1 is returned. if x == xa[xa.length-1] then left is determined such that left is the largest value for which xa[left] < xa[xa.length-n] for the largest value of n > 0 for which xa[xa.length-n] == xa[xa.length-1]. for all xa[0] < x < xa[xa.length-1] left is determined such that xa[left] <= x < xa[left+1]. If then xa[left] == xa[left-1] <= x then n is determined to be the smallest value for which xa[left-n] < xa[left] and then left - n is returned. if xa[left] <= x and left == 0, then n is determined to be the largest value for which xa[left] == xa[left+n] and then left + n is returned. if x < xa[0], then the BSpline cannot be evaluated so -1 is returned. For more info see the discussion in Mantis 27523.
        Parameters:
        x - value to find the left index for
        xa - array of spline knot times
        Returns:
        left index
      • findLeftIndexBisection

        public static int findLeftIndexBisection​(long x,
                                                 long[] xa)
        In the non-decreasing sequence xa[0:n-1], finds the left index such that xa[left] <= x < xa[left+1] If x < xa[0] the method returns left = -1. If x >= xa[n-1] the method returns left = n-1 (the last valid index to the array). These are the logical extensions of the basic condition when x is outside the interval defined by the array - formally they could be derived by putting xa[-1] = -inf and xa[n] = +inf. This method uses a straight bisection method to locate the left index.
        Parameters:
        xa - array with knot times
        x - value to find the left index for
        Returns:
        left index
      • findLeftIndexBisectionVar

        public static int findLeftIndexBisectionVar​(long x,
                                                    long[] xa)
        Variant of findLeftIndexBisection which returns left = xa.length - 2 if x == xa[xa.length-1]. This allows linear or cubic Hermite interpolation between xa[left] and xa[left+1] also in the case when x is exactly equal to the last point in the array.
        Parameters:
        x - value to find the left index for
        xa - array of spline knot times
        Returns:
        left index
      • findLeftIndexBisection

        public static int findLeftIndexBisection​(int x,
                                                 int[] xa)
        In the non-decreasing sequence xa[0:n-1], finds the left index such that xa[left] <= x < xa[left+1] If x < xa[0] the method returns left = -1. If x >= xa[n-1] the method returns left = n-1 (the last valid index to the array). These are the logical extensions of the basic condition when x is outside the interval defined by the array - formally they could be derived by putting xa[-1] = -inf and xa[n] = +inf. This method uses a straight bisection method to locate the left index.
        Parameters:
        xa - array with knot times
        x - value to find the left index for
        Returns:
        left index
      • findLeftIndexBisectionVar

        public static int findLeftIndexBisectionVar​(int x,
                                                    int[] xa)
        Variant of findLeftIndexBisection which returns left = xa.length - 2 if x == xa[xa.length-1]. This allows linear or cubic Hermite interpolation between xa[left] and xa[left+1] also in the case when x is exactly equal to the last point in the array.
        Parameters:
        x - value to find the left index for
        xa - array of spline knot times
        Returns:
        left index
      • calcBsplines

        public static void calcBsplines​(long x,
                                        long[] tau,
                                        int splineOrder,
                                        int leftIndex,
                                        double[] b0,
                                        double[] b1)
                                 throws java.lang.RuntimeException
        Returns the values and first derivatives of the four non-zero cubic B-splines in the interval tau(left) <= x < tau(left+1)

        Based on the subroutine BSPLVB in C. de Boor, A Practical Guide to Splines, Springer 1978

        Calculation of derivatives was added (perhaps inefficiently) as the analytical derivative of each statement in the original BSPLVB routine, using that (d/dx)deltar(j) = -1 and (d/dx)deltal(j) = +1.

        The order of the spline is set by the parameter ATT_SPLINE_ORDER (=4 for cubic)

        Parameters:
        tau - knot sequence
        x - point at which the B-splines should be evaluated
        splineOrder - order of the spline
        leftIndex - integer chosen (usually) such that tau(leftIndex) <= x < tau(leftIndex+1) (left can be found by findLeftIndex(long, long[], int)
        b0 - values of the cubic B-splines at point x
        b1 - first derivatives (wrt x) of the B-splines at point x
        Throws:
        java.lang.RuntimeException - if input is inconsistent
      • smallAngularDifferences

        @Deprecated
        public static double[] smallAngularDifferences​(double[] q0,
                                                       double[] q1)
        Deprecated.
        Deprecated : Use the Quaternion class instead and this method is not reliable E.g. double[] angleDiff = q1.smallAngularDifference(q2); using quaternions E.g. double[] angleDiff = new Quaternion(q1).smallAngularDifference(new Quaternion(q2)); using double arrays Compute the angular differences about the principal axes of two body-triads represented by two quaternions. This calculation comes from SAG-LL-30 where an inertial rotation vector between to attitudes represented by quaternion q0 and q1 is derived. It is a SMALL ANGLE approximation. If the algorithm finds a difference>EPS between the components of the 2 quaternions, it will flip sign of q1. This is vain if the angular difference is too large and a warning is then logged. Note this is a static utility.
        Parameters:
        q0 - First quaternion
        q1 - Second quaternion
        Returns:
        Array of length 3 with angle between principal axes TODO: Move this the Quaternion class
      • insertKnots

        public static long[] insertKnots​(long[] oldKnots,
                                         long[] tInsert,
                                         int multiplicity,
                                         int splineOrder)
        Insert one or more knots in a knot sequence with a certain multiplicity This will redefine a larger knot sequence of the same duration
        Parameters:
        oldKnots - The initial array of knot times
        tInsert - The array of knot insert times
        multiplicity - The multiplicity of the inserted knots
        splineOrder - spline order
        Returns:
        knots The updated knot times, including inserted knots of multiplicity
      • insertKnotsAndSplines

        public static AttitudeUtils.KnotsAndSplines insertKnotsAndSplines​(AttitudeUtils.KnotsAndSplines old,
                                                                          long[] tInsert,
                                                                          int multiplicity,
                                                                          int splineOrder)
        Insert one or more knots in a knot sequence with a certain multiplicity This will redefine a larger knot sequence of the same duration Additionally, corresponding spline coefficients are inserted with zero as their starting values. These zero values need to be replaced by a fit or updated values.
        Parameters:
        old - The initial array of knot times The initial array of spline coefficients
        tInsert - The array of knot insert times
        multiplicity - The multiplicity of the inserted knots
        Returns:
        knots The updated knot times, including inserted knots of multiplicity The updated spline coefficients, including inserted splines at knots of multiplicity
      • insertElements

        public static double[] insertElements​(double[] oldElements,
                                              long[] knots,
                                              long[] tInsert,
                                              int multiplicity,
                                              int splineOrder)
        Insert one or more elements in an array with a certain multiplicity This will redefine a larger array of the same duration. This is designed to work with a knot sequence.
        Parameters:
        oldElements - The initial array at of elements at knot times
        knots - The knot sequence which is not changed
        tInsert - The array of insert times
        multiplicity - The multiplicity of the inserted elements
        splineOrder - spline order
        Returns:
        elements The updated elements including inserted knots of multiplicity