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);
    }
  }
コード例 #2
0
 public static void packRosVector3ToVector3d(Vector3 rosVector, Vector3d vectorToPack) {
   vectorToPack.setX(rosVector.getX());
   vectorToPack.setY(rosVector.getY());
   vectorToPack.setZ(rosVector.getZ());
 }