BulletDskShape.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
 
#include "BulletDskShape.h"
 
#include <iostream>
#include <iomanip>
#include <numeric>
#include <sstream>
 
#include "NaifDskApi.h"
 
#include <QMutexLocker>
#include <QTime>
 
#include "FileName.h"
#include "IException.h"
#include "IString.h"
#include "Pvl.h"
#include "NaifDskPlateModel.h"
#include "NaifStatus.h"
 
using namespace std;
 
namespace Isis {
 
  BulletDskShape::BulletDskShape() :  m_mesh() { }
 
 
  BulletDskShape::BulletDskShape(const QString &dskfile) : m_mesh()  {
    loadFromDsk(dskfile);
    setMaximumDistance();
  }
 
 
  BulletDskShape::~BulletDskShape() {
    // Bullet does not clean up the mesh automatically, so we need to delete it manually
    if (m_mesh) {
      for (int i = 0; i < m_mesh->getIndexedMeshArray().size(); i++) {
        btIndexedMesh &v_mesh = m_mesh->getIndexedMeshArray()[i];
        delete[] v_mesh.m_triangleIndexBase;
        v_mesh.m_triangleIndexBase = nullptr;
        delete[] v_mesh.m_vertexBase;
        v_mesh.m_vertexBase = nullptr;
      }
    }
  }
 
 
  int BulletDskShape::getNumTriangles() const {
    size_t num_triangles = 0;
 
    if (m_mesh) {
      for(int i = 0; i < m_mesh->getIndexedMeshArray().size(); i++) {
        num_triangles += m_mesh->getIndexedMeshArray()[i].m_numTriangles;
      }
    }
 
    return num_triangles;
  }
 
 
  int BulletDskShape::getNumVertices() const {
    size_t num_vertices = 0;
 
    if (m_mesh) {
      for(int i = 0; i < m_mesh->getIndexedMeshArray().size(); i++) {
        num_vertices += m_mesh->getIndexedMeshArray()[i].m_numVertices;
      }
    }
 
    return num_vertices;
  }
 
 
  btVector3 BulletDskShape::getNormal(const int indexId, const int segment) const {
    btMatrix3x3 triangle = getTriangle(indexId, segment);
    btVector3 edge1 = triangle.getRow(1) - triangle.getRow(0);
    btVector3 edge2 = triangle.getRow(2) - triangle.getRow(0);
    return ( edge1.cross( edge2 ) );
  }
 
 
  btMatrix3x3 BulletDskShape::getTriangle(const int index, const int segment) const {
    btAssert ( index >= 0 );
    btAssert ( index < getIndexedMeshArray()[segment].m_numTriangles );
 
    btAssert ( segment >= 0 );
    btAssert ( segment < getIndexedMeshArray().size());
 
     // Set up pointers to triangle indexes
    const btIndexedMesh &v_mesh = m_mesh->getIndexedMeshArray()[segment];
 
    const int *t_index = static_cast<int32_t *> ((void *) v_mesh.m_triangleIndexBase);
    int p_index = 3 * index;
    int vndx0 = t_index[p_index];
    int vndx1 = t_index[p_index+1];
    int vndx2 = t_index[p_index+2];
 
    const btScalar *t_vertex = static_cast<const btScalar *> ((void *) v_mesh.m_vertexBase);
 
    btMatrix3x3 triangle(t_vertex[vndx0+0], t_vertex[vndx0+1], t_vertex[vndx0+2],
                         t_vertex[vndx1+0], t_vertex[vndx1+1], t_vertex[vndx1+2],
                         t_vertex[vndx2+0], t_vertex[vndx2+1], t_vertex[vndx2+2]);
    return ( triangle );
  }
 
 
  void BulletDskShape::loadFromDsk(const QString &dskfile) {
 
    SpiceInt                   handle;   
 
    // Sanity check
    FileName dskFile(dskfile);
    if ( !dskFile.fileExists() ) {
      QString mess = "NAIF DSK file [" + dskfile + "] does not exist.";
      throw IException(IException::User, mess, _FILEINFO_);
    }
 
    // Open the NAIF Digital Shape Kernel (DSK)
    dasopr_c( dskFile.expanded().toLatin1().data(), &handle );
    NaifStatus::CheckErrors();
 
    // Search to the first DLA segment
    SpiceBoolean  found;
    SpiceDLADescr segment;
    dlabfs_c( handle, &segment, &found );
    NaifStatus::CheckErrors();
    if ( !found ) {
      QString mess = "No segments found in DSK file " + dskfile ;
      throw IException(IException::User, mess, _FILEINFO_);
    }
 
    std::vector<SpiceDLADescr> segments;
    segments.push_back(segment);
 
    // Iterate until you find no more segments.
    while(found) {
      dlafns_c(handle, &segments.back(), &segment, &found);
      NaifStatus::CheckErrors();
      if (found)
        segments.push_back(segment);
    }
 
    // dskgd_c( v_handle, &v_dladsc, &v_dskdsc );
    // NaifStatus::CheckErrors();
 
    // Now allocate a new indexed mesh to contain all the DSK data
    m_mesh.reset( new btTriangleIndexVertexArray());
 
    for (size_t i = 0; i < segments.size(); i++) {
      SpiceInt nplates;
      SpiceInt nvertices;
 
      btIndexedMesh i_mesh;
 
      // Get size/counts
      dskz02_c( handle, &segments[i], &nvertices, &nplates);
      NaifStatus::CheckErrors();
 
      m_mesh->addIndexedMesh(i_mesh, PHY_INTEGER);
 
      // Get internal mesh reference and set parameters appropriately
      btIndexedMesh &v_mesh = m_mesh->getIndexedMeshArray()[i];
      v_mesh.m_vertexType = PHY_DOUBLE;
 
      // Set and allocate data for triangle indexes
      v_mesh.m_numTriangles = nplates;
      v_mesh.m_triangleIndexBase = new unsigned char[nplates * 3 * sizeof(int)];
      v_mesh.m_triangleIndexStride = (sizeof(int) * 3);
 
      // Set and allocate vertex data
      v_mesh.m_numVertices = nvertices;
      v_mesh.m_vertexBase = new unsigned char[nvertices * 3 * sizeof(double)];
      v_mesh.m_vertexStride = (sizeof(double) * 3);
 
      SpiceInt n;
      (void) dskv02_c(handle, &segments[i], 1, nvertices, &n,
                      ( SpiceDouble(*)[3] ) (v_mesh.m_vertexBase));
      NaifStatus::CheckErrors();
 
      // Read the indexes from the DSK
      (void) dskp02_c(handle, &segments[i], 1, nplates, &n,
                      ( SpiceInt(*)[3] ) (v_mesh.m_triangleIndexBase));
      NaifStatus::CheckErrors();
 
      // Got to reset the vertex indexes to 0-based
      int *pindex = static_cast<int *> ((void *) v_mesh.m_triangleIndexBase);
      int nverts = nplates * 3;
      for (int i = 0 ; i < nverts ; i++) {
        pindex[i] -= 1;
        btAssert ( pindex[i] >= 0 );
        btAssert ( pindex[i] < nvertices );
      }
    }
 
    // Close DSK
    dascls_c(handle);
 
    bool useQuantizedAabbCompression = true;
    // bool useQuantizedAabbCompression = false;
    btBvhTriangleMeshShape *v_triShape = new btBvhTriangleMeshShape(m_mesh.data(),
                                                                    useQuantizedAabbCompression);
    v_triShape->setUserPointer(this);
    btCollisionObject *vbody = new btCollisionObject();
    vbody->setCollisionShape(v_triShape);
    setTargetBody(vbody);
 
    return;
 
  }
 
}  // namespace Isis