Skip to content

Commit

Permalink
closest point cleanup. thread-safe
Browse files Browse the repository at this point in the history
  • Loading branch information
@amock
amock committed Apr 26, 2024
1 parent 1afa2d5 commit b7496bb
Show file tree
Hide file tree
Showing 7 changed files with 97 additions and 150 deletions.
2 changes: 1 addition & 1 deletion src/rmagine_embree/include/rmagine/map/embree/EmbreeGeometry.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ struct EmbreeClosestPointResult

struct EmbreePointQueryUserData
{
EmbreeScenePtr scene;
const EmbreeScene* scene;
EmbreeClosestPointResult* result;
};

Expand Down
2 changes: 1 addition & 1 deletion src/rmagine_embree/include/rmagine/map/embree/EmbreeMesh.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,7 @@ class EmbreeMesh
Memory<Vector, RAM> m_vertex_normals_transformed;

// boost::function<bool (RTCPointQueryFunctionArguments*)> m_closest_point_func;
RTCPointQueryFunction* m_closest_point_func_raw;
// RTCPointQueryFunction* m_closest_point_func_raw;

};

Expand Down
4 changes: 2 additions & 2 deletions src/rmagine_embree/include/rmagine/map/embree/EmbreeScene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,7 +170,7 @@ class EmbreeScene
void freeze();

// utility functions
EmbreeClosestPointResult closestPoint(const Point& qp, const float& max_distance = std::numeric_limits<float>::max());
EmbreeClosestPointResult closestPoint(const Point& qp, const float& max_distance = std::numeric_limits<float>::max()) const;

inline EmbreeDevicePtr device() const
{
Expand All @@ -179,6 +179,7 @@ class EmbreeScene

// Scene has no right to let parents stay alive
std::unordered_set<EmbreeInstanceWPtr> parents;

private:

std::unordered_map<unsigned int, EmbreeGeometryPtr > m_geometries;
Expand All @@ -188,7 +189,6 @@ class EmbreeScene

RTCScene m_scene;
EmbreeDevicePtr m_device;
RTCPointQueryContext m_pq_context;
};


Expand Down
4 changes: 4 additions & 0 deletions src/rmagine_embree/src/map/embree/EmbreeInstance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,10 @@
namespace rmagine
{


// TODO: implement instance level point query support
// - See: https://github.com/RenderKit/embree/blob/master/tutorials/closest_point/closest_point_device.cpp)

EmbreeInstance::EmbreeInstance(EmbreeDevicePtr device)
:Base(device)
{
Expand Down
213 changes: 74 additions & 139 deletions src/rmagine_embree/src/map/embree/EmbreeMesh.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,74 +24,61 @@ Point closestPointTriangle(
const Point& b,
const Point& c)
{
const Vector ab = b-a;
const Vector ac = c-a;
const Vector ap = p-a;
const Vector n = ab.cross(ac);

// TODO: comment this in and test
Matrix3x3 R;
R(0,0) = ab.x;
R(1,0) = ab.y;
R(2,0) = ab.z;

R(0,1) = ac.x;
R(1,1) = ac.y;
R(2,1) = ac.z;

R(0,2) = n.x;
R(1,2) = n.y;
R(2,2) = n.z;

Matrix3x3 Rinv = R.inv();
Vector p_in_t = Rinv * ap;
float p0 = p_in_t.x;
float p1 = p_in_t.y;

// Instead of this
const bool on_ab_edge = (p0 >= 0.f && p0 <= 1.f);
const bool on_ac_edge = (p1 >= 0.f && p1 <= 1.f);

if(on_ab_edge && on_ac_edge)
{
// in triangle
return ab * p0 + ac * p1 + a;
}
else if(on_ab_edge && !on_ac_edge)
{
// nearest point on edge (ab)
return ab * p0 + a;
}
else if(!on_ab_edge && on_ac_edge)
{
// nearest point on edge (ac)
return ac * p1 + a;
}
else
{
// nearest vertex
float d_ap = ap.l2normSquared();
float d_bp = (p - b).l2normSquared();
float d_cp = (p - c).l2normSquared();

if(d_ap < d_bp && d_ap < d_cp)
{
// a best
return a;
}
else if(d_bp < d_cp)
{
// b best
return b;
}
else
{
// c best
return c;
}
}
const Vector ab = b - a;
const Vector ac = c - a;
const Vector ap = p - a;

const float d1 = ab.dot(ap);
const float d2 = ac.dot(ap);
if (d1 <= 0.f && d2 <= 0.f)
{
return a;
}

const Vector bp = p - b;
const float d3 = ab.dot(bp);
const float d4 = ac.dot(bp);
if (d3 >= 0.f && d4 <= d3)
{
return b;
}

const Vector cp = p - c;
const float d5 = ab.dot(cp);
const float d6 = ac.dot(cp);
if (d6 >= 0.f && d5 <= d6)
{
return c;
}

const float vc = d1 * d4 - d3 * d2;
if (vc <= 0.f && d1 >= 0.f && d3 <= 0.f)
{
const float v = d1 / (d1 - d3);
return a + ab * v;
}

const float vb = d5 * d2 - d1 * d6;
if (vb <= 0.f && d2 >= 0.f && d6 <= 0.f)
{
const float v = d2 / (d2 - d6);
return a + ac * v;
}

const float va = d3 * d6 - d5 * d4;
if (va <= 0.f && (d4 - d3) >= 0.f && (d5 - d6) >= 0.f)
{
const float v = (d4 - d3) / ((d4 - d3) + (d5 - d6));
return b + (c - b) * v;
}

const float denom = 1.f / (va + vb + vc);
const float v = vb * denom;
const float w = vc * denom;
return a + ab * v + ac * w;
}


bool closestPointFunc(RTCPointQueryFunctionArguments* args)
{
assert(args->userPtr);
Expand All @@ -105,101 +92,49 @@ bool closestPointFunc(RTCPointQueryFunctionArguments* args)
EmbreePointQueryUserData* userData = (EmbreePointQueryUserData*)args->userPtr;

// query position in world space
Vector q{args->query->x, args->query->y, args->query->z};
const Vector q{args->query->x, args->query->y, args->query->z};

/*
* Get triangle information in global space
*/
EmbreeScenePtr scene = userData->scene;
EmbreeMeshPtr mesh = scene->getAs<EmbreeMesh>(geomID);
const EmbreeScene* scene = userData->scene;
const EmbreeMeshPtr mesh = scene->getAs<EmbreeMesh>(geomID);

if(mesh)
{
Face face = mesh->faces()[primID];
Vector face_normal = mesh->faceNormalsTransformed()[primID];
// Alex: I assume it can never happen that it is no mesh since the function is only used for point queries in meshes
const Face face = mesh->faces()[primID];
const Vector face_normal = mesh->faceNormalsTransformed()[primID];

Vertex v0 = mesh->verticesTransformed()[face.v0];
Vertex v1 = mesh->verticesTransformed()[face.v1];
Vertex v2 = mesh->verticesTransformed()[face.v2];
const Vertex v0 = mesh->verticesTransformed()[face.v0];
const Vertex v1 = mesh->verticesTransformed()[face.v1];
const Vertex v2 = mesh->verticesTransformed()[face.v2];

const Vector p = closestPointTriangle(q, v0, v1, v2);
const Vector p = closestPointTriangle(q, v0, v1, v2);

float d = (p - q).l2norm();
const float d = (p - q).l2norm();

if (d < args->query->radius)
{
args->query->radius = d;
if(d < userData->result->d)
{
userData->result->d = d;
userData->result->geomID = geomID;
userData->result->primID = primID;
userData->result->p = p;
userData->result->n = face_normal;
}
return true; // Return true to indicate that the query radius changed.
}
} else {
EmbreeInstancePtr inst = scene->getAs<EmbreeInstance>(geomID);

if(inst)
if (d < args->query->radius)
{
args->query->radius = d;
if(d < userData->result->d)
{
std::cout << "INSTANCING NOT SUPPORTED FOR POINTQUERIES YET!" << std::endl;
// inst->scene()
} else {
std::cout << "WARNING: " << geomID << " unknown type" << std::endl;
userData->result->d = d;
userData->result->geomID = geomID;
userData->result->primID = primID;
userData->result->p = p;
userData->result->n = face_normal;
}

return true; // Return true to indicate that the query radius changed.
}

return false;
}

bool EmbreeMesh::closestPointFunc2(RTCPointQueryFunctionArguments* args)
{
std::cout << "closestPointFunc2 called!" << std::endl;
return true;
}

EmbreeMesh::EmbreeMesh(EmbreeDevicePtr device)
:Base(device)
{
m_handle = rtcNewGeometry(device->handle(), RTC_GEOMETRY_TYPE_TRIANGLE);

rtcSetGeometryPointQueryFunction(m_handle, closestPointFunc);

// rtcSetGeometryPointQueryFunction(m_handle, [](RTCPointQueryFunctionArguments* args)
// {
// assert(args->userPtr);
// const unsigned int geomID = args->geomID;
// const unsigned int primID = args->primID;
// RTCPointQueryContext* context = args->context;
// const unsigned int stackSize = args->context->instStackSize;
// const unsigned int stackPtr = stackSize-1;

// EmbreePointQueryUserData* userData = (EmbreePointQueryUserData*)args->userPtr;

// // query position in world space
// Vector q{args->query->x, args->query->y, args->query->z};

// // std::cout << "closestPointFunc called in " << name << std::endl;

// std::cout << geomID << " " << primID << std::endl;

// return true;
// });

// rtcSetGeometryPointQueryFunction(m_handle, closestPointFunc2);
// bool (RTCPointQueryFunctionArguments*) bla = boost::bind( &EmbreeMesh::closestPointFunc2, this, _1 );
// boost::function<bool (RTCPointQueryFunctionArguments*)> func(
// boost::bind( &EmbreeMesh::closestPointFunc2, this, _1 ) );

// m_closest_point_func = boost::bind( &EmbreeMesh::closestPointFunc2, this, _1 );

// m_closest_point_func_raw = m_closest_point_func.target<RTCPointQueryFunction>();

// rtcSetGeometryPointQueryFunction(m_handle, &m_closest_point_func);

}

EmbreeMesh::EmbreeMesh(
Expand Down
18 changes: 11 additions & 7 deletions src/rmagine_embree/src/map/embree/EmbreeScene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@
#include <rmagine/math/assimp_conversions.h>
#include <rmagine/util/assimp/helper.h>

#include <omp.h>


namespace rmagine {

Expand All @@ -23,7 +25,6 @@ EmbreeScene::EmbreeScene(
{
setQuality(settings.quality);
setFlags(settings.flags);
rtcInitPointQueryContext(&m_pq_context);
}

EmbreeScene::~EmbreeScene()
Expand All @@ -40,7 +41,6 @@ EmbreeScene::~EmbreeScene()
m_ids.clear();

rtcReleaseScene(m_scene);

// std::cout << "[EmbreeScene::~EmbreeScene()] destroyed." << std::endl;
}

Expand Down Expand Up @@ -295,9 +295,13 @@ void EmbreeScene::freeze()
// std::cout << "[EmbreeScene::freeze()] finished optimizing scene.." << std::endl;
}

EmbreeClosestPointResult EmbreeScene::closestPoint(const Point& qp, const float& max_distance)
EmbreeClosestPointResult EmbreeScene::closestPoint(
const Point& qp,
const float& max_distance) const
{
// std::cout << "TODO23: check if closestPoint is working after refactoring" << std::endl;
RTCPointQueryContext ctx;
rtcInitPointQueryContext(&ctx);

RTCPointQuery query;
query.x = qp.x;
query.y = qp.y;
Expand All @@ -308,9 +312,10 @@ EmbreeClosestPointResult EmbreeScene::closestPoint(const Point& qp, const float&
EmbreeClosestPointResult result;

EmbreePointQueryUserData user_data;
user_data.scene = shared_from_this();
user_data.scene = this;
user_data.result = &result;
rtcPointQuery(m_scene, &query, &m_pq_context, nullptr, (void*)&user_data);

rtcPointQuery(m_scene, &query, &ctx, nullptr, (void*)&user_data);

return result;
}
Expand Down Expand Up @@ -360,7 +365,6 @@ EmbreeScenePtr make_embree_scene(
decompose(M, T, scale);

EmbreeScenePtr mesh_scene = std::make_shared<EmbreeScene>();

for(unsigned int i = 0; i<node->mNumMeshes; i++)
{
unsigned int mesh_id = node->mMeshes[i];
Expand Down
4 changes: 4 additions & 0 deletions tests/embree_sim/embree_closest_point.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,10 @@ int main(int argc, char ** argv)
qp = {0.1, 0.1, 20.0};
cp = map->closestPoint(qp).p;
std::cout << qp << " -> " << cp << std::endl;

qp = {-5.0, 0.0, 5.0};
cp = map->closestPoint(qp).p;
std::cout << qp << " -> " << cp << std::endl;



Expand Down

0 comments on commit b7496bb

Please sign in to comment.