コード例 #1
0
ファイル: CRCLPosemath.java プロジェクト: wshackle/crcl4java
 /**
  * Convert a PmCartesian to a crcl.PointType
  *
  * @param c Cartesian point to convert
  * @return Point equivalent of input cartesian
  */
 public static PointType toPointType(PmCartesian c) {
   PointType pt = new PointType();
   pt.setX(BigDecimal.valueOf(c.x));
   pt.setY(BigDecimal.valueOf(c.y));
   pt.setZ(BigDecimal.valueOf(c.z));
   return pt;
 }
コード例 #2
0
ファイル: CRCLPosemath.java プロジェクト: wshackle/crcl4java
  public static PoseType invert(PoseType p) {
    PoseType pOut = new PoseType();
    VectorType xAxisIn = p.getXAxis();
    VectorType zAxisIn = p.getZAxis();
    VectorType yAxisIn = cross(p.getZAxis(), p.getXAxis());
    VectorType xAxisOut = new VectorType();
    xAxisOut.setI(p.getXAxis().getI());
    xAxisOut.setJ(yAxisIn.getI());
    xAxisOut.setK(p.getZAxis().getI());
    pOut.setXAxis(xAxisOut);
    VectorType zAxisOut = new VectorType();
    zAxisOut.setI(p.getXAxis().getK());
    zAxisOut.setJ(yAxisIn.getK());
    zAxisOut.setK(p.getZAxis().getK());
    pOut.setZAxis(zAxisOut);
    //        VectorType yAxisOut = cross(zAxisOut,xAxisOut);

    PointType pt = new PointType();
    pt.setX(dot(xAxisIn, p.getPoint()).negate());
    pt.setY(dot(yAxisIn, p.getPoint()).negate());
    pt.setZ(dot(zAxisIn, p.getPoint()).negate());
    pOut.setPoint(pt);

    return pOut;
  }
コード例 #3
0
ファイル: CRCLPosemath.java プロジェクト: wshackle/crcl4java
 public static PoseType toPose(double mat[][]) {
   if (null == mat
       || mat.length != 4
       || mat[0].length != 4
       || mat[1].length != 4
       || mat[2].length != 4
       || mat[3].length != 4) {
     throw new IllegalArgumentException("toPose() matrix should be 4x4");
   }
   PoseType newPose = new PoseType();
   PointType pt = new PointType();
   pt.setX(BigDecimal.valueOf(mat[0][3]));
   pt.setY(BigDecimal.valueOf(mat[1][3]));
   pt.setZ(BigDecimal.valueOf(mat[2][3]));
   newPose.setPoint(pt);
   VectorType xAxis = new VectorType();
   xAxis.setI(BigDecimal.valueOf(mat[0][0]));
   xAxis.setJ(BigDecimal.valueOf(mat[0][1]));
   xAxis.setK(BigDecimal.valueOf(mat[0][2]));
   newPose.setXAxis(xAxis);
   VectorType zAxis = new VectorType();
   zAxis.setI(BigDecimal.valueOf(mat[2][0]));
   zAxis.setJ(BigDecimal.valueOf(mat[2][1]));
   zAxis.setK(BigDecimal.valueOf(mat[2][2]));
   newPose.setZAxis(zAxis);
   return newPose;
 }
コード例 #4
0
ファイル: CRCLPosemath.java プロジェクト: wshackle/crcl4java
 public static PointType multiply(BigDecimal dist, PointType p) {
   PointType out = new PointType();
   out.setX(p.getX().multiply(dist));
   out.setY(p.getY().multiply(dist));
   out.setZ(p.getZ().multiply(dist));
   return out;
 }
コード例 #5
0
ファイル: CRCLPosemath.java プロジェクト: wshackle/crcl4java
 public static PointType multiply(final BigDecimal dist, final VectorType v) {
   PointType out = new PointType();
   out.setX(v.getI().multiply(dist));
   out.setY(v.getJ().multiply(dist));
   out.setZ(v.getK().multiply(dist));
   return out;
 }
コード例 #6
0
ファイル: CRCLPosemath.java プロジェクト: wshackle/crcl4java
 public static PointType subtract(PointType p1, PointType p2) {
   PointType sum = new PointType();
   sum.setX(p1.getX().subtract(p2.getX()));
   sum.setY(p1.getY().subtract(p2.getY()));
   sum.setZ(p1.getZ().subtract(p2.getZ()));
   return sum;
 }
コード例 #7
0
ファイル: CRCLPosemath.java プロジェクト: wshackle/crcl4java
 public static PoseType identityPose() {
   PoseType newPose = new PoseType();
   PointType pt = new PointType();
   pt.setX(BigDecimal.ZERO);
   pt.setY(BigDecimal.ZERO);
   pt.setZ(BigDecimal.ZERO);
   newPose.setPoint(pt);
   VectorType xAxis = new VectorType();
   xAxis.setI(BigDecimal.ONE);
   xAxis.setJ(BigDecimal.ZERO);
   xAxis.setK(BigDecimal.ZERO);
   newPose.setXAxis(xAxis);
   VectorType zAxis = new VectorType();
   zAxis.setI(BigDecimal.ZERO);
   zAxis.setJ(BigDecimal.ZERO);
   zAxis.setK(BigDecimal.ONE);
   newPose.setZAxis(zAxis);
   return newPose;
 }
コード例 #8
0
ファイル: CRCLPosemath.java プロジェクト: wshackle/crcl4java
  public static PoseType multiply(PoseType p1, PoseType p2) {
    PoseType poseOut = new PoseType();
    VectorType yAxis1 = cross(p1.getZAxis(), p1.getXAxis());
    VectorType yAxis2 = cross(p2.getZAxis(), p2.getXAxis());
    VectorType xAxisOut = new VectorType();
    VectorType zAxisOut = new VectorType();
    PointType pt2 = p2.getPoint();
    PointType pt2rot = new PointType();
    pt2rot.setX(
        p1.getXAxis()
            .getI()
            .multiply(pt2.getX())
            .add(yAxis1.getI().multiply(pt2.getY()))
            .add(p1.getZAxis().getI().multiply(pt2.getZ())));
    pt2rot.setY(
        p1.getXAxis()
            .getJ()
            .multiply(pt2.getX())
            .add(yAxis1.getJ().multiply(pt2.getY()))
            .add(p1.getZAxis().getJ().multiply(pt2.getZ())));
    pt2rot.setZ(
        p1.getXAxis()
            .getK()
            .multiply(pt2.getX())
            .add(yAxis1.getK().multiply(pt2.getY()))
            .add(p1.getZAxis().getK().multiply(pt2.getZ())));
    PointType pt = add(p1.getPoint(), pt2rot);
    poseOut.setPoint(pt);
    //        xAxisOut.setI(
    //                p1.getXAxis().getI().multiply(p2.getXAxis().getI())
    //                .add(p1.getXAxis().getJ().multiply(yAxis2.getI()))
    //                .add(p1.getXAxis().getK().multiply(p2.getZAxis().getI()))
    //                );
    //        xAxisOut.setJ(
    //                p1.getXAxis().getI().multiply(p2.getXAxis().getJ())
    //                .add(p1.getXAxis().getJ().multiply(yAxis2.getJ()))
    //                .add(p1.getXAxis().getK().multiply(p2.getZAxis().getJ()))
    //                );
    //        xAxisOut.setK(
    //                p1.getXAxis().getI().multiply(p2.getXAxis().getK())
    //                .add(p1.getXAxis().getJ().multiply(yAxis2.getK()))
    //                .add(p1.getXAxis().getK().multiply(p2.getZAxis().getK()))
    //                );
    xAxisOut.setI(
        p1.getXAxis()
            .getI()
            .multiply(p2.getXAxis().getI())
            .add(yAxis1.getI().multiply(p2.getXAxis().getJ()))
            .add(p1.getZAxis().getI().multiply(p2.getXAxis().getK())));
    xAxisOut.setJ(
        p1.getXAxis()
            .getJ()
            .multiply(p2.getXAxis().getI())
            .add(yAxis1.getJ().multiply(p2.getXAxis().getJ()))
            .add(p1.getZAxis().getJ().multiply(p2.getXAxis().getK())));
    xAxisOut.setK(
        p1.getXAxis()
            .getK()
            .multiply(p2.getXAxis().getI())
            .add(yAxis1.getK().multiply(p2.getXAxis().getJ()))
            .add(p1.getZAxis().getK().multiply(p2.getXAxis().getK())));

    poseOut.setXAxis(xAxisOut);
    zAxisOut.setI(
        p1.getXAxis()
            .getI()
            .multiply(p2.getZAxis().getI())
            .add(yAxis1.getI().multiply(p2.getZAxis().getJ()))
            .add(p1.getZAxis().getI().multiply(p2.getZAxis().getK())));
    zAxisOut.setJ(
        p1.getXAxis()
            .getJ()
            .multiply(p2.getZAxis().getI())
            .add(yAxis1.getJ().multiply(p2.getZAxis().getJ()))
            .add(p1.getZAxis().getJ().multiply(p2.getZAxis().getK())));
    zAxisOut.setK(
        p1.getXAxis()
            .getK()
            .multiply(p2.getZAxis().getI())
            .add(yAxis1.getK().multiply(p2.getZAxis().getJ()))
            .add(p1.getZAxis().getK().multiply(p2.getZAxis().getK())));
    poseOut.setZAxis(zAxisOut);
    return poseOut;
  }