All Downloads are FREE. Search and download functionalities are using the official Maven repository.

us.ihmc.simulationconstructionset.util.ground.BalanceBeamTerrainObject Maven / Gradle / Ivy

package us.ihmc.simulationconstructionset.util.ground;

import us.ihmc.euclid.Axis3D;
import us.ihmc.euclid.geometry.BoundingBox3D;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;
import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.graphicsDescription.appearance.AppearanceDefinition;
import us.ihmc.jMonkeyEngineToolkit.HeightMapWithNormals;

public class BalanceBeamTerrainObject implements TerrainObject3D, HeightMapWithNormals
{
   private final Point2D origin;
   private final BoundingBox3D boundingBox;
   private final Vector2D direction;
   private final double width;

   private final Vector2D tempVector = new Vector2D();
   private final Graphics3DObject linkGraphics;
   private final double heightAboveGround;

   public BalanceBeamTerrainObject(Point2DReadOnly origin, double back, double forward, Vector2D direction, double width, double heightAboveGround,
                                   AppearanceDefinition appearance)
   {
      Point2D pForward = new Point2D(direction);
      pForward.scale(forward);

      Point2D pBack = new Point2D(direction);
      pBack.scale(-back);

      this.origin = new Point2D(origin);
      double xMin = Math.min(pForward.getX(), pBack.getX());
      double xMax = Math.max(pForward.getX(), pBack.getX());
      double yMin = Math.min(pForward.getY(), pBack.getY());
      double yMax = Math.max(pForward.getY(), pBack.getY());
      double zMin = Double.NEGATIVE_INFINITY;
      double zMax = zMin + heightAboveGround;

      Point3D minPoint = new Point3D(xMin, yMin, zMin);
      Point3D maxPoint = new Point3D(xMax, yMax, zMax);

      boundingBox = new BoundingBox3D(minPoint, maxPoint);

      this.direction = direction;
      this.width = width;
      this.heightAboveGround = heightAboveGround;

      linkGraphics = new Graphics3DObject();
      double height = width;
      linkGraphics.translate(origin.getX() + (xMin + xMax) / 2.0, origin.getY() + (yMin + yMax) / 2.0, heightAboveGround - height);
      double angle = Math.atan2(direction.getY(), direction.getX());
      linkGraphics.rotate(angle, Axis3D.Z);

      linkGraphics.addCube(forward + back, width, height, appearance);
   }

   @Override
   public double heightAndNormalAt(double x, double y, double z, Vector3DBasics normalToPack)
   {
      double heightAt = heightAt(x, y, z);
      surfaceNormalAt(x, y, z, normalToPack);

      return heightAt;
   }

   @Override
   public double heightAt(double x, double y, double z)
   {
      double xFromOrigin = x - origin.getX();
      double yFromOrigin = y - origin.getY();

      tempVector.set(xFromOrigin, yFromOrigin);
      double componentAlongBalanceBeam = tempVector.dot(direction);
      tempVector.set(direction);
      tempVector.scale(componentAlongBalanceBeam);
      tempVector.setX(xFromOrigin - tempVector.getX());
      tempVector.setY(xFromOrigin - tempVector.getY());
      double distanceFromCenterOfBalanceBeamSquared = tempVector.lengthSquared();
      if (distanceFromCenterOfBalanceBeamSquared < width * width / 4.0)
      {
         return heightAboveGround;
      }
      else
      {
         return -1000.0;
      }
   }

   public void surfaceNormalAt(double x, double y, double z, Vector3DBasics normal)
   {
      normal.set(0.0, 0.0, 1.0);
   }

   public void closestIntersectionTo(double x, double y, double z, Point3DBasics intersection)
   {
      intersection.setX(x); // Go Straight Up for now...
      intersection.setY(y);
      intersection.setZ(heightAt(x, y, z));
   }

   public void closestIntersectionAndNormalAt(double x, double y, double z, Point3DBasics intersection, Vector3DBasics normal)
   {
      intersection.setX(x); // Go Straight Up for now...
      intersection.setY(y);
      intersection.setZ(heightAt(x, y, z));

      surfaceNormalAt(x, y, z, normal);
   }

   @Override
   public boolean checkIfInside(double x, double y, double z, Point3DBasics intersectionToPack, Vector3DBasics normalToPack)
   {
      intersectionToPack.set(x, y, heightAt(x, y, z));
      surfaceNormalAt(x, y, z, normalToPack);

      return (z < intersectionToPack.getZ());
   }

   @Override
   public boolean isClose(double x, double y, double z)
   {
      return boundingBox.isInsideInclusive(x, y, z);
   }

   public double getXMin()
   {
      return boundingBox.getMinX();
   }

   public double getYMin()
   {
      return boundingBox.getMinY();
   }

   public double getXMax()
   {
      return boundingBox.getMaxX();
   }

   public double getYMax()
   {
      return boundingBox.getMaxY();
   }

   @Override
   public Graphics3DObject getLinkGraphics()
   {
      return linkGraphics;
   }

   @Override
   public BoundingBox3D getBoundingBox()
   {
      return boundingBox;
   }

   @Override
   public HeightMapWithNormals getHeightMapIfAvailable()
   {
      return this;
   }
}




© 2015 - 2025 Weber Informatics LLC | Privacy Policy