public static String tuple4dToString(Tuple4d tuple, NumberFormat format) { String xToString = format.format(tuple.getX()); String yToString = format.format(tuple.getY()); String zToString = format.format(tuple.getZ()); String sToString = format.format(tuple.getW()); return "(" + xToString + ", " + yToString + ", " + zToString + ", " + sToString + ")"; } }
/** * Checks the validity of a {@link Tuple4d} * @param tuple4dToCheck * @return null if the tuple4d is valid, or the error message. */ public static ObjectErrorType validateTuple4d(Tuple4d tuple4dToCheck) { if (tuple4dToCheck == null) return ObjectErrorType.NULL; if (isNaN(tuple4dToCheck.getX()) || isNaN(tuple4dToCheck.getY()) || isNaN(tuple4dToCheck.getZ()) || isNaN(tuple4dToCheck.getW())) return ObjectErrorType.CONTAINS_NaN; if (isInfinite(tuple4dToCheck.getX()) || isInfinite(tuple4dToCheck.getY()) || isInfinite(tuple4dToCheck.getZ()) || isInfinite(tuple4dToCheck.getW())) return ObjectErrorType.CONTAINS_INFINITY; return null; }
public void update(Tuple4d value) { update(value.getW()); update(value.getX()); update(value.getY()); update(value.getZ()); }
public static Vector4d generateRandomVector4d(Random random, Tuple4d lowerBound, Tuple4d upperBound) { Vector4d ret = new Vector4d(); ret.setX(generateRandomDouble(random, lowerBound.getX(), upperBound.getX())); ret.setY(generateRandomDouble(random, lowerBound.getY(), upperBound.getY())); ret.setZ(generateRandomDouble(random, lowerBound.getZ(), upperBound.getZ())); ret.setW(generateRandomDouble(random, lowerBound.getW(), upperBound.getW())); return ret; }
public static Quaternion convertTuple4d(Tuple4d tuple) { Quaternion quaternion = messageFactory.newFromType("geometry_msgs/Quaternion"); if(tuple == null) { quaternion.setX(Double.NaN); quaternion.setY(Double.NaN); quaternion.setZ(Double.NaN); quaternion.setW(Double.NaN); } else { quaternion.setX(tuple.getX()); quaternion.setY(tuple.getY()); quaternion.setZ(tuple.getZ()); quaternion.setW(tuple.getW()); } return quaternion; }