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

Go to download
Show more of this group Show more artifacts with this name
Show all versions of simulation-construction-set
Simulation Construction Set
There is a newer version: 0.25.2
package us.ihmc.simulationconstructionset.util.ground;

import us.ihmc.euclid.geometry.BoundingBox3D;
import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics;
import us.ihmc.graphicsDescription.HeightMap;
import us.ihmc.jMonkeyEngineToolkit.GroundProfile3D;
import us.ihmc.jMonkeyEngineToolkit.HeightMapWithNormals;

public abstract class GroundProfileFromHeightMap implements HeightMapWithNormals, GroundProfile3D
{
   public GroundProfileFromHeightMap()
   {
   }

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

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

      return (z < heightAt);
   }

   public void closestIntersectionAndNormalAt(double x, double y, double z, Point3DBasics intersectionToPack, Vector3DBasics normalToPack)
   {
      double heightAt = heightAndNormalAt(x, y, z, normalToPack);
      intersectionToPack.set(x, y, heightAt);
   }

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

   public static GroundProfileFromHeightMap createAGroundProfileFromAHeightMap(final HeightMap heightMap)
   {
      GroundProfileFromHeightMap ret = new GroundProfileFromHeightMap()
      {

         @Override
         public double heightAt(double x, double y, double z)
         {
            return heightMap.heightAt(x, y, z);
         }

         @Override
         public BoundingBox3D getBoundingBox()
         {
            return heightMap.getBoundingBox();
         }

         @Override
         public double heightAndNormalAt(double x, double y, double z, Vector3DBasics normalToPack)
         {
            normalToPack.set(0.0, 0.0, 1.0);
            return heightMap.heightAt(x, y, z);
         }
      };

      return ret;
   }

   public static GroundProfileFromHeightMap createAGroundProfileFromAHeightMapWithNormals(final HeightMapWithNormals heightMap)
   {
      GroundProfileFromHeightMap ret = new GroundProfileFromHeightMap()
      {

         @Override
         public double heightAt(double x, double y, double z)
         {
            return heightMap.heightAt(x, y, z);
         }

         @Override
         public BoundingBox3D getBoundingBox()
         {
            return heightMap.getBoundingBox();
         }

         @Override
         public double heightAndNormalAt(double x, double y, double z, Vector3DBasics normalToPack)
         {
            return heightMap.heightAndNormalAt(x, y, z, normalToPack);
         }
      };

      return ret;
   }
}