8 #include "EmbreeTargetShape.h" 
   15 #include "NaifDskApi.h" 
   18 #include "IException.h" 
   20 #include "NaifStatus.h" 
   30     tfar    = std::numeric_limits<float>::infinity(); 
 
   35     geomID  = RTC_INVALID_GEOMETRY_ID;
 
   36     primID  = RTC_INVALID_GEOMETRY_ID;
 
   37     instID  = RTC_INVALID_GEOMETRY_ID;
 
   57                                  const std::vector<double> &direction) {
 
   59     tfar    = std::numeric_limits<float>::infinity(); 
 
   64     geomID  = RTC_INVALID_GEOMETRY_ID;
 
   65     primID  = RTC_INVALID_GEOMETRY_ID;
 
   66     instID  = RTC_INVALID_GEOMETRY_ID;
 
   70     dir[0] = direction[0];
 
   71     dir[1] = direction[1];
 
   72     dir[2] = direction[2];
 
   89     tfar    = std::numeric_limits<float>::infinity(); 
 
   94     geomID  = RTC_INVALID_GEOMETRY_ID;
 
   95     primID  = RTC_INVALID_GEOMETRY_ID;
 
   96     instID  = RTC_INVALID_GEOMETRY_ID;
 
  100     dir[0] = direction[0];
 
  101     dir[1] = direction[1];
 
  102     dir[2] = direction[2];
 
  114     tfar    = std::numeric_limits<float>::infinity(); 
 
  120     geomID  = RTC_INVALID_GEOMETRY_ID;
 
  121     primID  = RTC_INVALID_GEOMETRY_ID;
 
  122     instID  = RTC_INVALID_GEOMETRY_ID;
 
  142                                  const std::vector<double> &direction) {
 
  144     tfar    = std::numeric_limits<float>::infinity(); 
 
  149     geomID  = RTC_INVALID_GEOMETRY_ID;
 
  151     instID  = RTC_INVALID_GEOMETRY_ID;
 
  155     dir[0] = direction[0];
 
  156     dir[1] = direction[1];
 
  157     dir[2] = direction[2];
 
  173     tfar    = std::numeric_limits<float>::infinity(); 
 
  178     geomID  = RTC_INVALID_GEOMETRY_ID;
 
  180     instID  = RTC_INVALID_GEOMETRY_ID;
 
  184     dir[0] = direction[0];
 
  185     dir[1] = direction[1];
 
  186     dir[2] = direction[2];
 
  214       : intersection(location),
 
  215         surfaceNormal(normal) ,
 
  229         m_device(rtcNewDevice(NULL)),
 
  230         m_scene(rtcDeviceNewScene(m_device,
 
  231                                   RTC_SCENE_STATIC | RTC_SCENE_HIGH_QUALITY | RTC_SCENE_ROBUST,
 
  245         m_device(rtcNewDevice(NULL)),
 
  246         m_scene(rtcDeviceNewScene(m_device,
 
  247                                   RTC_SCENE_STATIC | RTC_SCENE_HIGH_QUALITY | RTC_SCENE_ROBUST,
 
  267         m_device(rtcNewDevice(NULL)),
 
  268         m_scene(rtcDeviceNewScene(m_device,
 
  269                                   RTC_SCENE_STATIC | RTC_SCENE_HIGH_QUALITY | RTC_SCENE_ROBUST,
 
  272     pcl::PolygonMesh::Ptr mesh;
 
  278         QString msg = 
"DEMs cannot be used to create an EmbreeTargetShape.";
 
  291       QString msg = 
"Failed creating an EmbreeTargetShape from [" 
  314     SpiceDLADescr dlaDescriptor;  
 
  316     SpiceDSKDescr dskDescriptor;  
 
  318     SpiceInt      numVertices;    
 
  322       QString mess = 
"NAIF DSK file [" + file.
expanded() + 
"] does not exist.";
 
  327     dasopr_c( file.
expanded().toLatin1().data(), &dskHandle );
 
  332     dlabfs_c( dskHandle, &dlaDescriptor, &found );
 
  335       QString mess = 
"No segments found in DSK file [" + file.
expanded() + 
"]"; 
 
  339     dskgd_c( dskHandle, &dlaDescriptor, &dskDescriptor );
 
  343     dskz02_c( dskHandle, &dlaDescriptor, &numVertices, &numPlates );
 
  350     double *verticesArray = 
new double[numVertices * 3];
 
  351     int *polygonsArray = 
new int[numPlates * 3];
 
  354     SpiceInt numRead = 0;
 
  355     dskv02_c(dskHandle, &dlaDescriptor, 1, numVertices,
 
  356              &numRead, ( SpiceDouble(*)[3] )(verticesArray) );
 
  358     if ( numRead != numVertices ) {
 
  359       QString msg = 
"Failed reading all vertices from the DSK file, [" 
  361                     + 
toString(numVertices) + 
"] vertices read.";
 
  367     dskp02_c(dskHandle, &dlaDescriptor, 1, numPlates,
 
  368              &numRead, ( SpiceInt(*)[3] )(polygonsArray) );
 
  370     if ( numRead != numPlates ) {
 
  371       QString msg = 
"Failed reading all polygons from the DSK file, [" 
  373                     + 
toString(numPlates) + 
"] polygons read.";
 
  378     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
 
  379     for (
int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex) {
 
  380       cloud->push_back(pcl::PointXYZ(verticesArray[vertexIndex * 3],
 
  381                                      verticesArray[vertexIndex * 3 + 1],
 
  382                                      verticesArray[vertexIndex * 3 + 2]));
 
  386     std::vector<pcl::Vertices> polygons;
 
  387     for (
int plateIndex = 0; plateIndex < numPlates; ++plateIndex) {
 
  388       pcl::Vertices vertexIndices;
 
  390       vertexIndices.vertices.push_back(polygonsArray[plateIndex * 3] - 1);
 
  391       vertexIndices.vertices.push_back(polygonsArray[plateIndex * 3 + 1] - 1);
 
  392       vertexIndices.vertices.push_back(polygonsArray[plateIndex * 3 + 2] - 1);
 
  393       polygons.push_back(vertexIndices);
 
  397     pcl::PolygonMesh::Ptr mesh(
new pcl::PolygonMesh);
 
  398     mesh->polygons = polygons;
 
  399     pcl::PCLPointCloud2::Ptr cloudBlob(
new pcl::PCLPointCloud2);
 
  400     pcl::toPCLPointCloud2(*cloud, *cloudBlob);
 
  401     mesh->cloud = *cloudBlob;
 
  404     delete [] verticesArray;
 
  405     delete [] polygonsArray;
 
  421     pcl::PolygonMesh::Ptr mesh( 
new pcl::PolygonMesh );
 
  423     int loadStatus = pcl::io::load(file.
expanded().toStdString(), *mesh);
 
  424     if (loadStatus == -1) {
 
  425       QString msg = 
"Failed loading target shape file [" + file.
expanded() + 
"]";
 
  454     pcl::fromPCLPointCloud2(mesh->cloud, 
m_cloud);
 
  457     unsigned geomID = rtcNewTriangleMesh(
m_scene,
 
  467     rtcSetIntersectionFilterFunction(
m_scene, geomID,
 
  471     rtcSetOcclusionFilterFunction(
m_scene, geomID,
 
  495       vertices[v].
x = 
m_cloud.points[v].x;
 
  496       vertices[v].
y = 
m_cloud.points[v].y;
 
  497       vertices[v].
z = 
m_cloud.points[v].z;
 
  500     rtcUnmapBuffer(
m_scene, geomID, RTC_VERTEX_BUFFER);
 
  520       triangles[t].
v0 = 
m_mesh->polygons[t].vertices[0];
 
  521       triangles[t].
v1 = 
m_mesh->polygons[t].vertices[1];
 
  522       triangles[t].
v2 = 
m_mesh->polygons[t].vertices[2];
 
  525     rtcUnmapBuffer(
m_scene, geomID, RTC_INDEX_BUFFER);
 
  547       return m_mesh->polygons.size();
 
  603     diagonal[0] = sceneBoundary.upper_x - sceneBoundary.lower_x;
 
  604     diagonal[1] = sceneBoundary.upper_y - sceneBoundary.lower_y;
 
  605     diagonal[2] = sceneBoundary.upper_z - sceneBoundary.lower_z;
 
  657     if (ray.geomID == 0) {
 
  684     if (hitIndex > ray.
lastHit || hitIndex < 0) {
 
  685       QString msg = 
"Hit index [" + 
toString(hitIndex) + 
"] is out of bounds.";
 
  698     float u = ray.
hitUs[hitIndex];
 
  699     float v = ray.
hitVs[hitIndex];
 
  700     float w = 1.0 - u - v;
 
  703     intersection[0] = w*v0.x + v*v1.x + u*v2.x;
 
  704     intersection[1] = w*v0.y + v*v1.y + u*v2.y;
 
  705     intersection[2] = w*v0.z + v*v1.z + u*v2.z;
 
  712     surfaceNormal[0] = (v1.y - v0.y) * (v2.z - v0.z)
 
  713                        - (v1.z - v0.z) * (v2.y - v0.y);
 
  714     surfaceNormal[1] = (v1.z - v0.z) * (v2.x - v0.x)
 
  715                        - (v1.x - v0.x) * (v2.z - v0.z);
 
  716     surfaceNormal[2] = (v1.x - v0.x) * (v2.y - v0.y)
 
  717                        - (v1.y - v0.y) * (v2.x - v0.x);
 
  766       ray.geomID = RTC_INVALID_GEOMETRY_ID;
 
  784       ray.geomID = RTC_INVALID_GEOMETRY_ID;