Posemath { static public PM_ROTATION_VECTOR toRot(PM_RPY rpy) throws PmException { PM_ROTATION_VECTOR r = new PM_ROTATION_VECTOR(); pmRpyRotConvert(rpy, r); return r; }  static void writeTimeAndPoseListS(List<Double> tlist,
            List<PM_POSE> posList,
            String fname); static void writeTimeAndPoseList(List<Double> tlist,
            List<PM_POSE> posList,
            PrintStream psOut); static void csvSync(File f1, File f2, PrintStream psOut); static double shiftedSumOfProducts(List<Double> l1, List<Double> l2, double dshift); static void printCorrelationShifts(String fname1, int f1,
            String fname2, int f2,
            double start_shift, double end_shift, double inc,
            PrintStream ps); static void main(String args[]); static void findStillTimes(String args[]); static PM_LINE fitLine(List<? extends PmCartesian> l); static PM_CARTESIAN weightedAvg(PM_CARTESIAN c1, double s1,
            PM_CARTESIAN c2, double s2); static PM_POSE weightedAvg(PM_POSE p1, double s1,
            PM_POSE p2, double s2); static double[] computeLineFitErrors(PM_LINE line,
            List<? extends PmCartesian> l); static PM_CIRCLE fitCircle2D_XY(List<? extends PmCartesian> l); static boolean IS_FUZZ(double a, double fuzz); static double pmSqrt(double x); static double pmSq(double x); static boolean pmClose(double a, double b, double eps); static int pmCartCylConvert(PmCartesian v, PmCylindrical c); static PM_SPHERICAL toSph(PM_CARTESIAN v); static int pmCartSphConvert(PmCartesian v, PmSpherical s); static PM_CARTESIAN toCart(PM_SPHERICAL v); static int pmSphCartConvert(PmSpherical s, PmCartesian v); static PM_CYLINDRICAL toCyl(PM_SPHERICAL s); static int pmSphCylConvert(PmSpherical s, PmCylindrical c); static PM_CARTESIAN toCart(PM_CYLINDRICAL v); static int pmCylCartConvert(PmCylindrical c, PmCartesian v); static PM_SPHERICAL toSph(PM_CYLINDRICAL c); static int pmCylSphConvert(PmCylindrical c, PmSpherical s); static int pmAxisAngleQuatConvert(int axis, double a, PmQuaternion q); static PM_QUATERNION toQuat(PM_ROTATION_VECTOR v); static PmQuaternion toQuat(PmRotationVector v); static int pmRotQuatConvert(PmRotationVector r, PmQuaternion q); static PM_ROTATION_MATRIX toMat(PmRotationVector v); static PM_ROTATION_MATRIX toMat(PmEulerZyx zyx); static PM_ROTATION_MATRIX toMat(PmEulerZyz zyz); static PM_ROTATION_MATRIX toMat(PmRpy rpy); static int pmRotMatConvert(PmRotationVector r, PmRotationMatrix m); static int pmRotRpyConvert(PmRotationVector r, PmRpy rpy); static PM_ROTATION_VECTOR toRot(PM_RPY rpy); static PmRotationVector toRot(PmRpy rpy); static PM_ROTATION_VECTOR toRot(PM_QUATERNION v); static PmRotationVector toRot(PmQuaternion v); static PmRotationVector toRot(PmRotationMatrix m); static int pmQuatRotConvert(PmQuaternion q, PmRotationVector r); static PM_ROTATION_MATRIX toMat(PmQuaternion v); static double distFromPath(PM_CARTESIAN p, PM_CARTESIAN[] waypoints); static double maxA(double[] da); static List<PM_LINE> splitToLines(List<? extends PM_CARTESIAN> l,
            int num_lines, double max_error[], int best_indexes[]); static List<PM_LINE> fitLines(List<? extends PM_CARTESIAN> l,
            double tolerance,
            int max_lines); static ArrayList<PM_CARTESIAN> multiply(PM_XYA xya,
            List<? extends PM_CARTESIAN> l); static PM_CARTESIAN[] multiply(PM_XYA xya, PM_CARTESIAN l[]); static PM_CARTESIAN[] multiply(PM_POSE pose, PM_CARTESIAN l[]); static PmCartesian pmCartCentroid(List<? extends PmCartesian> l); static PmCartesian pmCartCentroid(PmCartesian l[]); static PM_CARTESIAN centroid(PM_CARTESIAN l[]); static PM_CARTESIAN centroid(List<? extends PM_CARTESIAN> l); static int pmQuatMatConvert(PmQuaternion q, PmRotationMatrix m); static int pmQuatZyzConvert(PmQuaternion q, PmEulerZyz zyz); static int pmQuatZyxConvert(PmQuaternion q, PmEulerZyx zyx); static PM_RPY toRpy(PM_QUATERNION v); static PmRpy toRpy(PmQuaternion v); static int pmQuatRpyConvert(PmQuaternion q, PmRpy rpy); static int pmMatRotConvert(PmRotationMatrix m, PmRotationVector r); static PM_QUATERNION toQuat(PM_ROTATION_MATRIX m); static PmQuaternion toQuat(PmRotationMatrix m); static int pmMatQuatConvert(PmRotationMatrix m, PmQuaternion q); static int pmMatZyzConvert(PmRotationMatrix m, PmEulerZyz zyz); static int pmMatZyxConvert(PmRotationMatrix m, PmEulerZyx zyx); static int pmMatRpyConvert(PmRotationMatrix m, PmRpy rpy); static PM_RPY toRpy(PmRotationMatrix m); static PM_RPY toRpy(PmRotationVector rv); static int pmZyzRotConvert(PmEulerZyz zyz, PmRotationVector r); static PM_QUATERNION toQuat(PM_EULER_ZYZ v); static int pmZyzQuatConvert(PmEulerZyz zyz, PmQuaternion q); static int pmZyzMatConvert(PmEulerZyz zyz, PmRotationMatrix m); static int pmZyzRpyConvert(PmEulerZyz zyz, PmRpy rpy); static int pmZyxRotConvert(PmEulerZyx zyx, PmRotationVector r); static PM_QUATERNION toQuat(PM_EULER_ZYX v); static int pmZyxQuatConvert(PmEulerZyx zyx, PmQuaternion q); static int pmZyxMatConvert(PmEulerZyx zyx, PmRotationMatrix m); static int pmZyxZyzConvert(PmEulerZyx zyx, PmEulerZyz zyz); static int pmZyxRpyConvert(PmEulerZyx zyx, PmRpy rpy); static int pmRpyRotConvert(PmRpy rpy, PmRotationVector r); static PM_QUATERNION toQuat(PM_RPY v); static PmQuaternion toQuat(PmRpy v); static int pmRpyQuatConvert(PmRpy rpy, PmQuaternion q); static int pmRpyMatConvert(PmRpy rpy, PmRotationMatrix m); static int pmRpyZyzConvert(PmRpy rpy, PmEulerZyz zyz); static int pmRpyZyxConvert(PmRpy rpy, PmEulerZyx zyx); static PM_POSE toPose(PM_HOMOGENEOUS h); static PM_HOMOGENEOUS toHom(PM_POSE p); static int pmPoseHomConvert(PmPose p, PmHomogeneous h); static int pmHomPoseConvert(PmHomogeneous h, PmPose p); static boolean pmCartCartCompare(PmCartesian v1, PmCartesian v2); static double dot(PmCartesian v1, PmCartesian v2); static double dot(PM_CARTESIAN v1, PM_CARTESIAN v2); static double pmCartCartDot(PmCartesian v1, PmCartesian v2); static PM_CARTESIAN cross(PM_CARTESIAN v1, PM_CARTESIAN v2); static PmCartesian cross(PmCartesian v1, PmCartesian v2); static int pmCartCartCross(PmCartesian v1, PmCartesian v2, PmCartesian vout); static double mag(PM_CARTESIAN v); static double mag(PmCartesian v); static double pmCartMag(PmCartesian v); static double disp(PM_CARTESIAN v1, PM_CARTESIAN v2); static double pmCartCartDisp(PmCartesian v1, PmCartesian v2); static PM_CARTESIAN add(PM_CARTESIAN v1, PM_CARTESIAN v2); static PmCartesian add(PmCartesian v1, PmCartesian v2); static int pmCartCartAdd(PmCartesian v1, PmCartesian v2, PmCartesian vout); static PM_CARTESIAN subtract(PM_CARTESIAN v1, PM_CARTESIAN v2); static PmCartesian subtract(PmCartesian v1, PmCartesian v2); static int pmCartCartSub(PmCartesian v1, PmCartesian v2, PmCartesian vout); static PM_CARTESIAN multiply(double d, PM_CARTESIAN v); static PM_CARTESIAN multiply(PM_CARTESIAN v, double d); static PmCartesian multiply(double d, PmCartesian v); static PmCartesian multiply(PmCartesian v, double d); static int pmCartScalMult(PmCartesian v1, double d, PmCartesian vout); static PM_CARTESIAN divide(PM_CARTESIAN v, double d); static int pmCartScalDiv(PmCartesian v1, double d, PmCartesian vout); static PM_CARTESIAN neg(PM_CARTESIAN v); static int pmCartNeg(PmCartesian v1, PmCartesian vout); static PM_CARTESIAN inv(PM_CARTESIAN v); static int pmCartInv(PmCartesian v1, PmCartesian vout); static PM_CARTESIAN norm(PM_CARTESIAN v); static int pmCartNorm(PmCartesian v, PmCartesian vout); static boolean isNorm(PM_CARTESIAN v); static boolean pmCartIsNorm(PmCartesian v); static PM_CARTESIAN proj(PM_CARTESIAN v1, PM_CARTESIAN v2); static int pmCartCartProj(PmCartesian v1, PmCartesian v2, PmCartesian vout); static int pmCartPlaneProj(PmCartesian v, PmCartesian normal, PmCartesian vout); static int pmCylCylAdd(PmCylindrical c1, PmCylindrical c2, PmCylindrical cout); static int pmCylCylSub(PmCylindrical c1, PmCylindrical c2, PmCylindrical cout); static PM_CYLINDRICAL multiply(double d, PM_CYLINDRICAL c); static PM_CYLINDRICAL multiply(PM_CYLINDRICAL c, double d); static int pmCylScalMult(PmCylindrical v1, double d, PmCylindrical vout); static PM_CYLINDRICAL divide(PM_CYLINDRICAL c, double d); static int pmCylScalDiv(PmCylindrical v1, double d, PmCylindrical vout); static boolean pmCylCylCompare(PmCylindrical v1, PmCylindrical v2); static double dot(PM_CYLINDRICAL v1, PM_CYLINDRICAL v2); static double pmCylCylDot(PmCylindrical c1, PmCylindrical c2); static double mag(PM_CYLINDRICAL v1); static double pmCylMag(PmCylindrical c); static PM_CYLINDRICAL cross(PM_CYLINDRICAL v1, PM_CYLINDRICAL v2); static int pmCylCylCross(PmCylindrical c1, PmCylindrical c2, PmCylindrical cout); static PM_CYLINDRICAL neg(PM_CYLINDRICAL v); static int pmCylNeg(PmCylindrical v1, PmCylindrical vout); static PM_CYLINDRICAL norm(PM_CYLINDRICAL v); static int pmCylNorm(PmCylindrical v, PmCylindrical vout); static PM_CYLINDRICAL inv(PM_CYLINDRICAL v); static int pmCylInv(PmCylindrical c, PmCylindrical cout); static boolean isNorm(PM_CYLINDRICAL v); static boolean pmCylIsNorm(PmCylindrical v); static PmQuaternion pmQuatAxisAngleMult(PmQuaternion q, int axis, double angle); static int pmQuatAxisAngleMult(PmQuaternion q, int axis, double angle, PmQuaternion pq); static PM_ROTATION_VECTOR multiply(PM_ROTATION_VECTOR r, double s); static PmRotationVector multiply(double s, PM_ROTATION_VECTOR r); static int pmRotScalMult(PmRotationVector r, double s, PmRotationVector rout); static PM_ROTATION_VECTOR divide(PM_ROTATION_VECTOR r, double s); static int pmRotScalDiv(PmRotationVector r, double s, PmRotationVector rout); static boolean isNorm(PM_ROTATION_VECTOR r); static boolean pmRotIsNorm(PmRotationVector r); static PM_ROTATION_VECTOR norm(PM_ROTATION_VECTOR r); static int pmRotNorm(PmRotationVector r, PmRotationVector rout); static boolean isNorm(PM_ROTATION_MATRIX m); static boolean pmMatIsNorm(PmRotationMatrix m); static int pmMatInv(PmRotationMatrix m, PmRotationMatrix mout); static PM_CARTESIAN multiply(PM_ROTATION_MATRIX m, PM_CARTESIAN v); static int pmMatCartMult(PmRotationMatrix m, PmCartesian v, PmCartesian vout); static PM_ROTATION_MATRIX multiply(PM_ROTATION_MATRIX m1, PM_ROTATION_MATRIX m2); static int pmMatMatMult(PmRotationMatrix m1, PmRotationMatrix m2,
            PmRotationMatrix mout); static boolean pmQuatQuatCompare(PmQuaternion q1, PmQuaternion q2); static double mag(PM_QUATERNION q); static double pmQuatMag(PmQuaternion q); static PM_QUATERNION norm(PM_QUATERNION q1); static int pmQuatNorm(PmQuaternion q1, PmQuaternion qout); static PM_QUATERNION inv(PM_QUATERNION q1); static int pmQuatInv(PmQuaternion q1, PmQuaternion qout); static boolean isNorm(PM_QUATERNION q1); static boolean pmQuatIsNorm(PmQuaternion q1); static PM_QUATERNION multiply(PM_QUATERNION q, double s); static PM_QUATERNION multiply(double s, PM_QUATERNION q); static int pmQuatScalMult(PmQuaternion q, double s, PmQuaternion qout); static PM_QUATERNION divide(PM_QUATERNION q, double s); static int pmQuatScalDiv(PmQuaternion q, double s, PmQuaternion qout); static PM_QUATERNION multiply(PM_QUATERNION q1, PM_QUATERNION q2); static int pmQuatQuatMult(PmQuaternion q1, PmQuaternion q2, PmQuaternion qout); static PM_CARTESIAN multiply(PmQuaternion q1, PM_CARTESIAN v2); static int pmQuatCartMult(PmQuaternion q1, PmCartesian v2, PmCartesian vout); static boolean pmPosePoseCompare(PmPose p1, PmPose p2); static PM_POSE inv(PM_POSE p); static PM_XYA inv(PM_XYA p); static PM_XYA multiply(PM_XYA p1, PM_XYA p2); static PM_CARTESIAN multiply(PM_XYA p1, PM_CARTESIAN p2); static int pmPoseInv(PmPose p1, PmPose p2); static PM_CARTESIAN multiply(PM_POSE p1, PM_CARTESIAN v2); static int pmPoseCartMult(PmPose p1, PmCartesian v2, PmCartesian vout); static PM_POSE multiply(PM_POSE p1, PM_POSE p2); static int pmPosePoseMult(PmPose p1, PmPose p2, PmPose pout); static PM_HOMOGENEOUS inv(PM_HOMOGENEOUS h); static int pmHomInv(PmHomogeneous h1, PmHomogeneous h2); static int pmLineInit(PmLine line, PmPose start, PmPose end); static int pmLinePoint(PmLine line, double len, PmPose point); static int pmCircleInit(PmCircle circle,
            PmPose start, PmPose end,
            PmCartesian center, PmCartesian normal,
            int turn); static List<PM_POSE> findStillPoints(List<Double> tlist,
            List<PM_POSE> posList, double velThresh, int avg_thresh,
            List<Double> stillTimes); static List<PM_POSE> prePostMultiplyList(List<? extends PM_POSE> l,
            PM_POSE pre, PM_POSE post); static double[][] poseListToHomMats(List<? extends PM_POSE> l); static List<PM_POSE> poseListChk(PM_POSE X,
            List<? extends PM_POSE> A,
            PM_POSE Y,
            List<? extends PM_POSE> B); static List<PM_POSE> poseListRandom(int size,
            double angle_noise,
            double cart_noise); static PM_POSE toPose(double dd[][]); static List<PM_POSE> homMatsPrePostMult(double X[][],
            double A[][],
            double Y[][]); static List<PM_POSE> homMatsChk(double X[][],
            double A[][],
            double Y[][],
            double B[][]); static List<PM_CARTESIAN> poseListToCartList(List<? extends PM_POSE> plin); static List<PM_RPY> poseListToRpyList(List<? extends PM_POSE> plin); static double[] cartListToMagArray(List<? extends PM_CARTESIAN> clin); static double[] rpyListToMagArray(List<? extends PM_RPY> rlin); static double mean(List<Double> l); static double mean(Double da[]); static double mean(double da[]); static double stddev(double da[]); static double stddev(Double da[]); static double stddev(List<Double> da); static List<PM_POSE> poseListPoseListMultiply(List<? extends PM_POSE> pl1,
            List<? extends PM_POSE> pl2); static List<PM_POSE> poseListInv(List<? extends PM_POSE> plin); static List<PM_POSE> homMatsToPoseList(double dd[][]); static List<PM_POSE> csvToPoseListF(File f,
            int x_pos,
            int y_pos,
            int z_pos,
            int roll_pos,
            int pitch_pos,
            int yaw_pos); static List<PM_POSE> csvToPoseList(String filename,
            int x_pos,
            int y_pos,
            int z_pos,
            int roll_pos,
            int pitch_pos,
            int yaw_pos); static List<PM_POSE> csvWithTimeToPoseListF(File f,
            List<Double> timeList,
            int time_pos,
            int x_pos,
            int y_pos,
            int z_pos,
            int roll_pos,
            int pitch_pos,
            int yaw_pos); static List<PM_POSE> csvWithTimeToPoseList(String filename,
            List<Double> timeList,
            int time_pos,
            int x_pos,
            int y_pos,
            int z_pos,
            int roll_pos,
            int pitch_pos,
            int yaw_pos); static List<Double> csvToListF(File f,
            int pos); static List<Double> csvToList(String filename,
            int pos); static PM_POSE getCk1Pose(); static PM_POSE getCk2Pose(); static double[][] csvToHomMatsChk(String filename,
            int x_pos,
            int y_pos,
            int z_pos,
            int roll_pos,
            int pitch_pos,
            int yaw_pos); static double[][] csvToHomMats(String filename,
            int x_pos,
            int y_pos,
            int z_pos,
            int roll_pos,
            int pitch_pos,
            int yaw_pos); static int pmCirclePoint(PmCircle circle, double angle, PmPose point); static PmCartesian point_on_line(PM_LINE line, PmCartesian p); static PM_CARTESIAN point_on_line(PM_LINE line, PM_CARTESIAN p); static PM_CARTESIAN point_on_line(PM_CARTESIAN s, PM_CARTESIAN u, PM_CARTESIAN p); static double dist_from_line(PM_LINE line, PM_CARTESIAN c); static PM_CARTESIAN point_on_line_segment(PM_CARTESIAN s, PM_CARTESIAN e, PM_CARTESIAN p); static double[][] tstArray(); static PM_CARTESIAN intersection(PM_LINE l1, PM_LINE l2); static boolean in_line_segment(PM_LINE l,
            PM_CARTESIAN c1, double tol); static public int pmErrno; static public boolean PM_PRINT_ERROR; static public boolean debug_on; static final double CART_FUZZ; static final double DOUBLECP_FUZZ; static final double DOUBLE_FUZZ; static final double E_EPSILON; static final double PM_2_PI; static final int PM_DIV_ERR; static final int PM_ERR; static final int PM_IMPL_ERR; static final int PM_NORM_ERR; static final double PM_PI; static final double PM_PI_2; static final double PM_PI_4; static final double QSIN_FUZZ; static final double QS_FUZZ; static final double Q_FUZZ; static final double RPY_P_FUZZ; static final double RS_FUZZ; static final double SINGULAR_EPSILON; static final double SQRT_FUZZ; static final double UNIT_QUAT_FUZZ; static final double UNIT_SC_FUZZ; static final double UNIT_VEC_FUZZ; static final double V_FUZZ; static final double ZYX_Y_FUZZ; static final double ZYZ_Y_FUZZ; static final double DBL_MAX; static final double PI; static final double PI_2; static final double PI_4; static final int PM_X; static final int PM_Y; static final int PM_Z; static public PmRotationMatrix last_bad_rotmat; }
@Test public void testToRot_PmRpy() throws Exception { System.out.println("toRot"); PmRpy rpy = new PmRpy(0.0,0.0,Math.PI/2.0); PmRotationVector expResult = new PmRotationVector(Math.PI/2.0,0.0,0.0,1.0); PmRotationVector result = Posemath.toRot(rpy); checkEquals("yaw",expResult,result); }