8#include "EmbreeTargetShape.h"
15#include "NaifDskApi.h"
18#include "IException.h"
20#include "NaifStatus.h"
30 ray.tfar = std::numeric_limits<float>::infinity();
31 ray.mask = 0xFFFFFFFF;
35 hit.geomID = RTC_INVALID_GEOMETRY_ID;
36 hit.primID = RTC_INVALID_GEOMETRY_ID;
37 hit.instID[0] = RTC_INVALID_GEOMETRY_ID;
57 const std::vector<double> &direction) {
59 ray.tfar = std::numeric_limits<float>::infinity();
60 ray.mask = 0xFFFFFFFF;
64 hit.geomID = RTC_INVALID_GEOMETRY_ID;
65 hit.primID = RTC_INVALID_GEOMETRY_ID;
66 hit.instID[0] = RTC_INVALID_GEOMETRY_ID;
67 ray.org_x = origin[0];
68 ray.org_y = origin[1];
69 ray.org_z = origin[2];
70 ray.dir_x = direction[0];
71 ray.dir_y = direction[1];
72 ray.dir_z = direction[2];
89 ray.tfar = std::numeric_limits<float>::infinity();
90 ray.mask = 0xFFFFFFFF;
94 hit.geomID = RTC_INVALID_GEOMETRY_ID;
95 hit.primID = RTC_INVALID_GEOMETRY_ID;
96 hit.instID[0] = RTC_INVALID_GEOMETRY_ID;
97 ray.org_x = origin[0];
98 ray.org_y = origin[1];
99 ray.org_z = origin[2];
100 ray.dir_x = direction[0];
101 ray.dir_y = direction[1];
102 ray.dir_z = direction[2];
114 ray.tfar = std::numeric_limits<float>::infinity();
115 ray.mask = 0xFFFFFFFF;
120 hit.geomID = RTC_INVALID_GEOMETRY_ID;
121 hit.primID = RTC_INVALID_GEOMETRY_ID;
122 hit.instID[0] = RTC_INVALID_GEOMETRY_ID;
142 const std::vector<double> &direction) {
144 ray.tfar = std::numeric_limits<float>::infinity();
145 ray.mask = 0xFFFFFFFF;
149 hit.geomID = RTC_INVALID_GEOMETRY_ID;
151 hit.instID[0] = RTC_INVALID_GEOMETRY_ID;
152 ray.org_x = origin[0];
153 ray.org_y = origin[1];
154 ray.org_z = origin[2];
155 ray.dir_x = direction[0];
156 ray.dir_y = direction[1];
157 ray.dir_z = direction[2];
173 ray.tfar = std::numeric_limits<float>::infinity();
174 ray.mask = 0xFFFFFFFF;
178 hit.geomID = RTC_INVALID_GEOMETRY_ID;
180 hit.instID[0] = RTC_INVALID_GEOMETRY_ID;
181 ray.org_x = origin[0];
182 ray.org_y = origin[1];
183 ray.org_z = origin[2];
184 ray.dir_x = direction[0];
185 ray.dir_y = direction[1];
186 ray.dir_z = direction[2];
232 rtcSetSceneFlags(
m_scene, RTC_SCENE_FLAG_ROBUST | RTC_SCENE_FLAG_CONTEXT_FILTER_FUNCTION);
233 rtcSetSceneBuildQuality(
m_scene, RTC_BUILD_QUALITY_HIGH);
249 rtcSetSceneFlags(
m_scene, RTC_SCENE_FLAG_ROBUST | RTC_SCENE_FLAG_CONTEXT_FILTER_FUNCTION);
250 rtcSetSceneBuildQuality(
m_scene, RTC_BUILD_QUALITY_HIGH);
270 rtcSetSceneFlags(
m_scene, RTC_SCENE_FLAG_ROBUST | RTC_SCENE_FLAG_CONTEXT_FILTER_FUNCTION);
271 rtcSetSceneBuildQuality(
m_scene, RTC_BUILD_QUALITY_HIGH);
273 pcl::PolygonMesh::Ptr mesh;
279 QString msg =
"DEMs cannot be used to create an EmbreeTargetShape.";
283 else if (file.
extension().toLower() ==
"bds") {
292 QString msg =
"Failed creating an EmbreeTargetShape from ["
315 SpiceDLADescr dlaDescriptor;
317 SpiceDSKDescr dskDescriptor;
319 SpiceInt numVertices;
323 QString mess =
"NAIF DSK file [" + file.
expanded() +
"] does not exist.";
328 dasopr_c( file.
expanded().toLatin1().data(), &dskHandle );
333 dlabfs_c( dskHandle, &dlaDescriptor, &found );
336 QString mess =
"No segments found in DSK file [" + file.
expanded() +
"]";
340 dskgd_c( dskHandle, &dlaDescriptor, &dskDescriptor );
344 dskz02_c( dskHandle, &dlaDescriptor, &numVertices, &numPlates );
351 double *verticesArray =
new double[numVertices * 3];
352 int *polygonsArray =
new int[numPlates * 3];
355 SpiceInt numRead = 0;
356 dskv02_c(dskHandle, &dlaDescriptor, 1, numVertices,
357 &numRead, ( SpiceDouble(*)[3] )(verticesArray) );
359 if ( numRead != numVertices ) {
360 QString msg =
"Failed reading all vertices from the DSK file, ["
362 +
toString(numVertices) +
"] vertices read.";
368 dskp02_c(dskHandle, &dlaDescriptor, 1, numPlates,
369 &numRead, ( SpiceInt(*)[3] )(polygonsArray) );
371 if ( numRead != numPlates ) {
372 QString msg =
"Failed reading all polygons from the DSK file, ["
374 +
toString(numPlates) +
"] polygons read.";
379 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
380 for (
int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex) {
381 cloud->push_back(pcl::PointXYZ(verticesArray[vertexIndex * 3],
382 verticesArray[vertexIndex * 3 + 1],
383 verticesArray[vertexIndex * 3 + 2]));
387 std::vector<pcl::Vertices> polygons;
388 for (
int plateIndex = 0; plateIndex < numPlates; ++plateIndex) {
389 pcl::Vertices vertexIndices;
391 vertexIndices.vertices.push_back(polygonsArray[plateIndex * 3] - 1);
392 vertexIndices.vertices.push_back(polygonsArray[plateIndex * 3 + 1] - 1);
393 vertexIndices.vertices.push_back(polygonsArray[plateIndex * 3 + 2] - 1);
394 polygons.push_back(vertexIndices);
398 pcl::PolygonMesh::Ptr mesh(
new pcl::PolygonMesh);
399 mesh->polygons = polygons;
400 pcl::PCLPointCloud2::Ptr cloudBlob(
new pcl::PCLPointCloud2);
401 pcl::toPCLPointCloud2(*cloud, *cloudBlob);
402 mesh->cloud = *cloudBlob;
405 delete [] verticesArray;
406 delete [] polygonsArray;
422 pcl::PolygonMesh::Ptr mesh(
new pcl::PolygonMesh );
424 int loadStatus = pcl::io::load(file.
expanded().toStdString(), *mesh);
425 if (loadStatus == -1) {
426 QString msg =
"Failed loading target shape file [" + file.
expanded() +
"]";
455 pcl::fromPCLPointCloud2(mesh->cloud,
m_cloud);
458 RTCGeometry rtcMesh = rtcNewGeometry(
m_device, RTC_GEOMETRY_TYPE_TRIANGLE);
466 vertices[v].
x =
m_cloud.points[v].x;
467 vertices[v].
y =
m_cloud.points[v].y;
468 vertices[v].
z =
m_cloud.points[v].z;
477 triangles[t].
v0 =
m_mesh->polygons[t].vertices[0];
478 triangles[t].
v1 =
m_mesh->polygons[t].vertices[1];
479 triangles[t].
v2 =
m_mesh->polygons[t].vertices[2];
488 rtcCommitGeometry(rtcMesh);
489 rtcAttachGeometry(
m_scene, rtcMesh);
490 rtcReleaseGeometry(rtcMesh);
565 return m_mesh->polygons.size();
621 diagonal[0] = sceneBoundary.upper_x - sceneBoundary.lower_x;
622 diagonal[1] = sceneBoundary.upper_y - sceneBoundary.lower_y;
623 diagonal[2] = sceneBoundary.upper_z - sceneBoundary.lower_z;
659 RTCIntersectContext context;
660 rtcInitIntersectContext(&context);
675 RTCIntersectContext context;
676 rtcInitIntersectContext(&context);
678 rtcOccluded1(
m_scene, &context, (RTCRay*)&ray);
681 if (isinf(ray.ray.tfar) && ray.ray.tfar < 0) {
708 if (hitIndex > ray.
lastHit || hitIndex < 0) {
709 QString msg =
"Hit index [" +
toString(hitIndex) +
"] is out of bounds.";
713 Vertex *vertices = (
Vertex *)rtcGetGeometryBufferData(geom, RTC_BUFFER_TYPE_VERTEX, 0);
714 Triangle *triangles = (
Triangle *)rtcGetGeometryBufferData(geom, RTC_BUFFER_TYPE_INDEX, 0);
725 float u = ray.
hitUs[hitIndex];
726 float v = ray.
hitVs[hitIndex];
727 float w = 1.0 - u - v;
730 intersection[0] = w*v0.
x + u*v1.
x + v*v2.
x;
731 intersection[1] = w*v0.
y + u*v1.
y + v*v2.
y;
732 intersection[2] = w*v0.
z + u*v1.
z + v*v2.
z;
739 surfaceNormal[0] = (v1.
y - v0.
y) * (v2.
z - v0.
z)
740 - (v1.
z - v0.
z) * (v2.
y - v0.
y);
741 surfaceNormal[1] = (v1.
z - v0.
z) * (v2.
x - v0.
x)
742 - (v1.
x - v0.
x) * (v2.
z - v0.
z);
743 surfaceNormal[2] = (v1.
x - v0.
x) * (v2.
y - v0.
y)
744 - (v1.
y - v0.
y) * (v2.
x - v0.
x);
783 if (args->context ==
nullptr)
786 assert(args->N == 1);
787 int *valid = args->valid;
788 if (valid[0] != -1) {
792 RTCHit *hit = (RTCHit *)args->hit;
806 ray->hit.geomID = RTC_INVALID_GEOMETRY_ID;
807 hit->geomID = RTC_INVALID_GEOMETRY_ID;
822 if (args->context ==
nullptr)
825 assert(args->N == 1);
826 int *valid = args->valid;
827 if (valid[0] != -1) {
831 RTCHit *hit = (RTCHit *)args->hit;
832 ray->hit.primID = hit->primID;
bool isOccluded(RTCOcclusionRay &ray)
Check if a ray intersects the target body.
void initMesh(pcl::PolygonMesh::Ptr mesh)
Internalize a PointCloudLibrary polygon mesh in the target shape.
pcl::PolygonMesh::Ptr readDSK(FileName file)
Read a NAIF type 2 DSK file into a PointCloudLibrary polygon mesh.
static void occlusionFilter(const RTCFilterFunctionNArguments *args)
Filter function for collecting multiple primitiveIDs This function is called by the Embree library du...
int numberOfPolygons() const
Return the number of polygons in the target shape.
int numberOfVertices() const
Return the number of vertices in the target shape.
virtual ~EmbreeTargetShape()
Desctructor.
bool isValid() const
Return if a valid mesh is internalized and ready for use.
RTCDevice m_device
!< The point cloud representation of the target.
void addVertices(int geomID)
Adds the vertices from the internalized vertex point cloud to the Embree scene.
static void multiHitFilter(const RTCFilterFunctionNArguments *args)
Filter function for collecting multiple hits during ray intersection.
pcl::PolygonMesh::Ptr readPC(FileName file)
Read a PointCloudLibrary file into a PointCloudLibrary polygon mesh.
pcl::PolygonMesh::Ptr m_mesh
!< The name of the target.
RTCScene m_scene
!< The Embree device for rendering the scene.
void addIndices(int geomID)
Adds the polygon vertex indices from the internalized polygon mesh to the Embree scene.
RayHitInformation getHitInformation(RTCMultiHitRay &ray, int hitIndex)
Extract the intersection point and unit surface normal from an RTCMultiHitRay that has been intersect...
RTCBounds sceneBounds() const
Returns the bounds of the Embree scene.
void intersectRay(RTCMultiHitRay &ray)
Intersect a ray with the target shape.
EmbreeTargetShape()
Default empty constructor.
pcl::PointCloud< pcl::PointXYZ > m_cloud
!< A boost shared pointer to the polygon mesh representation of the target.
QString name() const
Return the name of the target shape.
double maximumSceneDistance() const
Return the maximum distance within the scene.
File name manipulation and expansion.
bool fileExists() const
Returns true if the file exists; false otherwise.
QString baseName() const
Returns the name of the file without the path and without extensions.
QString expanded() const
Returns a QString of the full file name including the file path, excluding the attributes.
QString extension() const
Returns the last extension of the file name.
@ User
A type of error that could only have occurred due to a mistake on the user's part (e....
@ Programmer
This error is for when a programmer made an API call that was illegal.
@ Io
A type of error that occurred when performing an actual I/O operation.
static Vector vector(double v0, double v1, double v2)
Constructs a 3 dimensional vector with the given component values.
static double magnitude(const Vector &vector)
Computes the magnitude (i.e., the length) of the given vector using the Euclidean norm (L2 norm).
boost::numeric::ublas::vector< double > Vector
Definition for an Isis::LinearAlgebra::Vector of doubles.
static Vector normalize(const Vector &vector)
Returns a unit vector that is codirectional with the given vector by dividing each component of the v...
static void CheckErrors(bool resetNaif=true)
This method looks for any naif errors that might have occurred.
Container for cube-like labels.
This is free and unencumbered software released into the public domain.
QString toString(bool boolToConvert)
Global function to convert a boolean to a string.
Container for a tin, or triangular polygon.
int v1
The index of the second vertex in the tin.
int v2
The index of the third vertex in the tin.
int v0
The index of the first vertex in the tin.
float y
Vertex y position.
float x
Vertex x position.
float z
Vertex z position.
Struct for capturing multiple intersections when using embree::rtcintersectscene.
float hitUs[16]
Barycentric u coordinate of the hits.
unsigned hitGeomIDs[16]
IDs of the geometries (bodies) hit.
float hitVs[16]
Barycentric v coordinate of the hits.
unsigned hitPrimIDs[16]
IDs of the primitives (trinagles) hit.
RTCMultiHitRay()
Default constructor for RTCMultiHitRay.
int lastHit
Index of the last hit in the hit containers.
Struct for capturing occluded plates when using embree::rtcintersectscene.
RTCOcclusionRay()
Default constructor for RTCOcclussionRay.
unsigned ignorePrimID
IDs of the primitives (trinagles) which should be ignored.
int lastHit
Index of the last hit in the hit containers.