public void surfaceNormalAt(double x, double y, double z, Vector3d normal) {
double threshhold = 0.015;
normal.setX(0.0);
normal.setY(0.0);
normal.setZ(1.0);
if (!boundingBox.isXYInside(x, y) || (z > boundingBox.getZMax() - threshhold)) return;
if (Math.abs(x - boundingBox.getXMin()) < threshhold) {
normal.setX(-1.0);
normal.setY(0.0);
normal.setZ(0.0);
} else if (Math.abs(x - boundingBox.getXMax()) < threshhold) {
normal.setX(1.0);
normal.setY(0.0);
normal.setZ(0.0);
} else if (Math.abs(y - boundingBox.getYMin()) < threshhold) {
normal.setX(0.0);
normal.setY(-1.0);
normal.setZ(0.0);
} else if (Math.abs(y - boundingBox.getYMax()) < threshhold) {
normal.setX(0.0);
normal.setY(1.0);
normal.setZ(0.0);
}
}
public static void packRosVector3ToVector3d(Vector3 rosVector, Vector3d vectorToPack) {
vectorToPack.setX(rosVector.getX());
vectorToPack.setY(rosVector.getY());
vectorToPack.setZ(rosVector.getZ());
}