Esempio n. 1
0
 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;
 }
Esempio n. 2
0
 /**
  * Generate L2 norm of Vector WARNING: This function loses the BigDecimal precision and range in
  * VectorType
  *
  * @param v1 vector to compute norm of
  * @return norm of input vector
  */
 public static double norm(VectorType v1) {
   // FIXME(maybe?) It is difficult to take sqrt(BigDecimal) so
   // I punted and just hope double precision is good enough.
   double i = v1.getI().doubleValue();
   double j = v1.getJ().doubleValue();
   double k = v1.getK().doubleValue();
   return Math.sqrt(i * i + j * j + k * k);
 }
Esempio n. 3
0
 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;
 }
Esempio n. 4
0
 public static String toString(VectorType v) {
   if (null == v) {
     return "null";
   }
   return dataTypeThingToStartString(v)
       + "i="
       + toString(v.getI())
       + ","
       + "j="
       + toString(v.getJ())
       + ","
       + "k="
       + toString(v.getK())
       + "}";
 }
Esempio n. 5
0
 /**
  * Normalize the vector so its L2 Norm is 1 WARNING: This function uses norm() which loses the
  * BigDecimal precision and range in VectorType
  *
  * @param v vector to compute norm of
  * @return normalized input vector
  * @throws ZeroNormException if norm(v) less than Double.MIN_VALUE
  */
 public static VectorType normalize(VectorType v) {
   VectorType vout = new VectorType();
   double normv = norm(v);
   if (normv < Double.MIN_VALUE) {
     throw new ZeroNormException("Can't normalize vector with zero magnitude.");
   }
   BigDecimal normInv = BigDecimal.ONE.divide(BigDecimal.valueOf(norm(v)));
   vout.setI(v.getI().multiply(normInv));
   vout.setJ(v.getJ().multiply(normInv));
   vout.setK(v.getK().multiply(normInv));
   return vout;
 }
Esempio n. 6
0
 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;
 }
Esempio n. 7
0
 /**
  * Combine an rcslib Posemath translation and rotation(roll-pitch-yaw) in a PoseType
  *
  * @param tran translational component of pose
  * @param v rotational component of pose in roll-pith-yaw format.
  * @param pose_in optional pose to be set instead of creating new Pose
  * @return new Pose creating from combining inputs or pose_in if not null
  * @throws PmException if rotation vector can not be converted to matrix
  */
 public static PoseType toPoseType(PmCartesian tran, PmRpy v, /*@Nullable*/ PoseType pose_in)
     throws PmException {
   PoseType pose = pose_in;
   if (pose == null) {
     pose = new PoseType();
   }
   pose.setPoint(toPointType(tran));
   PmRotationMatrix mat = Posemath.toMat(v);
   VectorType xVec = new VectorType();
   xVec.setI(BigDecimal.valueOf(mat.x.x));
   xVec.setJ(BigDecimal.valueOf(mat.x.y));
   xVec.setK(BigDecimal.valueOf(mat.x.z));
   pose.setXAxis(xVec);
   VectorType zVec = new VectorType();
   zVec.setI(BigDecimal.valueOf(mat.z.x));
   zVec.setJ(BigDecimal.valueOf(mat.z.y));
   zVec.setK(BigDecimal.valueOf(mat.z.z));
   pose.setZAxis(zVec);
   return pose;
 }
Esempio n. 8
0
 /**
  * Convert the crcl.VectorType to a PmCartesian. crcl.VectorType is generally used as a unit
  * vector for rotation.
  *
  * @param v Vector to convert
  * @return PmCartesian equivalent of v
  */
 public static PmCartesian vectorToPmCartesian(VectorType v) {
   return new PmCartesian(v.getI().doubleValue(), v.getJ().doubleValue(), v.getK().doubleValue());
 }
Esempio n. 9
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;
  }
Esempio n. 10
0
  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;
  }
Esempio n. 11
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;
 }
Esempio n. 12
0
 /**
  * Compute cross product of two Vectors WARNING: The output may not be normalized even if the
  * input vectors are.
  *
  * @param v1 first vector
  * @param v2 second vector
  * @return cross product vector
  */
 public static VectorType cross(VectorType v1, VectorType v2) {
   VectorType vout = new VectorType();
   //        vout.x = v1.y * v2.z - v1.z * v2.y;
   //        vout.y = v1.z * v2.x - v1.x * v2.z;
   //        vout.z = v1.x * v2.y - v1.y * v2.x;
   vout.setI(v1.getJ().multiply(v2.getK()).subtract(v1.getK().multiply(v2.getJ())));
   vout.setJ(v1.getK().multiply(v2.getI()).subtract(v1.getI().multiply(v2.getK())));
   vout.setK(v1.getI().multiply(v2.getJ()).subtract(v1.getJ().multiply(v2.getI())));
   return vout;
 }
Esempio n. 13
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()));
 }
Esempio n. 14
0
 public static BigDecimal dot(VectorType v1, VectorType v2) {
   return v1.getI()
       .multiply(v2.getI())
       .add(v1.getJ().multiply(v2.getJ()))
       .add(v1.getK().multiply(v2.getK()));
 }