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;
}
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;
}
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;
}
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())
+ "}";
}
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;
}
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()));
}
/**
* 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());
}
