Package gaiasky.util.gaia.utils

Class AttitudeUtils

java.lang.Object
gaiasky.util.gaia.utils.AttitudeUtils
public class AttitudeUtils extends 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

    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 Details

    • AttitudeUtils

      public AttitudeUtils()

  • Method Details

    • 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 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:
      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