BulletShapeModel.cpp

Go to the documentation of this file.

#include "BulletShapeModel.h"

#include <numeric>

#include <QtGlobal>
#include <QVector>

#include "IException.h"
#include "Intercept.h"
#include "IString.h"
#include "Latitude.h"
#include "Longitude.h"
#include "NaifStatus.h"
#include "Pvl.h"
#include "ShapeModel.h"
#include "SpecialPixel.h"
#include "Statistics.h"
#include "SurfacePoint.h"
#include "Target.h"


using namespace std;

namespace Isis {

  BulletShapeModel::BulletShapeModel() : ShapeModel(), m_model(), m_tolerance(DBL_MAX),
                                         m_intercept(btVector3(0,0,0), btVector3(0,0,0)) {
    // defaults for ShapeModel parent class include:
    //     name = empty string
    //     surfacePoint = null sp
    //     hasIntersection = false
    //     hasNormal = false
    //     normal = (0,0,0)
    //     hasEllipsoidIntersection = false
    setName("Bullet");
  }

  BulletShapeModel::BulletShapeModel(Target *target, Pvl &pvl) :  
                                     ShapeModel(target), m_model(), m_tolerance(DBL_MAX),
                                     m_intercept(btVector3(0,0,0), btVector3(0,0,0)) {

    // defaults for ShapeModel parent class include:
    //     name = empty string
    //     surfacePoint = null sp
    //     hasIntersection = false
    //     hasNormal = false
    //     normal = (0,0,0)
    //     hasEllipsoidIntersection = false

    setName("Bullet");  // Really is used as type in the system at present!

    PvlGroup &kernels = pvl.findGroup("Kernels", Pvl::Traverse);

    QString shapefile;
    if (kernels.hasKeyword("ElevationModel")) {
      shapefile = (QString) kernels["ElevationModel"];
    }
    else { // if (kernels.hasKeyword("ShapeModel")) {
      shapefile = (QString) kernels["ShapeModel"];
    }

    QScopedPointer<BulletTargetShape> v_shape( BulletTargetShape::load(shapefile) );
    if (v_shape.isNull() ) {
      QString mess = "Cannot create a BulletShape from " + shapefile;
      throw IException(IException::User, mess, _FILEINFO_);
    }
    
    // Attempt to initialize the DSK file - exception ensues if errors occur
    // error thrown if ShapeModel=Null (i.e. Ellipsoid)
    m_model.reset(new BulletWorldManager(shapefile));
    m_model->addTarget( v_shape.take() );
  }


  BulletShapeModel::BulletShapeModel(BulletTargetShape *shape, Target *target, Pvl &pvl) : 
                                     ShapeModel(target), m_model(), m_tolerance(DBL_MAX), 
                                     m_intercept(btVector3(0,0,0), btVector3(0,0,0))  {
    // Attempt to initialize the DSK file - exception ensues if errors occur
    // error thrown if ShapeModel=Null (i.e. Ellipsoid)
    btAssert ( shape != 0 );
    m_model.reset( new BulletWorldManager( shape->name() ) );
    m_model->addTarget( shape );
    setName("Bullet");  // Really is used as type in the system at present!
  }

  BulletShapeModel::BulletShapeModel(BulletWorldManager *model, Target *target, Pvl &pvl) : 
                                     ShapeModel(target), m_model(model), m_tolerance(DBL_MAX), 
                                     m_intercept(btVector3(0,0,0), btVector3(0,0,0))  {

    // TODO create valid Target
    // Using this constructor, ellipsoidNormal(),
    // calculateSurfaceNormal(), and setLocalNormalFromIntercept()
    // methods can not be called
  }


  BulletShapeModel::~BulletShapeModel() { }


  double BulletShapeModel::getTolerance() const {
    return ( m_tolerance );
  }


  void   BulletShapeModel::setTolerance(const double &tolerance) {
    m_tolerance = tolerance;
  }


  bool BulletShapeModel::intersectSurface(std::vector<double> observerPos,
                                          std::vector<double> lookDirection) {

    clearSurfacePoint();
    btVector3 observer(observerPos[0], observerPos[1], observerPos[2]);
    btVector3 lookdir(lookDirection[0], lookDirection[1], lookDirection[2]);
    btVector3 rayEnd = castLookDir(observer, lookdir);
    BulletClosestRayCallback result(observer, rayEnd);
    bool success = m_model->raycast(observer, rayEnd, result);
    updateShapeModel(result);
    return ( success );
  }

  bool BulletShapeModel::intersectSurface(const Latitude &lat, const Longitude &lon,
                                          const std::vector<double> &observerPos,
                                          const bool &checkOcclusion) {

    // Set up for finding all rays along origin vector through lat/lon surface point
    clearSurfacePoint();
    btVector3 origin(0.0, 0.0, 0.0);
    btVector3 lookdir = latlonToVector(lat, lon);
    btVector3 rayEnd = castLookDir(origin, lookdir);
    BulletAllHitsRayCallback results( origin, rayEnd, false);

    // If no intersections (unlikely for this case), we are done!
    if ( !m_model->raycast(origin, rayEnd, results) ) {
      return ( false );
    }

    // Sort the intersections based on distance to the observer
    btVector3 observer(observerPos[0], observerPos[1], observerPos[2]);
    QVector<BulletClosestRayCallback> points = sortHits(results, observer);

    // If occlusion is being checked
    if ( checkOcclusion ) {
      for (int i = 0 ; i < points.size() ; i++) {

        // Check for occlusion
        //   If the hit is occluded then move on.
        //   Otherwise, it is the closest non-occluded point so take it.
        BulletClosestRayCallback &hit = points[i];
        if ( !isOccluded(hit, observer) ) {
          updateShapeModel( hit );
          break;
        }
      }
    }

    // If occlusion is not being checked, take the intersection closest to the observer
    else {
      updateShapeModel( points[0] );
    }

    // Is set by the update routine
    return ( hasIntersection() );
  }


  bool BulletShapeModel::intersectSurface(const SurfacePoint &surfpt, 
                                          const std::vector<double> &observerPos,
                                          const bool &checkOcclusion) {

    // Set up for finding all rays along origin vector through lat/lon surface point
    clearSurfacePoint();
    btVector3 origin(0.0, 0.0, 0.0);
    btVector3 surfPointVec = pointToVector(surfpt);
    btVector3 rayEnd = castLookDir(origin, surfPointVec);
    BulletAllHitsRayCallback results(origin, rayEnd, false);

    // Cast the ray to find all intersections
    //   If no intersections (unlikely for this case), we are done!
    if ( !m_model->raycast(origin, rayEnd, results) ) {
      return ( false );
    }

    // Sort the intersections based on distance to the surface point
    QVector<BulletClosestRayCallback> points = sortHits(results, surfPointVec);

    // If occlusion is being checked
    if ( checkOcclusion ) {
      btVector3 observer(observerPos[0], observerPos[1], observerPos[2]);
      for (int i = 0 ; i < points.size() ; i++) {

        // Check for occlusion
        //   If the hit is occluded then move on.
        //   Otherwise, it is the closest non-occluded point so take it.
        BulletClosestRayCallback &hit = points[i];
        if ( !isOccluded(hit, observer) ) {
          updateShapeModel( hit );
          break;
        }
      }
    }

    // If occlusion is not being checked, take the intersection closest to the observer
    else {
      updateShapeModel( points[0] );
    }

    // Is set by the update routine
    return ( hasIntersection() );

  }


  bool BulletShapeModel::isOccluded(const BulletClosestRayCallback &hit,
                                    const btVector3 &observer) const {
    // If the callback does not have an intersection return true
    if ( !hit.isValid() ) {
      return true;
    }
    
    // Check if the emission angle is greater than 90 degrees
    // If true, then it is occluded, if false then unknown
    btVector3 psB = (observer - hit.point()).normalized();
    btScalar  angle = std::acos( hit.normal().dot( psB ) ) * RAD2DEG;
    if ( std::fabs( angle ) > 90.0 - DBL_MIN) {
      return true;
    }

    // Ray cast from the observer to the intersection
    //   If we have an intersection, test for occlusion
    BulletClosestRayCallback results( observer, hit.point() );
    if ( !m_model->raycast(observer, hit.point(), results) ) {
      return false;
    }

    // Is this intersection the same as the previous intersection?
    if ( results.isVisible( hit, getTolerance() ) ) {
      return false;
    }

    return true;
  }


  void BulletShapeModel::setSurfacePoint(const SurfacePoint &surfacePoint) {
    std::vector<double> fakepos(3, 0.0);
    (void) intersectSurface(surfacePoint, fakepos, false);
  }


  void BulletShapeModel::clearSurfacePoint() {
    updateShapeModel( BulletClosestRayCallback() );
  }


  Distance BulletShapeModel::localRadius(const Latitude &lat,
                                         const Longitude &lon) {

    // Cast a ray from the origin through the surface point at the input lat/lon
    btVector3 origin(0.0, 0.0, 0.0);
    btVector3 lookdir = latlonToVector(lat, lon);
    btVector3 rayEnd = castLookDir(origin, lookdir);

    BulletAllHitsRayCallback result(origin, rayEnd, false);
    if ( m_model->raycast(origin, rayEnd, result) ) {
      BulletClosestRayCallback hit = result.hit();
      return ( Distance(hit.point().length(), Distance::Kilometers) );
    }
    return ( Distance() );
  }


  void BulletShapeModel::setLocalNormalFromIntercept()  {

    // Sanity check
    if ( !hasIntersection() ) { // hasIntersection()  <==>  !m_intercept.isNull()
      QString mess = "Intercept point does not exist - cannot provide normal vector";
      throw IException(IException::Programmer, mess, _FILEINFO_);
    }

    btVector3 normal = m_intercept.normal();
    setNormal(normal[0], normal[1], normal[2]);
  }


  bool BulletShapeModel::isDEM() const {
    return false;
  }


  bool BulletShapeModel::isVisibleFrom(const std::vector<double> observerPos,
                                       const std::vector<double> lookDirection)  {

    if ( !m_intercept.isValid() ) return (false);

    // Check if the emission angle is greater than 90 degrees
    // If true, then it is occluded, if false then unknown
    btVector3 observer(observerPos[0], observerPos[1], observerPos[2]);
    btVector3 psB = (observer - m_intercept.point()).normalized();
    btScalar  angle = std::acos( m_intercept.normal().dot( psB ) ) * RAD2DEG;
    if ( std::fabs( angle ) > 90.0 - DBL_MIN) {
      return false;
    }

    btVector3 rayEnd = castLookDir( observer,  btVector3(lookDirection[0], lookDirection[1], lookDirection[2]) );
    BulletClosestRayCallback results(observer, rayEnd);
    (void) m_model->raycast(observer, rayEnd, results);
    return ( m_intercept.isVisible( results, getTolerance() ) );
  }


  void BulletShapeModel::calculateLocalNormal(QVector<double *> neighborPoints) {

    // Sanity check
    if ( !hasIntersection() ) { // hasIntersection()  <==>  !m_intercept.isNull()
      QString mess = "Intercept point does not exist - cannot provide normal vector";
      throw IException(IException::Programmer, mess, _FILEINFO_);
    }

    setLocalNormalFromIntercept();
  }


  void BulletShapeModel::calculateDefaultNormal() {
    // ShapeModel (parent class) throws error if no intersection
     calculateSurfaceNormal();
  }

  void BulletShapeModel::calculateSurfaceNormal() {
    // ShapeModel (parent class) throws error if no intersection
    setNormal(ellipsoidNormal().toStdVector());// this takes care of setHasNormal(true);
  }


  QVector<double> BulletShapeModel::ellipsoidNormal()  {

    // Sanity check on state
    if ( !hasIntersection() ) {
       QString msg = "An intersection must be defined before computing the surface normal.";
       throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    if ( !surfaceIntersection()->Valid() ) {
       QString msg = "The surface point intersection must be valid to compute the surface normal.";
       throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    if (!hasValidTarget()) {
       QString msg = "A valid target must be defined before computing the surface normal.";
       throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // Get the coordinates of the current surface point
    SpiceDouble pB[3];
    surfaceIntersection()->ToNaifArray(pB);

    // Get the body radii and compute the true normal of the ellipsoid
    QVector<double> norm(3);
    // need a case for target == NULL
    QVector<Distance> radii = QVector<Distance>::fromStdVector(targetRadii());
    NaifStatus::CheckErrors();
    surfnm_c(radii[0].kilometers(), radii[1].kilometers(), radii[2].kilometers(),
             pB, &norm[0]);
    NaifStatus::CheckErrors();

    return (norm);
  }


  const BulletWorldManager &BulletShapeModel::model() const {
    return (*m_model);
  }


  btScalar BulletShapeModel::maxDistance() const {
    return m_model->getTarget()->maximumDistance();
  }


  btVector3  BulletShapeModel::castLookDir(const btVector3 &observer, 
                                           const btVector3 &lookdir) const {
    btScalar lookScale = observer.length() + maxDistance();
    return ( observer + lookdir.normalized() * lookScale );
  }


  btVector3 BulletShapeModel::latlonToVector(const Latitude &lat, const Longitude &lon) const {
    double latAngle = lat.radians();
    double lonAngle = lon.radians();
    btVector3 lookDir( cos(latAngle) * cos(lonAngle),
                       cos(latAngle) * sin(lonAngle),
                       sin(latAngle) );
    return ( lookDir );
  }


  btVector3 BulletShapeModel::pointToVector(const SurfacePoint &surfpt) const {
    btVector3 point;
    surfpt.ToNaifArray( &point[0] );
    return ( point );
  }


  SurfacePoint BulletShapeModel::makeSurfacePoint(const btVector3 &point) const {
    SurfacePoint surfpt;
    surfpt.FromNaifArray( &point[0] );
    return ( surfpt );
  }


  QVector<BulletClosestRayCallback> BulletShapeModel::sortHits(const BulletAllHitsRayCallback &hits,
                                                               const btVector3 &sortPoint) const {
    QMap<btScalar, BulletClosestRayCallback> sortMap;
    for (int i = 0 ; i < hits.size() ; i++) {
      BulletClosestRayCallback hit = hits.hit(i);
      sortMap.insert( hit.distance(sortPoint), hit );
    }
    return ( QVector<BulletClosestRayCallback>::fromList( sortMap.values() ) );
  }


  void BulletShapeModel::updateShapeModel(const BulletClosestRayCallback &result) {
    m_intercept = result;
    if ( m_intercept.isValid() ) {
      ShapeModel::setSurfacePoint( makeSurfacePoint(m_intercept.point()) ); // sets ShapeModel::m_hasIntersection=t, ShapeModel::m_hasNormal=f

      btVector3 normal = m_intercept.normal();
      setNormal(normal[0], normal[1], normal[2]);
    }
    else {
      ShapeModel::clearSurfacePoint();
      setHasNormal(false);
    }

    return;
  }
}; // namespace Isis