Posemath

java.lang.Object
- 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

Fields
Modifier and Type	Field and Description
`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

Constructors
Constructor and Description

Posemath()

Constructors
Constructor and Description
`Posemath()`

Method Summary

All Methods Static Methods Concrete Methods
Modifier and Type	Method and Description
`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 PmCartesian`	`cross(PmCartesian v1, PmCartesian 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`	`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_ROTATION_MATRIX`	`toMat(PM_RPY rpy)` Convert roll-pitch-yaw to rotation matrix
`static PmRotationMatrix`	`toMat(PmRotationVector v)` Convert rotation vector to rotation matrix
`static PmRotationMatrix`	`toMat(PmRpy rpy)` Convert roll-pitch-yaw 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 PmQuaternion`	`toQuat(PmRotationMatrix m)`
`static PmQuaternion`	`toQuat(PmRotationVector v)` Convert a rotation vector to a quaternion
`static PmQuaternion`	`toQuat(PmRpy v)`
`static PM_ROTATION_VECTOR`	`toRot(PM_QUATERNION v)` Convert quaternion to a rotation vector
`static PM_ROTATION_VECTOR`	`toRot(PM_RPY rpy)` Convert quaternion to a rotation vector
`static PmRotationVector`	`toRot(PmQuaternion v)` Convert quaternion to a rotation vector
`static PmRotationVector`	`toRot(PmRotationMatrix m)` Convert quaternion to a rotation vector
`static PmRotationVector`	`toRot(PmRpy rpy)` Convert roll-pitch-yaw to a rotation vector
`static PM_RPY`	`toRpy(PM_QUATERNION v)`
`static PmRpy`	`toRpy(PmQuaternion 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:: p1 - first PM_POSE to add; s1 - weighting scale factor for c1; p2 - second PM_POSE to add; s2 - weighting scale factor for c2
Returns:: c1*s1+c2*s2
Throws:: PmException - when multiplication fails

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 - when x less than zero

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 - value to check if close to b

b - value to check if close to b

eps - allowed difference between two values

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 - when conversion is not possible

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 - when conversion is not possible

toQuat
```
public static PmQuaternion toQuat(PmRotationVector v)
                           throws PmException
```
Convert a rotation vector to a quaternion

Parameters:

v - rotation vector to convert

Returns:

v as a quaternion

Throws:

PmException - when conversion is not possible

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 - when conversion is not possible

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 - when conversion is not possible

toMat
```
public static PmRotationMatrix toMat(PmRotationVector 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 - when conversion is not possible

toMat
```
public static PM_ROTATION_MATRIX toMat(PM_RPY rpy)
                                throws PmException
```
Convert roll-pitch-yaw to rotation matrix

Parameters:

rpy - roll-pitch-yaw to convert (must be normalized)

Returns:

v as a rotation matrix

Throws:

PmException - when conversion is not possible

toMat
```
public static PmRotationMatrix toMat(PmRpy rpy)
                              throws PmException
```
Convert roll-pitch-yaw to rotation matrix

Parameters:

rpy - roll-pitch-yaw to convert (must be normalized)

Returns:

v as a rotation matrix

Throws:

PmException - when conversion is not possible

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 - when conversion is not possible

pmRotRpyConvert

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

Throws:: PmException

toRot
```
public static PM_ROTATION_VECTOR toRot(PM_RPY rpy)
                                throws PmException
```
Convert quaternion to a rotation vector

Parameters:

rpy - roll-pitch-yaw to convert (must be normalized)

Returns:

v as a rotation vector

Throws:

PmException - when conversion is not possible

toRot
```
public static PmRotationVector toRot(PmRpy rpy)
                              throws PmException
```
Convert roll-pitch-yaw to a rotation vector

Parameters:

rpy - roll-pitch-yaw to convert (must be normalized)

Returns:

v as a rotation vector

Throws:

PmException - when conversion is not possible

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 - when conversion is not possible

toRot
```
public static PmRotationVector toRot(PmQuaternion 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 - when conversion is not possible

toRot
```
public static PmRotationVector toRot(PmRotationMatrix m)
                              throws PmException
```
Convert quaternion to a rotation vector

Parameters:

m - matrix to convert (must be normalized)

Returns:

v as a rotation vector

Throws:

PmException - when conversion is not possible

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 - when conversion is not possible

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 - when conversion is not possible

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

toRpy

public static PmRpy toRpy(PmQuaternion 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

toQuat

public static PmQuaternion toQuat(PmRotationMatrix 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

toQuat

public static PM_QUATERNION toQuat(PM_RPY v)
                            throws PmException

Throws:: PmException

toQuat

public static PmQuaternion toQuat(PmRpy 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

cross

public static PmCartesian cross(PmCartesian v1,
                                PmCartesian 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

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

Class Posemath

Field Summary

Constructor Summary

Method Summary

Methods inherited from class java.lang.Object

Field Detail

pmErrno

PM_PRINT_ERROR

debug_on

CART_FUZZ

DOUBLECP_FUZZ

DOUBLE_FUZZ

E_EPSILON

PM_2_PI

PM_DIV_ERR

PM_ERR

PM_IMPL_ERR

PM_NORM_ERR

PM_PI

PM_PI_2

PM_PI_4

QSIN_FUZZ

QS_FUZZ

Q_FUZZ

RPY_P_FUZZ

RS_FUZZ

SINGULAR_EPSILON

SQRT_FUZZ

UNIT_QUAT_FUZZ

UNIT_SC_FUZZ

UNIT_VEC_FUZZ

V_FUZZ

ZYX_Y_FUZZ

ZYZ_Y_FUZZ

DBL_MAX

PI

PI_2

PI_4

PM_X

PM_Y

PM_Z

last_bad_rotmat

Constructor Detail

Posemath

Method Detail

writeTimeAndPoseListS

writeTimeAndPoseList

csvSync

shiftedSumOfProducts

printCorrelationShifts

main

findStillTimes

fitLine

weightedAvg

weightedAvg

computeLineFitErrors

fitCircle2D_XY

IS_FUZZ

pmSqrt

pmSq

pmClose

pmCartCylConvert

toSph

pmCartSphConvert

toCart

pmSphCartConvert

toCyl

pmSphCylConvert

toCart

pmCylCartConvert

toSph

pmCylSphConvert

pmAxisAngleQuatConvert

toQuat

toQuat

pmRotQuatConvert

toMat

toMat

toMat

toMat