rcs.posemath
Class Posemath

java.lang.Object
  extended by rcs.posemath.Posemath

public class Posemath
extends java.lang.Object

Class created only for its static functions to convert and manipulate the pose representations in the rest of the package. Adapted from _posemath.c and posemath.cc.


Field Summary
static double CART_FUZZ
           
static double DBL_MAX
           
static boolean debug_on
           
static double DOUBLE_FUZZ
           
static double DOUBLECP_FUZZ
           
static double E_EPSILON
           
static PmRotationMatrix last_bad_rotmat
           
static double PI
           
static double PI_2
           
static double PI_4
           
static double PM_2_PI
           
static int PM_DIV_ERR
           
static int PM_ERR
           
static int PM_IMPL_ERR
           
static int PM_NORM_ERR
           
static double PM_PI
           
static double PM_PI_2
           
static double PM_PI_4
           
static boolean PM_PRINT_ERROR
           
static int PM_X
           
static int PM_Y
           
static int PM_Z
           
static int pmErrno
           
static double Q_FUZZ
           
static double QS_FUZZ
           
static double QSIN_FUZZ
           
static double RPY_P_FUZZ
           
static double RS_FUZZ
           
static double SINGULAR_EPSILON
           
static double SQRT_FUZZ
           
static double UNIT_QUAT_FUZZ
           
static double UNIT_SC_FUZZ
           
static double UNIT_VEC_FUZZ
           
static double V_FUZZ
           
static double ZYX_Y_FUZZ
           
static double ZYZ_Y_FUZZ
           
 
Constructor Summary
Posemath()
           
 
Method Summary
static PM_CARTESIAN add(PM_CARTESIAN v1, PM_CARTESIAN v2)
           
static PmCartesian add(PmCartesian v1, PmCartesian v2)
           
static double[] cartListToMagArray(java.util.List<? extends PM_CARTESIAN> clin)
           
static PM_CARTESIAN centroid(java.util.List<? extends PM_CARTESIAN> l)
           
static PM_CARTESIAN centroid(PM_CARTESIAN[] l)
           
static double[] computeLineFitErrors(PM_LINE line, java.util.List<? extends PmCartesian> l)
          Compute an array of errors or the distance between each point in list l to line line.
static PM_CARTESIAN cross(PM_CARTESIAN v1, PM_CARTESIAN v2)
           
static PM_CYLINDRICAL cross(PM_CYLINDRICAL v1, PM_CYLINDRICAL v2)
           
static void csvSync(java.io.File f1, java.io.File f2, java.io.PrintStream psOut)
           
static double[][] csvToHomMats(java.lang.String filename, int x_pos, int y_pos, int z_pos, int roll_pos, int pitch_pos, int yaw_pos)
           
static double[][] csvToHomMatsChk(java.lang.String filename, int x_pos, int y_pos, int z_pos, int roll_pos, int pitch_pos, int yaw_pos)
           
static java.util.List<java.lang.Double> csvToList(java.lang.String filename, int pos)
           
static java.util.List<java.lang.Double> csvToListF(java.io.File f, int pos)
           
static java.util.List<PM_POSE> csvToPoseList(java.lang.String filename, int x_pos, int y_pos, int z_pos, int roll_pos, int pitch_pos, int yaw_pos)
           
static java.util.List<PM_POSE> csvToPoseListF(java.io.File f, int x_pos, int y_pos, int z_pos, int roll_pos, int pitch_pos, int yaw_pos)
           
static java.util.List<PM_POSE> csvWithTimeToPoseList(java.lang.String filename, java.util.List<java.lang.Double> timeList, int time_pos, int x_pos, int y_pos, int z_pos, int roll_pos, int pitch_pos, int yaw_pos)
           
static java.util.List<PM_POSE> csvWithTimeToPoseListF(java.io.File f, java.util.List<java.lang.Double> timeList, int time_pos, int x_pos, int y_pos, int z_pos, int roll_pos, int pitch_pos, int yaw_pos)
           
static double disp(PM_CARTESIAN v1, PM_CARTESIAN v2)
           
static double dist_from_line(PM_LINE line, PM_CARTESIAN c)
           
static double distFromPath(PM_CARTESIAN p, PM_CARTESIAN[] waypoints)
          Return the minimum distance of the point p from the path specified by the array of waypoints.
static PM_CARTESIAN divide(PM_CARTESIAN v, double d)
           
static PM_CYLINDRICAL divide(PM_CYLINDRICAL c, double d)
           
static PM_QUATERNION divide(PM_QUATERNION q, double s)
           
static PM_ROTATION_VECTOR divide(PM_ROTATION_VECTOR r, double s)
           
static double dot(PM_CARTESIAN v1, PM_CARTESIAN v2)
           
static double dot(PM_CYLINDRICAL v1, PM_CYLINDRICAL v2)
           
static double dot(PmCartesian v1, PmCartesian v2)
           
static java.util.List<PM_POSE> findStillPoints(java.util.List<java.lang.Double> tlist, java.util.List<PM_POSE> posList, double velThresh, int avg_thresh, java.util.List<java.lang.Double> stillTimes)
           
static void findStillTimes(java.lang.String[] args)
           
static PM_CIRCLE fitCircle2D_XY(java.util.List<? extends PmCartesian> l)
          Ignores the z value in each element of the list and returns a circle fit with least-squares error to x,y points
static PM_LINE fitLine(java.util.List<? extends PmCartesian> l)
          Ignores the z value in each element of the list and returns a line fit with least-squares error to x,y points
static java.util.List<PM_LINE> fitLines(java.util.List<? extends PM_CARTESIAN> l, double tolerance, int max_lines)
           
static PM_POSE getCk1Pose()
           
static PM_POSE getCk2Pose()
           
static java.util.List<PM_POSE> homMatsChk(double[][] X, double[][] A, double[][] Y, double[][] B)
           
static java.util.List<PM_POSE> homMatsPrePostMult(double[][] X, double[][] A, double[][] Y)
           
static java.util.List<PM_POSE> homMatsToPoseList(double[][] dd)
           
static boolean in_line_segment(PM_LINE l, PM_CARTESIAN c1, double tol)
           
static PM_CARTESIAN intersection(PM_LINE l1, PM_LINE l2)
           
static PM_CARTESIAN inv(PM_CARTESIAN v)
           
static PM_CYLINDRICAL inv(PM_CYLINDRICAL v)
           
static PM_HOMOGENEOUS inv(PM_HOMOGENEOUS h)
           
static PM_POSE inv(PM_POSE p)
           
static PM_QUATERNION inv(PM_QUATERNION q1)
           
static PM_XYA inv(PM_XYA p)
           
static boolean IS_FUZZ(double a, double fuzz)
           
static boolean isNorm(PM_CARTESIAN v)
           
static boolean isNorm(PM_CYLINDRICAL v)
           
static boolean isNorm(PM_QUATERNION q1)
           
static boolean isNorm(PM_ROTATION_MATRIX m)
           
static boolean isNorm(PM_ROTATION_VECTOR r)
           
static double mag(PM_CARTESIAN v)
           
static double mag(PM_CYLINDRICAL v1)
           
static double mag(PM_QUATERNION q)
           
static double mag(PmCartesian v)
           
static void main(java.lang.String[] args)
           
static double maxA(double[] da)
           
static double mean(double[] da)
           
static double mean(java.lang.Double[] da)
           
static double mean(java.util.List<java.lang.Double> l)
           
static PM_CARTESIAN multiply(double d, PM_CARTESIAN v)
           
static PM_CYLINDRICAL multiply(double d, PM_CYLINDRICAL c)
           
static PM_QUATERNION multiply(double s, PM_QUATERNION q)
           
static PmRotationVector multiply(double s, PM_ROTATION_VECTOR r)
           
static PmCartesian multiply(double d, PmCartesian v)
           
static PM_CARTESIAN multiply(PM_CARTESIAN v, double d)
           
static PM_CYLINDRICAL multiply(PM_CYLINDRICAL c, double d)
           
static PM_CARTESIAN multiply(PM_POSE p1, PM_CARTESIAN v2)
           
static PM_CARTESIAN[] multiply(PM_POSE pose, PM_CARTESIAN[] l)
           
static PM_POSE multiply(PM_POSE p1, PM_POSE p2)
           
static PM_QUATERNION multiply(PM_QUATERNION q, double s)
           
static PM_QUATERNION multiply(PM_QUATERNION q1, PM_QUATERNION q2)
           
static PM_CARTESIAN multiply(PM_ROTATION_MATRIX m, PM_CARTESIAN v)
           
static PM_ROTATION_MATRIX multiply(PM_ROTATION_MATRIX m1, PM_ROTATION_MATRIX m2)
           
static PM_ROTATION_VECTOR multiply(PM_ROTATION_VECTOR r, double s)
           
static java.util.ArrayList<PM_CARTESIAN> multiply(PM_XYA xya, java.util.List<? extends PM_CARTESIAN> l)
           
static PM_CARTESIAN multiply(PM_XYA p1, PM_CARTESIAN p2)
           
static PM_CARTESIAN[] multiply(PM_XYA xya, PM_CARTESIAN[] l)
           
static PM_XYA multiply(PM_XYA p1, PM_XYA p2)
           
static PmCartesian multiply(PmCartesian v, double d)
           
static PM_CARTESIAN multiply(PmQuaternion q1, PM_CARTESIAN v2)
           
static PM_CARTESIAN neg(PM_CARTESIAN v)
           
static PM_CYLINDRICAL neg(PM_CYLINDRICAL v)
           
static PM_CARTESIAN norm(PM_CARTESIAN v)
           
static PM_CYLINDRICAL norm(PM_CYLINDRICAL v)
           
static PM_QUATERNION norm(PM_QUATERNION q1)
           
static PM_ROTATION_VECTOR norm(PM_ROTATION_VECTOR r)
           
static int pmAxisAngleQuatConvert(int axis, double a, PmQuaternion q)
          Store a quaternion based on a single rotation about an axis
static int pmCartCartAdd(PmCartesian v1, PmCartesian v2, PmCartesian vout)
           
static boolean pmCartCartCompare(PmCartesian v1, PmCartesian v2)
           
static int pmCartCartCross(PmCartesian v1, PmCartesian v2, PmCartesian vout)
           
static double pmCartCartDisp(PmCartesian v1, PmCartesian v2)
           
static double pmCartCartDot(PmCartesian v1, PmCartesian v2)
           
static int pmCartCartProj(PmCartesian v1, PmCartesian v2, PmCartesian vout)
           
static int pmCartCartSub(PmCartesian v1, PmCartesian v2, PmCartesian vout)
           
static PmCartesian pmCartCentroid(java.util.List<? extends PmCartesian> l)
           
static PmCartesian pmCartCentroid(PmCartesian[] l)
           
static int pmCartCylConvert(PmCartesian v, PmCylindrical c)
           
static int pmCartInv(PmCartesian v1, PmCartesian vout)
           
static boolean pmCartIsNorm(PmCartesian v)
           
static double pmCartMag(PmCartesian v)
           
static int pmCartNeg(PmCartesian v1, PmCartesian vout)
           
static int pmCartNorm(PmCartesian v, PmCartesian vout)
           
static int pmCartPlaneProj(PmCartesian v, PmCartesian normal, PmCartesian vout)
           
static int pmCartScalDiv(PmCartesian v1, double d, PmCartesian vout)
           
static int pmCartScalMult(PmCartesian v1, double d, PmCartesian vout)
           
static int pmCartSphConvert(PmCartesian v, PmSpherical s)
          Convert a PmCartesian to PmSpherical
static int pmCircleInit(PmCircle circle, PmPose start, PmPose end, PmCartesian center, PmCartesian normal, int turn)
           
static int pmCirclePoint(PmCircle circle, double angle, PmPose point)
           
static boolean pmClose(double a, double b, double eps)
          Determine if a within eps of b.
static int pmCylCartConvert(PmCylindrical c, PmCartesian v)
          Convert cylindrical to cartesian.
static int pmCylCylAdd(PmCylindrical c1, PmCylindrical c2, PmCylindrical cout)
           
static boolean pmCylCylCompare(PmCylindrical v1, PmCylindrical v2)
           
static int pmCylCylCross(PmCylindrical c1, PmCylindrical c2, PmCylindrical cout)
           
static double pmCylCylDot(PmCylindrical c1, PmCylindrical c2)
           
static int pmCylCylSub(PmCylindrical c1, PmCylindrical c2, PmCylindrical cout)
           
static int pmCylInv(PmCylindrical c, PmCylindrical cout)
           
static boolean pmCylIsNorm(PmCylindrical v)
           
static double pmCylMag(PmCylindrical c)
           
static int pmCylNeg(PmCylindrical v1, PmCylindrical vout)
           
static int pmCylNorm(PmCylindrical v, PmCylindrical vout)
           
static int pmCylScalDiv(PmCylindrical v1, double d, PmCylindrical vout)
           
static int pmCylScalMult(PmCylindrical v1, double d, PmCylindrical vout)
           
static int pmCylSphConvert(PmCylindrical c, PmSpherical s)
          Convert a cylindrical to a spherical
static int pmHomInv(PmHomogeneous h1, PmHomogeneous h2)
           
static int pmHomPoseConvert(PmHomogeneous h, PmPose p)
           
static int pmLineInit(PmLine line, PmPose start, PmPose end)
           
static int pmLinePoint(PmLine line, double len, PmPose point)
           
static int pmMatCartMult(PmRotationMatrix m, PmCartesian v, PmCartesian vout)
           
static int pmMatInv(PmRotationMatrix m, PmRotationMatrix mout)
           
static boolean pmMatIsNorm(PmRotationMatrix m)
           
static int pmMatMatMult(PmRotationMatrix m1, PmRotationMatrix m2, PmRotationMatrix mout)
           
static int pmMatNorm(PmRotationMatrix m, PmRotationMatrix mout)
           
static int pmMatQuatConvert(PmRotationMatrix m, PmQuaternion q)
           
static int pmMatRotConvert(PmRotationMatrix m, PmRotationVector r)
           
static int pmMatRpyConvert(PmRotationMatrix m, PmRpy rpy)
           
static int pmMatZyxConvert(PmRotationMatrix m, PmEulerZyx zyx)
           
static int pmMatZyzConvert(PmRotationMatrix m, PmEulerZyz zyz)
           
static int pmPoseCartMult(PmPose p1, PmCartesian v2, PmCartesian vout)
           
static int pmPoseHomConvert(PmPose p, PmHomogeneous h)
           
static int pmPoseInv(PmPose p1, PmPose p2)
           
static boolean pmPosePoseCompare(PmPose p1, PmPose p2)
           
static int pmPosePoseMult(PmPose p1, PmPose p2, PmPose pout)
           
static PmQuaternion pmQuatAxisAngleMult(PmQuaternion q, int axis, double angle)
           
static int pmQuatAxisAngleMult(PmQuaternion q, int axis, double angle, PmQuaternion pq)
           
static int pmQuatCartMult(PmQuaternion q1, PmCartesian v2, PmCartesian vout)
           
static int pmQuatInv(PmQuaternion q1, PmQuaternion qout)
           
static boolean pmQuatIsNorm(PmQuaternion q1)
           
static double pmQuatMag(PmQuaternion q)
           
static int pmQuatMatConvert(PmQuaternion q, PmRotationMatrix m)
           
static int pmQuatNorm(PmQuaternion q1, PmQuaternion qout)
           
static boolean pmQuatQuatCompare(PmQuaternion q1, PmQuaternion q2)
           
static int pmQuatQuatMult(PmQuaternion q1, PmQuaternion q2, PmQuaternion qout)
           
static int pmQuatRotConvert(PmQuaternion q, PmRotationVector r)
          Convert a quaternion to a rotation vector
static int pmQuatRpyConvert(PmQuaternion q, PmRpy rpy)
           
static int pmQuatScalDiv(PmQuaternion q, double s, PmQuaternion qout)
           
static int pmQuatScalMult(PmQuaternion q, double s, PmQuaternion qout)
           
static int pmQuatZyxConvert(PmQuaternion q, PmEulerZyx zyx)
           
static int pmQuatZyzConvert(PmQuaternion q, PmEulerZyz zyz)
           
static boolean pmRotIsNorm(PmRotationVector r)
           
static int pmRotMatConvert(PmRotationVector r, PmRotationMatrix m)
          Convert a rotation vector to a rotation matrix.
static int pmRotNorm(PmRotationVector r, PmRotationVector rout)
           
static int pmRotQuatConvert(PmRotationVector r, PmQuaternion q)
          Convert a rotation vector to a quaternion
static int pmRotRpyConvert(PmRotationVector r, PmRpy rpy)
           
static int pmRotScalDiv(PmRotationVector r, double s, PmRotationVector rout)
           
static int pmRotScalMult(PmRotationVector r, double s, PmRotationVector rout)
           
static int pmRpyMatConvert(PmRpy rpy, PmRotationMatrix m)
           
static int pmRpyQuatConvert(PmRpy rpy, PmQuaternion q)
           
static int pmRpyRotConvert(PmRpy rpy, PmRotationVector r)
           
static int pmRpyZyxConvert(PmRpy rpy, PmEulerZyx zyx)
           
static int pmRpyZyzConvert(PmRpy rpy, PmEulerZyz zyz)
           
static int pmSphCartConvert(PmSpherical s, PmCartesian v)
          Convert PmSherical to PmCartesian
static int pmSphCylConvert(PmSpherical s, PmCylindrical c)
          Convert a spherical to cylindrical
static double pmSq(double x)
          Square of x or x*x.
static double pmSqrt(double x)
          Square root function that adds some extra checks similiar to the C++ pmSqrt() primarily only exists for converting C++ apps.
static int pmZyxMatConvert(PmEulerZyx zyx, PmRotationMatrix m)
           
static int pmZyxQuatConvert(PmEulerZyx zyx, PmQuaternion q)
           
static int pmZyxRotConvert(PmEulerZyx zyx, PmRotationVector r)
           
static int pmZyxRpyConvert(PmEulerZyx zyx, PmRpy rpy)
           
static int pmZyxZyzConvert(PmEulerZyx zyx, PmEulerZyz zyz)
           
static int pmZyzMatConvert(PmEulerZyz zyz, PmRotationMatrix m)
           
static int pmZyzQuatConvert(PmEulerZyz zyz, PmQuaternion q)
           
static int pmZyzRotConvert(PmEulerZyz zyz, PmRotationVector r)
           
static int pmZyzRpyConvert(PmEulerZyz zyz, PmRpy rpy)
           
static PM_CARTESIAN point_on_line_segment(PM_CARTESIAN s, PM_CARTESIAN e, PM_CARTESIAN p)
           
static PM_CARTESIAN point_on_line(PM_CARTESIAN s, PM_CARTESIAN u, PM_CARTESIAN p)
           
static PM_CARTESIAN point_on_line(PM_LINE line, PM_CARTESIAN p)
           
static PmCartesian point_on_line(PM_LINE line, PmCartesian p)
           
static java.util.List<PM_POSE> poseListChk(PM_POSE X, java.util.List<? extends PM_POSE> A, PM_POSE Y, java.util.List<? extends PM_POSE> B)
           
static java.util.List<PM_POSE> poseListInv(java.util.List<? extends PM_POSE> plin)
           
static java.util.List<PM_POSE> poseListPoseListMultiply(java.util.List<? extends PM_POSE> pl1, java.util.List<? extends PM_POSE> pl2)
           
static java.util.List<PM_POSE> poseListRandom(int size, double angle_noise, double cart_noise)
           
static java.util.List<PM_CARTESIAN> poseListToCartList(java.util.List<? extends PM_POSE> plin)
           
static double[][] poseListToHomMats(java.util.List<? extends PM_POSE> l)
           
static java.util.List<PM_RPY> poseListToRpyList(java.util.List<? extends PM_POSE> plin)
           
static java.util.List<PM_POSE> prePostMultiplyList(java.util.List<? extends PM_POSE> l, PM_POSE pre, PM_POSE post)
           
static void printCorrelationShifts(java.lang.String fname1, int f1, java.lang.String fname2, int f2, double start_shift, double end_shift, double inc, java.io.PrintStream ps)
           
static PM_CARTESIAN proj(PM_CARTESIAN v1, PM_CARTESIAN v2)
           
static double[] rpyListToMagArray(java.util.List<? extends PM_RPY> rlin)
           
static double shiftedSumOfProducts(java.util.List<java.lang.Double> l1, java.util.List<java.lang.Double> l2, double dshift)
           
static java.util.List<PM_LINE> splitToLines(java.util.List<? extends PM_CARTESIAN> l, int num_lines, double[] max_error, int[] best_indexes)
           
static double stddev(double[] da)
           
static double stddev(java.lang.Double[] da)
           
static double stddev(java.util.List<java.lang.Double> da)
           
static PM_CARTESIAN subtract(PM_CARTESIAN v1, PM_CARTESIAN v2)
           
static PmCartesian subtract(PmCartesian v1, PmCartesian v2)
           
static PM_CARTESIAN toCart(PM_CYLINDRICAL v)
          Convert a cylindrical to a cartesian
static PM_CARTESIAN toCart(PM_SPHERICAL v)
          Convert PM_SPHERICAL to PM_CARTESIAN
static PM_CYLINDRICAL toCyl(PM_SPHERICAL s)
          Convert a spherical to cylindrical
static PM_HOMOGENEOUS toHom(PM_POSE p)
           
static PM_ROTATION_MATRIX toMat(PM_QUATERNION v)
          Convert a quaternion to a rotation matrix.
static PM_ROTATION_MATRIX toMat(PM_ROTATION_VECTOR v)
          Convert rotation vector to rotation matrix
static PM_POSE toPose(double[][] dd)
           
static PM_POSE toPose(PM_HOMOGENEOUS h)
           
static PM_QUATERNION toQuat(PM_EULER_ZYX v)
           
static PM_QUATERNION toQuat(PM_EULER_ZYZ v)
           
static PM_QUATERNION toQuat(PM_ROTATION_MATRIX m)
           
static PM_QUATERNION toQuat(PM_ROTATION_VECTOR v)
          Convert a rotation vector to a quaternion
static PM_QUATERNION toQuat(PM_RPY v)
           
static PM_ROTATION_VECTOR toRot(PM_QUATERNION v)
          Convert quaternion to a rotation vector
static PM_ROTATION_VECTOR toRot(PM_RPY rpy)
           
static PM_RPY toRpy(PM_QUATERNION v)
           
static PM_RPY toRpy(PmRotationMatrix m)
           
static PM_SPHERICAL toSph(PM_CARTESIAN v)
          Convert a PM_CARTESIAN to PM_SPHERICAL
static PM_SPHERICAL toSph(PM_CYLINDRICAL c)
          Convert a cylindrical to a spherical
static double[][] tstArray()
           
static PM_CARTESIAN weightedAvg(PM_CARTESIAN c1, double s1, PM_CARTESIAN c2, double s2)
          Adds two PM_CARTESIANS and returns a weightedAvg ie.
static PM_POSE weightedAvg(PM_POSE p1, double s1, PM_POSE p2, double s2)
          Adds two PM_POSES and returns a weightedAvg ie.
static void writeTimeAndPoseList(java.util.List<java.lang.Double> tlist, java.util.List<PM_POSE> posList, java.io.PrintStream psOut)
           
static void writeTimeAndPoseListS(java.util.List<java.lang.Double> tlist, java.util.List<PM_POSE> posList, java.lang.String fname)
           
 
Methods inherited from class java.lang.Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
 

Field Detail

pmErrno

public static int pmErrno

PM_PRINT_ERROR

public static boolean PM_PRINT_ERROR

debug_on

public static boolean debug_on

CART_FUZZ

public static final double CART_FUZZ
See Also:
Constant Field Values

DOUBLECP_FUZZ

public static final double DOUBLECP_FUZZ
See Also:
Constant Field Values

DOUBLE_FUZZ

public static final double DOUBLE_FUZZ
See Also:
Constant Field Values

E_EPSILON

public static final double E_EPSILON
See Also:
Constant Field Values

PM_2_PI

public static final double PM_2_PI
See Also:
Constant Field Values

PM_DIV_ERR

public static final int PM_DIV_ERR
See Also:
Constant Field Values

PM_ERR

public static final int PM_ERR
See Also:
Constant Field Values

PM_IMPL_ERR

public static final int PM_IMPL_ERR
See Also:
Constant Field Values

PM_NORM_ERR

public static final int PM_NORM_ERR
See Also:
Constant Field Values

PM_PI

public static final double PM_PI
See Also:
Constant Field Values

PM_PI_2

public static final double PM_PI_2
See Also:
Constant Field Values

PM_PI_4

public static final double PM_PI_4
See Also:
Constant Field Values

QSIN_FUZZ

public static final double QSIN_FUZZ
See Also:
Constant Field Values

QS_FUZZ

public static final double QS_FUZZ
See Also:
Constant Field Values

Q_FUZZ

public static final double Q_FUZZ
See Also:
Constant Field Values

RPY_P_FUZZ

public static final double RPY_P_FUZZ
See Also:
Constant Field Values

RS_FUZZ

public static final double RS_FUZZ
See Also:
Constant Field Values

SINGULAR_EPSILON

public static final double SINGULAR_EPSILON
See Also:
Constant Field Values

SQRT_FUZZ

public static final double SQRT_FUZZ
See Also:
Constant Field Values

UNIT_QUAT_FUZZ

public static final double UNIT_QUAT_FUZZ
See Also:
Constant Field Values

UNIT_SC_FUZZ

public static final double UNIT_SC_FUZZ
See Also:
Constant Field Values

UNIT_VEC_FUZZ

public static final double UNIT_VEC_FUZZ
See Also:
Constant Field Values

V_FUZZ

public static final double V_FUZZ
See Also:
Constant Field Values

ZYX_Y_FUZZ

public static final double ZYX_Y_FUZZ
See Also:
Constant Field Values

ZYZ_Y_FUZZ

public static final double ZYZ_Y_FUZZ
See Also:
Constant Field Values

DBL_MAX

public static final double DBL_MAX
See Also:
Constant Field Values

PI

public static final double PI
See Also:
Constant Field Values

PI_2

public static final double PI_2
See Also:
Constant Field Values

PI_4

public static final double PI_4
See Also:
Constant Field Values

PM_X

public static final int PM_X
See Also:
Constant Field Values

PM_Y

public static final int PM_Y
See Also:
Constant Field Values

PM_Z

public static final int PM_Z
See Also:
Constant Field Values

last_bad_rotmat

public static PmRotationMatrix last_bad_rotmat
Constructor Detail

Posemath

public Posemath()
Method Detail

writeTimeAndPoseListS

public static void writeTimeAndPoseListS(java.util.List<java.lang.Double> tlist,
                                         java.util.List<PM_POSE> posList,
                                         java.lang.String fname)

writeTimeAndPoseList

public static void writeTimeAndPoseList(java.util.List<java.lang.Double> tlist,
                                        java.util.List<PM_POSE> posList,
                                        java.io.PrintStream psOut)
                                 throws java.lang.Exception
Throws:
java.lang.Exception

csvSync

public static void csvSync(java.io.File f1,
                           java.io.File f2,
                           java.io.PrintStream psOut)
                    throws java.lang.Exception
Throws:
java.lang.Exception

shiftedSumOfProducts

public static double shiftedSumOfProducts(java.util.List<java.lang.Double> l1,
                                          java.util.List<java.lang.Double> l2,
                                          double dshift)

printCorrelationShifts

public static void printCorrelationShifts(java.lang.String fname1,
                                          int f1,
                                          java.lang.String fname2,
                                          int f2,
                                          double start_shift,
                                          double end_shift,
                                          double inc,
                                          java.io.PrintStream ps)
                                   throws java.lang.Exception
Throws:
java.lang.Exception

main

public static void main(java.lang.String[] args)
                 throws java.lang.Exception
Throws:
java.lang.Exception

findStillTimes

public static void findStillTimes(java.lang.String[] args)

fitLine

public static PM_LINE fitLine(java.util.List<? extends PmCartesian> l)
Ignores the z value in each element of the list and returns a line fit with least-squares error to x,y points

Parameters:
l - list of PmCartesian points to fit
Returns:
PM_LINE with start,end and unit vector of the line.

weightedAvg

public static PM_CARTESIAN weightedAvg(PM_CARTESIAN c1,
                                       double s1,
                                       PM_CARTESIAN c2,
                                       double s2)
Adds two PM_CARTESIANS and returns a weightedAvg ie. c1*s1+c2*s2;

Parameters:
c1 - first PM_CARTESIAN to add
s1 - weighting scale factor for c1
c2 - second PM_CARTESIAN to add
s2 - weighting scale factor for c2
Returns:
c1*s1+c2*s2

weightedAvg

public static PM_POSE weightedAvg(PM_POSE p1,
                                  double s1,
                                  PM_POSE p2,
                                  double s2)
                           throws PmException
Adds two PM_POSES and returns a weightedAvg ie. c1*s1+c2*s2;

Parameters:
c1 - first PM_POSE to add
s1 - weighting scale factor for c1
c2 - second PM_POSE to add
s2 - weighting scale factor for c2
Returns:
c1*s1+c2*s2
Throws:
PmException

computeLineFitErrors

public static double[] computeLineFitErrors(PM_LINE line,
                                            java.util.List<? extends PmCartesian> l)
Compute an array of errors or the distance between each point in list l to line line.

Parameters:
line - line points should be near
l - list of points near line
Returns:
array of errors

fitCircle2D_XY

public static PM_CIRCLE fitCircle2D_XY(java.util.List<? extends PmCartesian> l)
Ignores the z value in each element of the list and returns a circle fit with least-squares error to x,y points

Parameters:
l - list of PmCartesian points to fit
Returns:
PM_CIRCLE with center and radius of the fitted circle.

IS_FUZZ

public static boolean IS_FUZZ(double a,
                              double fuzz)
                       throws PmException
Throws:
PmException

pmSqrt

public static double pmSqrt(double x)
                     throws PmException
Square root function that adds some extra checks similiar to the C++ pmSqrt() primarily only exists for converting C++ apps. Otherwise use Math.sqrt().

Parameters:
x - number to take square root of
Returns:
square root of x
Throws:
PmException

pmSq

public static double pmSq(double x)
Square of x or x*x.

Parameters:
x - number to square
Returns:
square of x

pmClose

public static boolean pmClose(double a,
                              double b,
                              double eps)
Determine if a within eps of b.

Parameters:
a -
b -
eps -
Returns:
(Math.abs(a - b) < eps)

pmCartCylConvert

public static int pmCartCylConvert(PmCartesian v,
                                   PmCylindrical c)

toSph

public static PM_SPHERICAL toSph(PM_CARTESIAN v)
Convert a PM_CARTESIAN to PM_SPHERICAL

Parameters:
v - cartesian to convert
Returns:
v in spherical coords

pmCartSphConvert

public static int pmCartSphConvert(PmCartesian v,
                                   PmSpherical s)
Convert a PmCartesian to PmSpherical

Parameters:
v - cartesian to convert
s - spherical to store converted results in
Returns:
0

toCart

public static PM_CARTESIAN toCart(PM_SPHERICAL v)
Convert PM_SPHERICAL to PM_CARTESIAN

Parameters:
v - cartesian to convert
Returns:
v in spherical coords

pmSphCartConvert

public static int pmSphCartConvert(PmSpherical s,
                                   PmCartesian v)
Convert PmSherical to PmCartesian

Parameters:
s - spherical to convert
v - cartesian to store converted result
Returns:
0

toCyl

public static PM_CYLINDRICAL toCyl(PM_SPHERICAL s)
Convert a spherical to cylindrical

Parameters:
s - spherical to convert
Returns:
s in cylindrical coordinates.

pmSphCylConvert

public static int pmSphCylConvert(PmSpherical s,
                                  PmCylindrical c)
Convert a spherical to cylindrical

Parameters:
s - spherical to convert
c - cylindrical to store converted value
Returns:
0

toCart

public static PM_CARTESIAN toCart(PM_CYLINDRICAL v)
Convert a cylindrical to a cartesian

Parameters:
v - cylindrical to convert
Returns:
v in cartesian coords

pmCylCartConvert

public static int pmCylCartConvert(PmCylindrical c,
                                   PmCartesian v)
Convert cylindrical to cartesian.

Parameters:
c - cylindrical to convert
v - cartesian to store result
Returns:
0

toSph

public static PM_SPHERICAL toSph(PM_CYLINDRICAL c)
Convert a cylindrical to a spherical

Parameters:
c - cylindrical to convert
Returns:
c in spherical coordinates

pmCylSphConvert

public static int pmCylSphConvert(PmCylindrical c,
                                  PmSpherical s)
Convert a cylindrical to a spherical

Parameters:
c - cylindrical to convert
s - spherical to store result
Returns:
0

pmAxisAngleQuatConvert

public static int pmAxisAngleQuatConvert(int axis,
                                         double a,
                                         PmQuaternion q)
                                  throws PmException
Store a quaternion based on a single rotation about an axis

Parameters:
axis - either PM_X,PM_Y, or PM_Z
a - angle in radians for rotation
q - quaternion to store result
Returns:
0
Throws:
PmException

toQuat

public static PM_QUATERNION toQuat(PM_ROTATION_VECTOR v)
                            throws PmException
Convert a rotation vector to a quaternion

Parameters:
v - rotation vector to convert
Returns:
v as a quaternion
Throws:
PmException

pmRotQuatConvert

public static int pmRotQuatConvert(PmRotationVector r,
                                   PmQuaternion q)
                            throws PmException
Convert a rotation vector to a quaternion

Parameters:
r - rotation vector to convert
q - quaternion to store result
Returns:
0
Throws:
PmException

toMat

public static PM_ROTATION_MATRIX toMat(PM_ROTATION_VECTOR v)
                                throws PmException
Convert rotation vector to rotation matrix

Parameters:
v - rotation vector to convert (must be normalized)
Returns:
v as a rotation matrix
Throws:
PmException

pmRotMatConvert

public static int pmRotMatConvert(PmRotationVector r,
                                  PmRotationMatrix m)
                           throws PmException
Convert a rotation vector to a rotation matrix.

Parameters:
r - rotation vector to convert
m - rotation matrix to store result
Returns:
0
Throws:
PmException

pmRotRpyConvert

public static int pmRotRpyConvert(PmRotationVector r,
                                  PmRpy rpy)
                           throws PmException
Throws:
PmException

toRot

public static PM_ROTATION_VECTOR toRot(PM_QUATERNION v)
                                throws PmException
Convert quaternion to a rotation vector

Parameters:
v - quaternion to convert (must be normalized)
Returns:
v as a rotation vector
Throws:
PmException

pmQuatRotConvert

public static int pmQuatRotConvert(PmQuaternion q,
                                   PmRotationVector r)
                            throws PmException
Convert a quaternion to a rotation vector

Parameters:
q - quaternion to convert
r - rotation vector to store result
Returns:
0
Throws:
PmException

toMat

public static PM_ROTATION_MATRIX toMat(PM_QUATERNION v)
                                throws PmException
Convert a quaternion to a rotation matrix.

Parameters:
v - quaternion to convert (must be normalized)
Returns:
v as a rotation matrix
Throws:
PmException

distFromPath

public static double distFromPath(PM_CARTESIAN p,
                                  PM_CARTESIAN[] waypoints)
Return the minimum distance of the point p from the path specified by the array of waypoints. The distance is the distance to the closest line segment specified by each pair of consecutive points.

Parameters:
p - point we want to know how close it is to a path
waypoints - array of waypoints to specify a path.
Returns:
distance to nearest line segment in whatever units (p and waypoints are in)

maxA

public static double maxA(double[] da)

splitToLines

public static java.util.List<PM_LINE> splitToLines(java.util.List<? extends PM_CARTESIAN> l,
                                                   int num_lines,
                                                   double[] max_error,
                                                   int[] best_indexes)

fitLines

public static java.util.List<PM_LINE> fitLines(java.util.List<? extends PM_CARTESIAN> l,
                                               double tolerance,
                                               int max_lines)

multiply

public static java.util.ArrayList<PM_CARTESIAN> multiply(PM_XYA xya,
                                                         java.util.List<? extends PM_CARTESIAN> l)

multiply

public static PM_CARTESIAN[] multiply(PM_XYA xya,
                                      PM_CARTESIAN[] l)

multiply

public static PM_CARTESIAN[] multiply(PM_POSE pose,
                                      PM_CARTESIAN[] l)
                               throws PmException
Throws:
PmException

pmCartCentroid

public static PmCartesian pmCartCentroid(java.util.List<? extends PmCartesian> l)

pmCartCentroid

public static PmCartesian pmCartCentroid(PmCartesian[] l)

centroid

public static PM_CARTESIAN centroid(PM_CARTESIAN[] l)

centroid

public static PM_CARTESIAN centroid(java.util.List<? extends PM_CARTESIAN> l)

pmQuatMatConvert

public static int pmQuatMatConvert(PmQuaternion q,
                                   PmRotationMatrix m)
                            throws PmException
Throws:
PmException

pmQuatZyzConvert

public static int pmQuatZyzConvert(PmQuaternion q,
                                   PmEulerZyz zyz)
                            throws PmException
Throws:
PmException

pmQuatZyxConvert

public static int pmQuatZyxConvert(PmQuaternion q,
                                   PmEulerZyx zyx)
                            throws PmException
Throws:
PmException

toRpy

public static PM_RPY toRpy(PM_QUATERNION v)
                    throws PmException
Throws:
PmException

pmQuatRpyConvert

public static int pmQuatRpyConvert(PmQuaternion q,
                                   PmRpy rpy)
                            throws PmException
Throws:
PmException

pmMatRotConvert

public static int pmMatRotConvert(PmRotationMatrix m,
                                  PmRotationVector r)
                           throws PmException
Throws:
PmException

toQuat

public static PM_QUATERNION toQuat(PM_ROTATION_MATRIX m)
                            throws PmException
Throws:
PmException

pmMatQuatConvert

public static int pmMatQuatConvert(PmRotationMatrix m,
                                   PmQuaternion q)
                            throws PmException
Throws:
PmException

pmMatZyzConvert

public static int pmMatZyzConvert(PmRotationMatrix m,
                                  PmEulerZyz zyz)
                           throws PmException
Throws:
PmException

pmMatZyxConvert

public static int pmMatZyxConvert(PmRotationMatrix m,
                                  PmEulerZyx zyx)
                           throws PmException
Throws:
PmException

pmMatRpyConvert

public static int pmMatRpyConvert(PmRotationMatrix m,
                                  PmRpy rpy)
                           throws PmException
Throws:
PmException

toRpy

public static PM_RPY toRpy(PmRotationMatrix m)
                    throws PmException
Throws:
PmException

pmZyzRotConvert

public static int pmZyzRotConvert(PmEulerZyz zyz,
                                  PmRotationVector r)
                           throws PmException
Throws:
PmException

toQuat

public static PM_QUATERNION toQuat(PM_EULER_ZYZ v)
                            throws PmException
Throws:
PmException

pmZyzQuatConvert

public static int pmZyzQuatConvert(PmEulerZyz zyz,
                                   PmQuaternion q)
                            throws PmException
Throws:
PmException

pmZyzMatConvert

public static int pmZyzMatConvert(PmEulerZyz zyz,
                                  PmRotationMatrix m)
                           throws PmException
Throws:
PmException

pmZyzRpyConvert

public static int pmZyzRpyConvert(PmEulerZyz zyz,
                                  PmRpy rpy)
                           throws PmException
Throws:
PmException

pmZyxRotConvert

public static int pmZyxRotConvert(PmEulerZyx zyx,
                                  PmRotationVector r)
                           throws PmException
Throws:
PmException

toQuat

public static PM_QUATERNION toQuat(PM_EULER_ZYX v)
                            throws PmException
Throws:
PmException

pmZyxQuatConvert

public static int pmZyxQuatConvert(PmEulerZyx zyx,
                                   PmQuaternion q)
                            throws PmException
Throws:
PmException

pmZyxMatConvert

public static int pmZyxMatConvert(PmEulerZyx zyx,
                                  PmRotationMatrix m)
                           throws PmException
Throws:
PmException

pmZyxZyzConvert

public static int pmZyxZyzConvert(PmEulerZyx zyx,
                                  PmEulerZyz zyz)
                           throws PmException
Throws:
PmException

pmZyxRpyConvert

public static int pmZyxRpyConvert(PmEulerZyx zyx,
                                  PmRpy rpy)
                           throws PmException
Throws:
PmException

pmRpyRotConvert

public static int pmRpyRotConvert(PmRpy rpy,
                                  PmRotationVector r)
                           throws PmException
Throws:
PmException

toRot

public static PM_ROTATION_VECTOR toRot(PM_RPY rpy)
                                throws PmException
Throws:
PmException

toQuat

public static PM_QUATERNION toQuat(PM_RPY v)
                            throws PmException
Throws:
PmException

pmRpyQuatConvert

public static int pmRpyQuatConvert(PmRpy rpy,
                                   PmQuaternion q)
                            throws PmException
Throws:
PmException

pmRpyMatConvert

public static int pmRpyMatConvert(PmRpy rpy,
                                  PmRotationMatrix m)
                           throws PmException
Throws:
PmException

pmRpyZyzConvert

public static int pmRpyZyzConvert(PmRpy rpy,
                                  PmEulerZyz zyz)
                           throws PmException
Throws:
PmException

pmRpyZyxConvert

public static int pmRpyZyxConvert(PmRpy rpy,
                                  PmEulerZyx zyx)
                           throws PmException
Throws:
PmException

toPose

public static PM_POSE toPose(PM_HOMOGENEOUS h)
                      throws PmException
Throws:
PmException

toHom

public static PM_HOMOGENEOUS toHom(PM_POSE p)
                            throws PmException
Throws:
PmException

pmPoseHomConvert

public static int pmPoseHomConvert(PmPose p,
                                   PmHomogeneous h)
                            throws PmException
Throws:
PmException

pmHomPoseConvert

public static int pmHomPoseConvert(PmHomogeneous h,
                                   PmPose p)
                            throws PmException
Throws:
PmException

pmCartCartCompare

public static boolean pmCartCartCompare(PmCartesian v1,
                                        PmCartesian v2)
                                 throws PmException
Throws:
PmException

dot

public static double dot(PmCartesian v1,
                         PmCartesian v2)

dot

public static double dot(PM_CARTESIAN v1,
                         PM_CARTESIAN v2)

pmCartCartDot

public static double pmCartCartDot(PmCartesian v1,
                                   PmCartesian v2)

cross

public static PM_CARTESIAN cross(PM_CARTESIAN v1,
                                 PM_CARTESIAN v2)
                          throws PmException
Throws:
PmException

pmCartCartCross

public static int pmCartCartCross(PmCartesian v1,
                                  PmCartesian v2,
                                  PmCartesian vout)
                           throws PmException
Throws:
PmException

mag

public static double mag(PM_CARTESIAN v)

mag

public static double mag(PmCartesian v)

pmCartMag

public static double pmCartMag(PmCartesian v)

disp

public static double disp(PM_CARTESIAN v1,
                          PM_CARTESIAN v2)
                   throws PmException
Throws:
PmException

pmCartCartDisp

public static double pmCartCartDisp(PmCartesian v1,
                                    PmCartesian v2)
                             throws PmException
Throws:
PmException

add

public static PM_CARTESIAN add(PM_CARTESIAN v1,
                               PM_CARTESIAN v2)

add

public static PmCartesian add(PmCartesian v1,
                              PmCartesian v2)

pmCartCartAdd

public static int pmCartCartAdd(PmCartesian v1,
                                PmCartesian v2,
                                PmCartesian vout)

subtract

public static PM_CARTESIAN subtract(PM_CARTESIAN v1,
                                    PM_CARTESIAN v2)

subtract

public static PmCartesian subtract(PmCartesian v1,
                                   PmCartesian v2)

pmCartCartSub

public static int pmCartCartSub(PmCartesian v1,
                                PmCartesian v2,
                                PmCartesian vout)

multiply

public static PM_CARTESIAN multiply(double d,
                                    PM_CARTESIAN v)

multiply

public static PM_CARTESIAN multiply(PM_CARTESIAN v,
                                    double d)

multiply

public static PmCartesian multiply(double d,
                                   PmCartesian v)

multiply

public static PmCartesian multiply(PmCartesian v,
                                   double d)

pmCartScalMult

public static int pmCartScalMult(PmCartesian v1,
                                 double d,
                                 PmCartesian vout)

divide

public static PM_CARTESIAN divide(PM_CARTESIAN v,
                                  double d)
                           throws PmException
Throws:
PmException

pmCartScalDiv

public static int pmCartScalDiv(PmCartesian v1,
                                double d,
                                PmCartesian vout)
                         throws PmException
Throws:
PmException

neg

public static PM_CARTESIAN neg(PM_CARTESIAN v)
                        throws PmException
Throws:
PmException

pmCartNeg

public static int pmCartNeg(PmCartesian v1,
                            PmCartesian vout)
                     throws PmException
Throws:
PmException

inv

public static PM_CARTESIAN inv(PM_CARTESIAN v)
                        throws PmException
Throws:
PmException

pmCartInv

public static int pmCartInv(PmCartesian v1,
                            PmCartesian vout)
                     throws PmException
Throws:
PmException

norm

public static PM_CARTESIAN norm(PM_CARTESIAN v)
                         throws PmException
Throws:
PmException

pmCartNorm

public static int pmCartNorm(PmCartesian v,
                             PmCartesian vout)
                      throws PmException
Throws:
PmException

isNorm

public static boolean isNorm(PM_CARTESIAN v)
                      throws PmException
Throws:
PmException

pmCartIsNorm

public static boolean pmCartIsNorm(PmCartesian v)
                            throws PmException
Throws:
PmException

proj

public static PM_CARTESIAN proj(PM_CARTESIAN v1,
                                PM_CARTESIAN v2)
                         throws PmException
Throws:
PmException

pmCartCartProj

public static int pmCartCartProj(PmCartesian v1,
                                 PmCartesian v2,
                                 PmCartesian vout)
                          throws PmException
Throws:
PmException

pmCartPlaneProj

public static int pmCartPlaneProj(PmCartesian v,
                                  PmCartesian normal,
                                  PmCartesian vout)
                           throws PmException
Throws:
PmException

pmCylCylAdd

public static int pmCylCylAdd(PmCylindrical c1,
                              PmCylindrical c2,
                              PmCylindrical cout)
                       throws PmException
Throws:
PmException

pmCylCylSub

public static int pmCylCylSub(PmCylindrical c1,
                              PmCylindrical c2,
                              PmCylindrical cout)
                       throws PmException
Throws:
PmException

multiply

public static PM_CYLINDRICAL multiply(double d,
                                      PM_CYLINDRICAL c)
                               throws PmException
Throws:
PmException

multiply

public static PM_CYLINDRICAL multiply(PM_CYLINDRICAL c,
                                      double d)
                               throws PmException
Throws:
PmException

pmCylScalMult

public static int pmCylScalMult(PmCylindrical v1,
                                double d,
                                PmCylindrical vout)
                         throws PmException
Throws:
PmException

divide

public static PM_CYLINDRICAL divide(PM_CYLINDRICAL c,
                                    double d)
                             throws PmException
Throws:
PmException

pmCylScalDiv

public static int pmCylScalDiv(PmCylindrical v1,
                               double d,
                               PmCylindrical vout)
                        throws PmException
Throws:
PmException

pmCylCylCompare

public static boolean pmCylCylCompare(PmCylindrical v1,
                                      PmCylindrical v2)
                               throws PmException
Throws:
PmException

dot

public static double dot(PM_CYLINDRICAL v1,
                         PM_CYLINDRICAL v2)
                  throws PmException
Throws:
PmException

pmCylCylDot

public static double pmCylCylDot(PmCylindrical c1,
                                 PmCylindrical c2)
                          throws PmException
Throws:
PmException

mag

public static double mag(PM_CYLINDRICAL v1)
                  throws PmException
Throws:
PmException

pmCylMag

public static double pmCylMag(PmCylindrical c)
                       throws PmException
Throws:
PmException

cross

public static PM_CYLINDRICAL cross(PM_CYLINDRICAL v1,
                                   PM_CYLINDRICAL v2)
                            throws PmException
Throws:
PmException

pmCylCylCross

public static int pmCylCylCross(PmCylindrical c1,
                                PmCylindrical c2,
                                PmCylindrical cout)
                         throws PmException
Throws:
PmException

neg

public static PM_CYLINDRICAL neg(PM_CYLINDRICAL v)
                          throws PmException
Throws:
PmException

pmCylNeg

public static int pmCylNeg(PmCylindrical v1,
                           PmCylindrical vout)
                    throws PmException
Throws:
PmException

norm

public static PM_CYLINDRICAL norm(PM_CYLINDRICAL v)
                           throws PmException
Throws:
PmException

pmCylNorm

public static int pmCylNorm(PmCylindrical v,
                            PmCylindrical vout)
                     throws PmException
Throws:
PmException

inv

public static PM_CYLINDRICAL inv(PM_CYLINDRICAL v)
                          throws PmException
Throws:
PmException

pmCylInv

public static int pmCylInv(PmCylindrical c,
                           PmCylindrical cout)
                    throws PmException
Throws:
PmException

isNorm

public static boolean isNorm(PM_CYLINDRICAL v)
                      throws PmException
Throws:
PmException

pmCylIsNorm

public static boolean pmCylIsNorm(PmCylindrical v)
                           throws PmException
Throws:
PmException

pmQuatAxisAngleMult

public static PmQuaternion pmQuatAxisAngleMult(PmQuaternion q,
                                               int axis,
                                               double angle)
                                        throws PmException
Throws:
PmException

pmQuatAxisAngleMult

public static int pmQuatAxisAngleMult(PmQuaternion q,
                                      int axis,
                                      double angle,
                                      PmQuaternion pq)
                               throws PmException
Throws:
PmException

multiply

public static PM_ROTATION_VECTOR multiply(PM_ROTATION_VECTOR r,
                                          double s)
                                   throws PmException
Throws:
PmException

multiply

public static PmRotationVector multiply(double s,
                                        PM_ROTATION_VECTOR r)
                                 throws PmException
Throws:
PmException

pmRotScalMult

public static int pmRotScalMult(PmRotationVector r,
                                double s,
                                PmRotationVector rout)
                         throws PmException
Throws:
PmException

divide

public static PM_ROTATION_VECTOR divide(PM_ROTATION_VECTOR r,
                                        double s)
                                 throws PmException
Throws:
PmException

pmRotScalDiv

public static int pmRotScalDiv(PmRotationVector r,
                               double s,
                               PmRotationVector rout)
                        throws PmException
Throws:
PmException

isNorm

public static boolean isNorm(PM_ROTATION_VECTOR r)
                      throws PmException
Throws:
PmException

pmRotIsNorm

public static boolean pmRotIsNorm(PmRotationVector r)
                           throws PmException
Throws:
PmException

norm

public static PM_ROTATION_VECTOR norm(PM_ROTATION_VECTOR r)
                               throws PmException
Throws:
PmException

pmRotNorm

public static int pmRotNorm(PmRotationVector r,
                            PmRotationVector rout)
                     throws PmException
Throws:
PmException

pmMatNorm

public static int pmMatNorm(PmRotationMatrix m,
                            PmRotationMatrix mout)
                     throws PmException
Throws:
PmException

isNorm

public static boolean isNorm(PM_ROTATION_MATRIX m)
                      throws PmException
Throws:
PmException

pmMatIsNorm

public static boolean pmMatIsNorm(PmRotationMatrix m)
                           throws PmException
Throws:
PmException

pmMatInv

public static int pmMatInv(PmRotationMatrix m,
                           PmRotationMatrix mout)
                    throws PmException
Throws:
PmException

multiply

public static PM_CARTESIAN multiply(PM_ROTATION_MATRIX m,
                                    PM_CARTESIAN v)
                             throws PmException
Throws:
PmException

pmMatCartMult

public static int pmMatCartMult(PmRotationMatrix m,
                                PmCartesian v,
                                PmCartesian vout)
                         throws PmException
Throws:
PmException

multiply

public static PM_ROTATION_MATRIX multiply(PM_ROTATION_MATRIX m1,
                                          PM_ROTATION_MATRIX m2)
                                   throws PmException
Throws:
PmException

pmMatMatMult

public static int pmMatMatMult(PmRotationMatrix m1,
                               PmRotationMatrix m2,
                               PmRotationMatrix mout)
                        throws PmException
Throws:
PmException

pmQuatQuatCompare

public static boolean pmQuatQuatCompare(PmQuaternion q1,
                                        PmQuaternion q2)
                                 throws PmException
Throws:
PmException

mag

public static double mag(PM_QUATERNION q)
                  throws PmException
Throws:
PmException

pmQuatMag

public static double pmQuatMag(PmQuaternion q)
                        throws PmException
Throws:
PmException

norm

public static PM_QUATERNION norm(PM_QUATERNION q1)
                          throws PmException
Throws:
PmException

pmQuatNorm

public static int pmQuatNorm(PmQuaternion q1,
                             PmQuaternion qout)
                      throws PmException
Throws:
PmException

inv

public static PM_QUATERNION inv(PM_QUATERNION q1)
                         throws PmException
Throws:
PmException

pmQuatInv

public static int pmQuatInv(PmQuaternion q1,
                            PmQuaternion qout)
                     throws PmException
Throws:
PmException

isNorm

public static boolean isNorm(PM_QUATERNION q1)
                      throws PmException
Throws:
PmException

pmQuatIsNorm

public static boolean pmQuatIsNorm(PmQuaternion q1)
                            throws PmException
Throws:
PmException

multiply

public static PM_QUATERNION multiply(PM_QUATERNION q,
                                     double s)
                              throws PmException
Throws:
PmException

multiply

public static PM_QUATERNION multiply(double s,
                                     PM_QUATERNION q)
                              throws PmException
Throws:
PmException

pmQuatScalMult

public static int pmQuatScalMult(PmQuaternion q,
                                 double s,
                                 PmQuaternion qout)
                          throws PmException
Throws:
PmException

divide

public static PM_QUATERNION divide(PM_QUATERNION q,
                                   double s)
                            throws PmException
Throws:
PmException

pmQuatScalDiv

public static int pmQuatScalDiv(PmQuaternion q,
                                double s,
                                PmQuaternion qout)
                         throws PmException
Throws:
PmException

multiply

public static PM_QUATERNION multiply(PM_QUATERNION q1,
                                     PM_QUATERNION q2)
                              throws PmException
Throws:
PmException

pmQuatQuatMult

public static int pmQuatQuatMult(PmQuaternion q1,
                                 PmQuaternion q2,
                                 PmQuaternion qout)
                          throws PmException
Throws:
PmException

multiply

public static PM_CARTESIAN multiply(PmQuaternion q1,
                                    PM_CARTESIAN v2)
                             throws PmException
Throws:
PmException

pmQuatCartMult

public static int pmQuatCartMult(PmQuaternion q1,
                                 PmCartesian v2,
                                 PmCartesian vout)
                          throws PmException
Throws:
PmException

pmPosePoseCompare

public static boolean pmPosePoseCompare(PmPose p1,
                                        PmPose p2)
                                 throws PmException
Throws:
PmException

inv

public static PM_POSE inv(PM_POSE p)
                   throws PmException
Throws:
PmException

inv

public static PM_XYA inv(PM_XYA p)
                  throws PmException
Throws:
PmException

multiply

public static PM_XYA multiply(PM_XYA p1,
                              PM_XYA p2)

multiply

public static PM_CARTESIAN multiply(PM_XYA p1,
                                    PM_CARTESIAN p2)

pmPoseInv

public static int pmPoseInv(PmPose p1,
                            PmPose p2)
                     throws PmException
Throws:
PmException

multiply

public static PM_CARTESIAN multiply(PM_POSE p1,
                                    PM_CARTESIAN v2)
                             throws PmException
Throws:
PmException

pmPoseCartMult

public static int pmPoseCartMult(PmPose p1,
                                 PmCartesian v2,
                                 PmCartesian vout)
                          throws PmException
Throws:
PmException

multiply

public static PM_POSE multiply(PM_POSE p1,
                               PM_POSE p2)
                        throws PmException
Throws:
PmException

pmPosePoseMult

public static int pmPosePoseMult(PmPose p1,
                                 PmPose p2,
                                 PmPose pout)
                          throws PmException
Throws:
PmException

inv

public static PM_HOMOGENEOUS inv(PM_HOMOGENEOUS h)
                          throws PmException
Throws:
PmException

pmHomInv

public static int pmHomInv(PmHomogeneous h1,
                           PmHomogeneous h2)
                    throws PmException
Throws:
PmException

pmLineInit

public static int pmLineInit(PmLine line,
                             PmPose start,
                             PmPose end)
                      throws PmException
Throws:
PmException

pmLinePoint

public static int pmLinePoint(PmLine line,
                              double len,
                              PmPose point)
                       throws PmException
Throws:
PmException

pmCircleInit

public static int pmCircleInit(PmCircle circle,
                               PmPose start,
                               PmPose end,
                               PmCartesian center,
                               PmCartesian normal,
                               int turn)
                        throws PmException
Throws:
PmException

findStillPoints

public static java.util.List<PM_POSE> findStillPoints(java.util.List<java.lang.Double> tlist,
                                                      java.util.List<PM_POSE> posList,
                                                      double velThresh,
                                                      int avg_thresh,
                                                      java.util.List<java.lang.Double> stillTimes)
                                               throws PmException
Throws:
PmException

prePostMultiplyList

public static java.util.List<PM_POSE> prePostMultiplyList(java.util.List<? extends PM_POSE> l,
                                                          PM_POSE pre,
                                                          PM_POSE post)

poseListToHomMats

public static double[][] poseListToHomMats(java.util.List<? extends PM_POSE> l)

poseListChk

public static java.util.List<PM_POSE> poseListChk(PM_POSE X,
                                                  java.util.List<? extends PM_POSE> A,
                                                  PM_POSE Y,
                                                  java.util.List<? extends PM_POSE> B)
                                           throws PmException
Throws:
PmException

poseListRandom

public static java.util.List<PM_POSE> poseListRandom(int size,
                                                     double angle_noise,
                                                     double cart_noise)
                                              throws PmException
Throws:
PmException

toPose

public static PM_POSE toPose(double[][] dd)
                      throws PmException
Throws:
PmException

homMatsPrePostMult

public static java.util.List<PM_POSE> homMatsPrePostMult(double[][] X,
                                                         double[][] A,
                                                         double[][] Y)
                                                  throws PmException
Throws:
PmException

homMatsChk

public static java.util.List<PM_POSE> homMatsChk(double[][] X,
                                                 double[][] A,
                                                 double[][] Y,
                                                 double[][] B)
                                          throws PmException
Throws:
PmException

poseListToCartList

public static java.util.List<PM_CARTESIAN> poseListToCartList(java.util.List<? extends PM_POSE> plin)

poseListToRpyList

public static java.util.List<PM_RPY> poseListToRpyList(java.util.List<? extends PM_POSE> plin)

cartListToMagArray

public static double[] cartListToMagArray(java.util.List<? extends PM_CARTESIAN> clin)

rpyListToMagArray

public static double[] rpyListToMagArray(java.util.List<? extends PM_RPY> rlin)

mean

public static double mean(java.util.List<java.lang.Double> l)

mean

public static double mean(java.lang.Double[] da)

mean

public static double mean(double[] da)

stddev

public static double stddev(double[] da)

stddev

public static double stddev(java.lang.Double[] da)

stddev

public static double stddev(java.util.List<java.lang.Double> da)

poseListPoseListMultiply

public static java.util.List<PM_POSE> poseListPoseListMultiply(java.util.List<? extends PM_POSE> pl1,
                                                               java.util.List<? extends PM_POSE> pl2)

poseListInv

public static java.util.List<PM_POSE> poseListInv(java.util.List<? extends PM_POSE> plin)

homMatsToPoseList

public static java.util.List<PM_POSE> homMatsToPoseList(double[][] dd)

csvToPoseListF

public static java.util.List<PM_POSE> csvToPoseListF(java.io.File f,
                                                     int x_pos,
                                                     int y_pos,
                                                     int z_pos,
                                                     int roll_pos,
                                                     int pitch_pos,
                                                     int yaw_pos)
                                              throws java.lang.Exception
Throws:
java.lang.Exception

csvToPoseList

public static java.util.List<PM_POSE> csvToPoseList(java.lang.String filename,
                                                    int x_pos,
                                                    int y_pos,
                                                    int z_pos,
                                                    int roll_pos,
                                                    int pitch_pos,
                                                    int yaw_pos)
                                             throws java.lang.Exception
Throws:
java.lang.Exception

csvWithTimeToPoseListF

public static java.util.List<PM_POSE> csvWithTimeToPoseListF(java.io.File f,
                                                             java.util.List<java.lang.Double> timeList,
                                                             int time_pos,
                                                             int x_pos,
                                                             int y_pos,
                                                             int z_pos,
                                                             int roll_pos,
                                                             int pitch_pos,
                                                             int yaw_pos)
                                                      throws java.lang.Exception
Throws:
java.lang.Exception

csvWithTimeToPoseList

public static java.util.List<PM_POSE> csvWithTimeToPoseList(java.lang.String filename,
                                                            java.util.List<java.lang.Double> timeList,
                                                            int time_pos,
                                                            int x_pos,
                                                            int y_pos,
                                                            int z_pos,
                                                            int roll_pos,
                                                            int pitch_pos,
                                                            int yaw_pos)
                                                     throws java.lang.Exception
Throws:
java.lang.Exception

csvToListF

public static java.util.List<java.lang.Double> csvToListF(java.io.File f,
                                                          int pos)
                                                   throws java.lang.Exception
Throws:
java.lang.Exception

csvToList

public static java.util.List<java.lang.Double> csvToList(java.lang.String filename,
                                                         int pos)
                                                  throws java.lang.Exception
Throws:
java.lang.Exception

getCk1Pose

public static PM_POSE getCk1Pose()

getCk2Pose

public static PM_POSE getCk2Pose()

csvToHomMatsChk

public static double[][] csvToHomMatsChk(java.lang.String filename,
                                         int x_pos,
                                         int y_pos,
                                         int z_pos,
                                         int roll_pos,
                                         int pitch_pos,
                                         int yaw_pos)

csvToHomMats

public static double[][] csvToHomMats(java.lang.String filename,
                                      int x_pos,
                                      int y_pos,
                                      int z_pos,
                                      int roll_pos,
                                      int pitch_pos,
                                      int yaw_pos)

pmCirclePoint

public static int pmCirclePoint(PmCircle circle,
                                double angle,
                                PmPose point)
                         throws PmException
Throws:
PmException

point_on_line

public static PmCartesian point_on_line(PM_LINE line,
                                        PmCartesian p)

point_on_line

public static PM_CARTESIAN point_on_line(PM_LINE line,
                                         PM_CARTESIAN p)

point_on_line

public static PM_CARTESIAN point_on_line(PM_CARTESIAN s,
                                         PM_CARTESIAN u,
                                         PM_CARTESIAN p)

dist_from_line

public static double dist_from_line(PM_LINE line,
                                    PM_CARTESIAN c)

point_on_line_segment

public static PM_CARTESIAN point_on_line_segment(PM_CARTESIAN s,
                                                 PM_CARTESIAN e,
                                                 PM_CARTESIAN p)

tstArray

public static double[][] tstArray()

intersection

public static PM_CARTESIAN intersection(PM_LINE l1,
                                        PM_LINE l2)
                                 throws PmException
Throws:
PmException

in_line_segment

public static boolean in_line_segment(PM_LINE l,
                                      PM_CARTESIAN c1,
                                      double tol)
                               throws java.lang.Exception
Throws:
java.lang.Exception