Пример #1
0
 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;
 }
Пример #2
0
 public static double[][] toHomMat(PoseType poseIn) {
   double mat[][] =
       new double[][] {
         {1.0, 0.0, 0.0, 0.0},
         {0.0, 1.0, 0.0, 0.0},
         {0.0, 0.0, 1.0, 0.0},
         {0.0, 0.0, 0.0, 1.0}
       };
   PointType pt = poseIn.getPoint();
   mat[0][3] = pt.getX().doubleValue();
   mat[1][3] = pt.getY().doubleValue();
   mat[2][3] = pt.getZ().doubleValue();
   VectorType xAxis = poseIn.getXAxis();
   mat[0][0] = xAxis.getI().doubleValue();
   mat[0][1] = xAxis.getJ().doubleValue();
   mat[0][2] = xAxis.getK().doubleValue();
   VectorType yAxis = cross(poseIn.getZAxis(), poseIn.getXAxis());
   mat[1][0] = yAxis.getI().doubleValue();
   mat[1][1] = yAxis.getJ().doubleValue();
   mat[1][2] = yAxis.getK().doubleValue();
   VectorType zAxis = poseIn.getZAxis();
   mat[2][0] = zAxis.getI().doubleValue();
   mat[2][1] = zAxis.getJ().doubleValue();
   mat[2][2] = zAxis.getK().doubleValue();
   return mat;
 }
Пример #3
0
 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;
 }
Пример #4
0
 public static String toString(PointType pt) {
   if (null == pt) {
     return "null";
   }
   return dataTypeThingToStartString(pt)
       + "x="
       + toString(pt.getX())
       + ","
       + "y="
       + toString(pt.getY())
       + ","
       + "z="
       + toString(pt.getZ())
       + "}";
 }
Пример #5
0
  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;
  }
Пример #6
0
 public static BigDecimal dot(VectorType v1, PointType p2) {
   return v1.getI()
       .multiply(p2.getX())
       .add(v1.getJ().multiply(p2.getY()))
       .add(v1.getK().multiply(p2.getZ()));
 }
Пример #7
0
 /**
  * Convert crcl.PointType to rcs.posemath.PmCartesian
  *
  * @param pt Point to be converted
  * @return PmCartesian equivalent
  */
 public static PmCartesian pointToPmCartesian(final PointType pt) {
   return new PmCartesian(
       pt.getX().doubleValue(), pt.getY().doubleValue(), pt.getZ().doubleValue());
 }