maspack.spatialmotion
Class EulerFilter

java.lang.Object
  extended by maspack.spatialmotion.EulerFilter

public class EulerFilter
extends java.lang.Object


Constructor Summary
EulerFilter()
           
 
Method Summary
static int filter(double[] ref, double[] rpy, double eps, double[] out)
          Performs an Euler filter to prevent artifacts from gimbal lock.
static double findNearestAngle(double ref, double ang)
          Given an angle ang, find an equivalent angle that is within +/- PI of a given reference angle ref.
static boolean isFlipped(int flag)
           
static boolean isShifted(int flag)
           
 
Methods inherited from class java.lang.Object
equals, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
 

Constructor Detail

EulerFilter

public EulerFilter()
Method Detail

filter

public static int filter(double[] ref,
                         double[] rpy,
                         double eps,
                         double[] out)
Performs an Euler filter to prevent artifacts from gimbal lock. Out of all possible solutions, finds the one that is closest to the reference set of angles.

Parameters:
ref - Reference set of angles {roll, pitch, yaw}
rpy - Current rpy angles to adjust {roll, pitch, yaw}
eps - Threshold to deal with singularity at (yaw - k*Pi/2) < eps
out - Adjusted rpy angles
Returns:
Flag consisting of potentially FLIP_FLAG if angles flipped, or SHIFT_FLAG if yaw/roll shifted

findNearestAngle

public static double findNearestAngle(double ref,
                                      double ang)
Given an angle ang, find an equivalent angle that is within +/- PI of a given reference angle ref.

Parameters:
ref - reference angle (radians)
ang - initial angle (radians)
Returns:
angle equivalent to ang within +/- PI of ref.

isShifted

public static boolean isShifted(int flag)

isFlipped

public static boolean isFlipped(int flag)