ShapeModel.cpp

#include "ShapeModel.h"

#include <QDebug>

#include <algorithm>
#include <cfloat>
#include <iostream>
#include <iomanip>
#include <vector>

#include <cmath>

#include <SpiceUsr.h>
#include <SpiceZfc.h>
#include <SpiceZmc.h>

#include "Distance.h"
#include "SurfacePoint.h"
#include "IException.h"
#include "IString.h"
#include "NaifStatus.h"
#include "Spice.h"
#include "Target.h"

using namespace std;

namespace Isis {
  ShapeModel::ShapeModel() {
    Initialize();
    m_target = NULL;
  }


  ShapeModel::ShapeModel(Target *target) {
    Initialize();
    m_target = target;
  }


  void ShapeModel::Initialize() {
    m_name = new QString();
    m_surfacePoint = new SurfacePoint();
    m_hasIntersection = false;
    m_hasNormal = false;
    m_normal.resize(3,0.);
    m_hasEllipsoidIntersection = false;
  }


  ShapeModel::~ShapeModel() {

    delete m_name;
    m_name = NULL;

    delete m_surfacePoint;
    m_surfacePoint = NULL;
  }


  bool ShapeModel::intersectSurface(const Latitude &lat, const Longitude &lon,
                                    const std::vector<double> &observerPos,
                                    const bool &backCheck) {
    // Distance radius = localRadius(lat, lon);
    return (intersectSurface(SurfacePoint(lat, lon, localRadius(lat, lon)), observerPos, backCheck));
  }


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

    // The default behavior is to set the point in the model without
    //  intersection tests at all
    setSurfacePoint(surfpt);
    return (true);
  }

  void ShapeModel::calculateEllipsoidalSurfaceNormal()  {
    // The below code is not truly normal unless the ellipsoid is a sphere.  TODO Should this be
    // fixed? Send an email asking Jeff and Stuart.  See Naif routine surfnm.c to get the true
    // for an ellipsoid.  For testing purposes to match old results do as Isis3 currently does until
    // Jeff and Stuart respond.

    if (!m_hasIntersection || !surfaceIntersection()->Valid()) {
     QString msg = "A valid intersection 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];
    pB[0] = surfaceIntersection()->GetX().kilometers();
    pB[1] = surfaceIntersection()->GetY().kilometers();
    pB[2] = surfaceIntersection()->GetZ().kilometers();

    // Unitize the vector
    SpiceDouble upB[3];
    SpiceDouble dist;
    unorm_c(pB, upB, &dist);
    memcpy(&m_normal[0], upB, sizeof(double) * 3);

    m_hasNormal = true;
  }


  double ShapeModel::emissionAngle(const std::vector<double> &observerBodyFixedPosition) {

    // Calculate the surface normal if we haven't yet
    if (!hasNormal()) calculateDefaultNormal();

    // Get vector from center of body to surface point
    SpiceDouble pB[3];
    pB[0] = surfaceIntersection()->GetX().kilometers();
    pB[1] = surfaceIntersection()->GetY().kilometers();
    pB[2] = surfaceIntersection()->GetZ().kilometers();

    // Get vector from surface point to observer and normalize it
    SpiceDouble psB[3], upsB[3], dist;
    vsub_c((ConstSpiceDouble *) &observerBodyFixedPosition[0], pB, psB);
    unorm_c(psB, upsB, &dist);

    double angle = vdot_c((SpiceDouble *) &m_normal[0], upsB);
    if(angle > 1.0) return 0.0;
    if(angle < -1.0) return 180.0;
    return acos(angle) * RAD2DEG;
  }


  bool ShapeModel::hasEllipsoidIntersection() {
    return m_hasEllipsoidIntersection;
  }


  double ShapeModel::incidenceAngle(const std::vector<double> &illuminatorBodyFixedPosition) {

    // Calculate the surface normal if we haven't yet.
    if (!m_hasNormal) calculateDefaultNormal();

    // Get vector from center of body to surface point
    SpiceDouble pB[3];
    pB[0] = surfaceIntersection()->GetX().kilometers();
    pB[1] = surfaceIntersection()->GetY().kilometers();
    pB[2] = surfaceIntersection()->GetZ().kilometers();

    // Get vector from surface point to sun and normalize it
    SpiceDouble puB[3], upuB[3], dist;
    vsub_c((SpiceDouble *) &illuminatorBodyFixedPosition[0], pB, puB);
    unorm_c(puB, upuB, &dist);

    double angle = vdot_c((SpiceDouble *) &m_normal[0], upuB);
    if(angle > 1.0) return 0.0;
    if(angle < -1.0) return 180.0;
    return acos(angle) * RAD2DEG;
  }


  bool ShapeModel::intersectEllipsoid(const std::vector<double> observerBodyFixedPosition,
                                      const std::vector<double> &observerLookVectorToTarget) {

    // Clear out previous surface point and normal
    clearSurfacePoint();

    SpiceDouble lookB[3];

    // This memcpy does:
    // lookB[0] = observerLookVectorToTarget[0];
    // lookB[1] = observerLookVectorToTarget[1];
    // lookB[2] = observerLookVectorToTarget[2];
    memcpy(lookB,&observerLookVectorToTarget[0], 3*sizeof(double));

    // get target radii
    std::vector<Distance> radii = targetRadii();
    SpiceDouble a = radii[0].kilometers();
    SpiceDouble b = radii[1].kilometers();
    SpiceDouble c = radii[2].kilometers();

    // check if observer look vector intersects the target
    SpiceDouble intersectionPoint[3];
    SpiceBoolean intersected = false;

    NaifStatus::CheckErrors();
    surfpt_c((SpiceDouble *) &observerBodyFixedPosition[0], lookB, a, b, c,
             intersectionPoint, &intersected);
    NaifStatus::CheckErrors();

    if (intersected) {
      m_surfacePoint->FromNaifArray(intersectionPoint);
      m_hasIntersection = true;
    }
    else {
      m_hasIntersection = false;
    }

    m_hasEllipsoidIntersection = m_hasIntersection;
    return m_hasIntersection;
  }


  double ShapeModel::phaseAngle(const std::vector<double> &observerBodyFixedPosition,
                                const std::vector<double> &illuminatorBodyFixedPosition) {

    // Get vector from center of body to surface point
    SpiceDouble pB[3];
    pB[0] = surfaceIntersection()->GetX().kilometers();
    pB[1] = surfaceIntersection()->GetY().kilometers();
    pB[2] = surfaceIntersection()->GetZ().kilometers();

    // Get vector from surface point to observer and normalize it
    SpiceDouble psB[3], upsB[3], dist;
    vsub_c((SpiceDouble *) &observerBodyFixedPosition[0], pB, psB);
    unorm_c(psB, upsB, &dist);

    // Get vector from surface point to sun and normalize it
    SpiceDouble puB[3], upuB[3];
    vsub_c((SpiceDouble *) &illuminatorBodyFixedPosition[0], pB, puB);
    unorm_c(puB, upuB, &dist);

    double angle = vdot_c(upsB, upuB);

    // How can these lines be tested???
    if(angle > 1.0) return 0.0;
    if(angle < -1.0) return 180.0;
    return acos(angle) * RAD2DEG;
  }


  SurfacePoint *ShapeModel::surfaceIntersection() const {
    return m_surfacePoint;
  }


  bool ShapeModel::hasIntersection() {
    return m_hasIntersection;
  }


  bool ShapeModel::hasNormal() const {
    return m_hasNormal;
  }


  void ShapeModel::clearSurfacePoint() {
    setHasIntersection(false);
    m_hasEllipsoidIntersection = false;
  }


  std::vector<double> ShapeModel::normal() {
    if (m_hasNormal ) {
      return m_normal;
    }
    else {
      QString message = "The local normal has not been computed.";
      throw IException(IException::Unknown, message, _FILEINFO_);
    }

  }

  bool ShapeModel::isVisibleFrom(const std::vector<double> observerPos,
                                 const std::vector<double> lookDirection)  {
    if ( hasIntersection() ) {
      if ( fabs(emissionAngle(observerPos)) <= 90.0 ) {
        return (true);
      }
    }

    // All other conditions indicate the point is not visable from the observer
    return (false);
  }

  bool ShapeModel::hasValidTarget() const {
    return (m_target != NULL);
  }


  std::vector<Distance> ShapeModel::targetRadii() const {
    if (hasValidTarget()) {
      return m_target->radii();
    }
    else {
      QString message = "Unable to find target radii for ShapeModel. Target is NULL. ";
      throw IException(IException::Programmer, message, _FILEINFO_);
     }
  }


  void ShapeModel::setNormal(const std::vector<double> normal) {
    if (m_hasIntersection) {
      m_normal = normal;
      m_hasNormal = true;
    }
    else {
      QString message = "No intersection point in known.  A normal can not be set.";
      throw IException(IException::Unknown, message, _FILEINFO_);
    }
  }


  void ShapeModel::setNormal(const double a, const double b, const double c) {
    if (m_hasIntersection) {
      m_normal[0] = a;
      m_normal[1] = b;
      m_normal[2] = c;
      m_hasNormal = true;
    }
    else {
      QString message = "No intersection point in known.  A normal can not be set.";
      throw IException(IException::Unknown, message, _FILEINFO_);
    }
  }


  void ShapeModel::setName(QString name) {
     *m_name = name;
  }


  QString ShapeModel::name() const{
    return *m_name;
  }


  void ShapeModel::setHasIntersection(bool b) {
    m_hasIntersection  = b;
    m_hasNormal = false;
  }


  void ShapeModel::setSurfacePoint(const SurfacePoint &surfacePoint) {
    *m_surfacePoint  = surfacePoint;

    // Update status of intersection and normal
    m_hasIntersection  = true;
    // Set normal as not calculated
    setHasNormal(false);
  }


  void ShapeModel::setHasNormal(bool status) {
    m_hasNormal = status;
  }


  double ShapeModel::resolution() {
    if (hasValidTarget() && m_hasIntersection) {
      return m_target->spice()->resolution();
    }
    else {
      QString message = "No valid intersection point for computing resolution.";
      throw IException(IException::Programmer, message, _FILEINFO_);
    }
  }

}