BulletClosestRayCallback.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
 
#include <cmath>
 
#include <QtGlobal>
#include <QScopedPointer>
#include <QString>
#include <QVector>
 
#include "BulletClosestRayCallback.h"
#include "BulletTargetShape.h"
#include "Constants.h"
#include "IException.h"
#include "IsisBullet.h"
#include "SurfacePoint.h"
 
#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
 
namespace Isis {
 
 
  BulletClosestRayCallback::BulletClosestRayCallback() : 
    btCollisionWorld::ClosestRayResultCallback(btVector3(0,0,0), btVector3(0,0,0)), 
                            m_point(0.0, 0.0, 0.0), m_normal(0.0, 0.0, 0.0),
                            m_triangleIndex(-1), m_partId(-1) {
 
    // Set default ray tracing flags
    m_flags = defaultFlags();
  }
 
 
  BulletClosestRayCallback::BulletClosestRayCallback(const BulletClosestRayCallback &result, 
                                                     const btVector3 &point,
                                                     const btVector3 &normal) :
                            btCollisionWorld::ClosestRayResultCallback(result.observer(), result.lookdir()) {
    *this = result;
    m_point = point;
    m_normal = normal;
    return;
  }
 
 
  BulletClosestRayCallback::BulletClosestRayCallback(const btVector3 &observer, 
                                                     const btVector3 &lookdir) : 
                            btCollisionWorld::ClosestRayResultCallback(observer, lookdir), 
                            m_point(0.0, 0.0, 0.0), m_normal(0.0, 0.0, 0.0),
                            m_triangleIndex(-1), m_partId(-1) { 
 
    // Set default ray tracing flags
    m_flags = defaultFlags();
  }
 
 
  BulletClosestRayCallback::BulletClosestRayCallback(const btVector3 &observer, 
                                                     const btVector3 &lookdir,
                                                     const btCollisionWorld::RayResultCallback &source, 
                                                     btCollisionWorld::LocalRayResult &rayResult, 
                                                     bool normalInWorldSpace) :
                            btCollisionWorld::ClosestRayResultCallback(observer, lookdir), 
                            m_point(0.0, 0.0, 0.0), m_normal(0.0, 0.0, 0.0),
                            m_triangleIndex(-1), m_partId(-1)  {
     copyRayResult(*this, source);
     addSingleResult(rayResult, normalInWorldSpace);
  }
 
 
  BulletClosestRayCallback::~BulletClosestRayCallback() { }
 
 
  bool BulletClosestRayCallback::isValid() const {
    return ( hasHit() );
  }
 
 
  btVector3 BulletClosestRayCallback::observer() const {
    return ( ClosestRayResultCallback::m_rayFromWorld );
  }
 
 
  btVector3 BulletClosestRayCallback::lookdir() const {
    return ( ClosestRayResultCallback::m_rayToWorld );
  }
 
 
  btScalar BulletClosestRayCallback::fraction() const {
    return ( RayResultCallback::m_closestHitFraction );
  }
 
 
  btVector3 BulletClosestRayCallback::point() const {
    if ( hasHit() ) { 
      return ( m_point );
    }
    throw IException(IException::Programmer, 
                     "No hits in ray trace so no surface point!", 
                     _FILEINFO_);
    return ( btVector3(0.0, 0.0, 0.0) );
  }
 
 
  btVector3 BulletClosestRayCallback::normal() const {
    if ( hasHit() ) {
      return ( m_normal );
    }
 
    throw IException(IException::Programmer, 
                     "No hits in ray trace so no normal!", 
                     _FILEINFO_);
    return btVector3(0.0, 0.0, 0.0);
  }
 
 
  int BulletClosestRayCallback::triangleIndex() const {
    return ( m_triangleIndex );
  }
 
 
  int BulletClosestRayCallback::partId() const {
    return ( m_partId );
  }
 
 
  btScalar BulletClosestRayCallback::distance() const {
    if ( !isValid() ) return (DBL_MAX);
    return ( observer().distance(point()) );
  }
 
 
  btScalar BulletClosestRayCallback::distance(const BulletClosestRayCallback &other) const {
    if ( !(isValid() && other.isValid()) ) return (DBL_MAX);
    return ( point().distance( other.point() ));
  }
 
 
  btScalar BulletClosestRayCallback::distance(const btVector3 &other) const {
    if ( !isValid() ) return (DBL_MAX);
    return ( point().distance(other) );
  }
 
 
  bool BulletClosestRayCallback::operator==(const BulletClosestRayCallback &other) const {
    if ( !(isValid() && other.isValid()) ) return (false);
    if ( triangleIndex() != other.triangleIndex() ) return (false);
    if ( partId() != other.partId() ) return (false);
    return ( true );
  }
 
 
  //TODO This needs a better name
  bool BulletClosestRayCallback::isVisible(const BulletClosestRayCallback &other,
                                            const btScalar tolerance) const {
    if ( !(isValid() && other.isValid()) ) return (false);
    if ( triangleIndex() != other.triangleIndex() ) return (false);
    if ( partId() != other.partId() ) return (false);
    
    // How close are the two intercept points?
    if ( distance( other ) > tolerance ) return ( false );
 
    return (true);  // Its visable!
  }
 
  const BulletTargetShape *BulletClosestRayCallback::body() const {
    if ( !isValid() ) return (0);
    return ( static_cast<const BulletTargetShape *> (m_collisionObject->getUserPointer()) );
  }
 
 
  btScalar BulletClosestRayCallback::addSingleResult(btCollisionWorld::LocalRayResult &rayResult,
                                                     bool normalInWorldSpace) {
    btScalar hitFraction = ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
 
    m_point = m_hitPointWorld;
    m_normal = m_hitNormalWorld;
 
    // Triangle information
    m_triangleIndex = rayResult.m_localShapeInfo->m_triangleIndex;
    m_partId        = rayResult.m_localShapeInfo->m_shapePart;
    return (hitFraction);
  }
 
 
   unsigned int BulletClosestRayCallback::defaultFlags() const {
     return ( (btTriangleRaycastCallback::kF_FilterBackfaces | 
               btTriangleRaycastCallback::kF_KeepUnflippedNormal |
               btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest) );
   }
 
  void BulletClosestRayCallback::copyRayResult(btCollisionWorld::RayResultCallback &dest, 
                                               const btCollisionWorld::RayResultCallback &source) {
    dest = source;
    return;
  }
 
} // namespace Isis