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;
}
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;
}
/**
* 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;
}
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;
}
/**
* 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;
}
/**
* 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;
}
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;
}