maspack.spatialmotion
Class EulerFilter
java.lang.Object
maspack.spatialmotion.EulerFilter
public class EulerFilter
- extends java.lang.Object
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 |
EulerFilter
public EulerFilter()
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) < epsout
- 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)