diff --git a/CMakeLists.txt b/CMakeLists.txt index 8280c82f..c27e78b6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,7 +52,7 @@ else() endif() endif() set(COMPREHENSIVE_WARNING_FLAGS "-Wall -Wextra -Wpedantic -pedantic-errors -Werror -Wfatal-errors -Wno-psabi") - set(CMAKE_CXX_FLAGS "-g ${COMPREHENSIVE_WARNING_FLAGS} -O3") + set(CMAKE_CXX_FLAGS "-g ${COMPREHENSIVE_WARNING_FLAGS} -O0") if(CMAKE_CXX_COMPILER_ID MATCHES GNU) # Make it possible to compile complex constexpr functions (gcc only) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fconstexpr-ops-limit=5000000000") diff --git a/examples/draw_triangles_intersections.cpp b/examples/draw_triangles_intersections.cpp index e5e19755..1adb713f 100644 --- a/examples/draw_triangles_intersections.cpp +++ b/examples/draw_triangles_intersections.cpp @@ -332,11 +332,11 @@ int main() auto start_wr = (vmi * start).less_one_dim(); // wr to tvp std::cout << "start_wr = " << start_wr << std::endl; - auto [hit, ti, tn] = tvp->navmesh->find_triangle_crossing (start_wr, dirn); - if (ti[0] == std::numeric_limits::max()) { + auto [hit, ti] = tvp->navmesh->find_triangle_crossing (start_wr, dirn, vm); + if (ti == std::numeric_limits::max()) { std::cout << "NO HIT\n"; } else { - std::cout << "Indices: " << ti[0] << "," << ti[1] << "," << ti[2] << std::endl; + std::cout << "Indices: " << ti << std::endl; std::cout << "Contains hit " << hit << std::endl; sv = std::make_unique>(hit, 0.07, mplot::colour::springgreen2); @@ -346,11 +346,11 @@ int main() } auto start_wr_fr2 = (vmi * start_fr2).less_one_dim(); // wr to tvp - auto [hit_fr2, ti_fr2, tn_fr2] = tvp->navmesh->find_triangle_crossing (start_wr_fr2, dirn_fr2); - if (ti[0] == std::numeric_limits::max()) { + auto [hit_fr2, ti_fr2] = tvp->navmesh->find_triangle_crossing (start_wr_fr2, dirn_fr2, vm); + if (ti_fr2 == std::numeric_limits::max()) { std::cout << "NO HIT\n"; } else { - std::cout << "Indices: " << ti_fr2[0] << "," << ti_fr2[1] << "," << ti_fr2[2] << std::endl; + std::cout << "Indices: " << ti_fr2 << std::endl; std::cout << "Contains hit " << hit_fr2 << std::endl; sv = std::make_unique>(hit_fr2, 0.07, mplot::colour::springgreen2); @@ -361,11 +361,11 @@ int main() auto start_wr_bh = (vmi * start_bh).less_one_dim(); // wr to tvp std::cout << "start_wr = " << start_wr << std::endl; - auto [hit_bh, ti_bh, tn_bh] = tvp->navmesh->find_triangle_crossing (start_wr_bh, dirn_bh); - if (ti_bh[0] == std::numeric_limits::max()) { + auto [hit_bh, ti_bh] = tvp->navmesh->find_triangle_crossing (start_wr_bh, dirn_bh, vm); + if (ti_bh == std::numeric_limits::max()) { std::cout << "NO HIT\n"; } else { - std::cout << "Indices: " << ti_bh[0] << "," << ti_bh[1] << "," << ti_bh[2] << std::endl; + std::cout << "Indices: " << ti_bh << std::endl; std::cout << "Contains hit " << hit_bh << std::endl; sv = std::make_unique>(hit_bh, 0.07, mplot::colour::springgreen2); diff --git a/examples/grid_border2.cpp b/examples/grid_border2.cpp index e84b1f66..3720cf95 100644 --- a/examples/grid_border2.cpp +++ b/examples/grid_border2.cpp @@ -15,11 +15,13 @@ #include #include #include +#include int main() { mplot::Visual v(1600, 1000, "Flat GridVisual grids with borders"); v.lightingEffects(); + v.rotateAboutNearest (true); // Create a grid to show in the scene constexpr unsigned int Nside = 4; // You can change this @@ -172,6 +174,35 @@ int main() gv->addLabel ("Triangles, border (smaller is as expected)", lblpos, mplot::TextFeatures(0.08f)); gv->finalize(); v.addVisualModel (gv); + + offset[0] += grid.width_of_pixels() * 1.2f; + gv = std::make_unique>(&grid, offset); + v.bindmodel (gv); + gv->gridVisMode = mplot::GridVisMode::Triangles; + gv->setScalarData (&data); + gv->cm.setType (mplot::ColourMapType::Cork); + gv->zScale.do_autoscale = false; + gv->zScale.null_scaling(); + gv->colourScale.do_autoscale = false; + gv->colourScale.compute_scaling (-1, 1); + // Border specific parameters + gv->showborder (false); + gv->addLabel ("Triangles, no border, showing halfedges", lblpos, mplot::TextFeatures(0.08f)); + gv->finalize(); + auto gvp = v.addVisualModel (gv); + + // Make a navmesh for this last one + gvp->make_navmesh(); + + // Add a Normals visual for the last one, too + auto nrm = std::make_unique> (gvp); + v.bindmodel (nrm); + nrm->options.set (mplot::normalsvisual_flags::show_halfedges); + nrm->options.set (mplot::normalsvisual_flags::show_boundary_next); + nrm->options.set (mplot::normalsvisual_flags::show_boundary_prev); + nrm->finalize(); + v.addVisualModel (nrm); + v.keepOpen(); return 0; diff --git a/examples/model_crawler.cpp b/examples/model_crawler.cpp index 0cc8bb45..7ca951f1 100644 --- a/examples/model_crawler.cpp +++ b/examples/model_crawler.cpp @@ -93,8 +93,8 @@ int main (int argc, char** argv) // Find the triangle that we're initially located above with // mplot::NavMesh::find_triangle_hit. This updates internal state in NavMesh. It could be // executed automatically in compute_mesh_movement - auto[hp_scene, tn0, ti0] = gvp->navmesh->find_triangle_hit (ca_view, sph_view); - std::cout << "Find hit finds hit point " << hp_scene << std::endl; + auto[hp_scene, ti0] = gvp->navmesh->find_triangle_hit (ca_view, sph_view); + std::cout << "Find hit finds hit point " << hp_scene << " with ti0 halfedge: " << ti0 << std::endl; int move_counter = 0; constexpr int move_max = 1000; @@ -106,12 +106,8 @@ int main (int argc, char** argv) // Compute a new movement over the landscape mesh (the sphere) try { ca_view = gvp->navmesh->compute_mesh_movement (mv_ca, ca_view, sph_view, hoverheight); - } catch (mplot::NavException& e) { - if (e.m_type == mplot::NavException::type::off_edge) { - std::cout << "You can handle movements that go off the edge of a flat model\n"; - } + } catch (std::exception& e) { std::cout << "Exception navigating mesh at movement count " << move_counter << ": " << e.what() << std::endl; - throw e; } // Update the viewmatrix of the coord arrows, setting its position within the scene diff --git a/examples/triangle_intersect.cpp b/examples/triangle_intersect.cpp index f99165c5..7cb21a6e 100644 --- a/examples/triangle_intersect.cpp +++ b/examples/triangle_intersect.cpp @@ -71,11 +71,11 @@ int main (int argc, char** argv) auto start_wr = (vmi * start).less_one_dim(); // wr to tvp std::cout << "start_wr = " << start_wr << std::endl; - auto [hit, ti, tn] = tvp->navmesh->find_triangle_crossing (start_wr, dirn); - if (ti[0] == std::numeric_limits::max()) { + auto [hit, ti] = tvp->navmesh->find_triangle_crossing (start_wr, dirn, vm); + if (ti == std::numeric_limits::max()) { std::cout << "NO HIT\n"; } else { - std::cout << "Indices: " << ti[0] << "," << ti[1] << "," << ti[2] << std::endl; + std::cout << "Indices: " << ti << std::endl; std::cout << "Contains hit " << hit << std::endl; sv = std::make_unique>(hit, start_sphr * 1.1f, mplot::colour::springgreen2); diff --git a/maths b/maths index db4ab32e..67a41672 160000 --- a/maths +++ b/maths @@ -1 +1 @@ -Subproject commit db4ab32e0bc8b98fbd58ac4eb6289dae4cbf1247 +Subproject commit 67a4167212fcebf86469bf9f7eb5abfdd9685db4 diff --git a/mplot/GridVisual.h b/mplot/GridVisual.h index 72ff5fae..1c6056a2 100644 --- a/mplot/GridVisual.h +++ b/mplot/GridVisual.h @@ -493,12 +493,12 @@ namespace mplot // Triangle 1 I ii = ri * dims[0] + ci; this->indices[ind_idx++] = (ii); - this->indices[ind_idx++] = (ii + dims[0] + 1); // NNE this->indices[ind_idx++] = (ii + 1); // NE + this->indices[ind_idx++] = (ii + dims[0] + 1); // NNE // Triangle 2 this->indices[ind_idx++] = (ii); - this->indices[ind_idx++] = (ii + dims[0]); // NN this->indices[ind_idx++] = (ii + dims[0] + 1); // NNE + this->indices[ind_idx++] = (ii + dims[0]); // NN } } } else if (this->grid->get_order() == sm::gridorder::topleft_to_bottomright) { diff --git a/mplot/NavMesh.h b/mplot/NavMesh.h index e3ae2eb5..178ffd8a 100644 --- a/mplot/NavMesh.h +++ b/mplot/NavMesh.h @@ -25,50 +25,42 @@ #include #include #include +#include namespace mplot { - // Exception that returns triangles that were near the location of the error - struct NavException : public std::exception + namespace mesh { - enum class type : uint32_t { generic, no_intersection, zero_mv, mv_to_vertex, undetected_crossing, nan_mv, off_edge }; + /* + * Half-edge data structures for ordered meshes + */ + template requires std::is_integral_v + struct halfedge + { + // two vertex indices for start and end of this halfedge + sm::vec vi = { std::numeric_limits::max(), std::numeric_limits::max() }; + I twin = std::numeric_limits::max(); // twin half edge + I next = std::numeric_limits::max(); // next half edge in face (or hole) + I prev = std::numeric_limits::max(); // prev half edge in face (or hole) + I flags = 0; // 0x1: boundary halfedge + }; - NavException (const type _type) : m_type(_type) {} - NavException (const type _type, const std::vector>& t) : m_type(_type) { this->tris = t; } + template requires std::is_integral_v + struct vertex + { + // Coordinate position of vertex + sm::vec p = {}; + // A halfedge (hi: halfedge index) emanating from this vertex + I hi = std::numeric_limits::max(); + }; - using std::exception::what; - const char* what() + template requires std::is_integral_v + struct face { - switch (m_type) { - case type::no_intersection: - return "No intersection (at start) with triangle or neighbours"; - break; - case type::zero_mv: - return "Zero length mv_inplane so stop/freeze/crash"; - break; - case type::mv_to_vertex: - return "We've moved to a vertex, should have captured this case"; - break; - case type::undetected_crossing: - return "Should have detected crossing just now"; - break; - case type::nan_mv: - return "mv_inplane contained NaN"; - break; - case type::off_edge: - return "The movement went off the edge of the model"; - break; - case type::generic: - default: - break; - } - return "Generic"; - } - // Error type determines message generated - type m_type = type::generic; - // Triangles of interest (as indices into NavMesh::vertex) - std::vector> tris; - }; + // The index of the starting halfedge that records the existence of this face + I hi = std::numeric_limits::max(); + }; + } /*! * Navigation mesh of triangles. @@ -81,45 +73,101 @@ namespace mplot * Minimum set of vertices to generate a topological mesh. populated by * VisualModel::make_navmesh() */ - std::vector> vertex; + std::vector> vertex = {}; /*! - * The edges that make up the same triangles as are shown with the parent VisualModel's - * indices & vertexPositions, but in terms of this->vertex. Each edge must be two indices - * in *ascending numerical order*. populated by VisualModel::make_navmesh() + * The vector of half edges in the mesh */ - std::set> edges; + std::vector> halfedge = {}; /*! - * Triangles too. Might be more useful than edges. Triangle given as indices into - * this->vertex. populated by VisualModel::make_navmesh() + * Triangle mesh faces. populated by VisualModel::make_navmesh() */ - sm::vvec, sm::vec, sm::vec, sm::vec>> triangles; - - /*! - * Maps index in vertex to the original parent->indices index. populated by - * VisualModel::make_navmesh() - */ - sm::vvec> vertexidx_to_indices; + std::vector> triangles = {}; //! Holds a copy of the bb of the parent model sm::range> bb; //! When navigating, this is the 'current triangle' that you're located over/near - std::array ti0 = {}; + uint32_t ti0 = std::numeric_limits::max(); - /*! - * The normal of ti0. This is the current triangle normal (in our mesh's frame of - * reference) that our agent/camera is 'next to' - */ - sm::vec tn0 = {}; + void save (const std::string& filename) const + { + std::cout << "Save NavMesh to " << filename << std::endl; - /*! - * Stabilisation flag: if true, no rotation is applied when moving over a triangle boundary - * in NavMesh::compute_mesh_movement. If false, then a rotation about the triangle boundary - * is made. - */ - bool stabilised = false; + std::ofstream fout (filename, std::ios::binary | std::ios::out | std::ios::trunc); + if (!fout.is_open()) { + std::cerr << "NavMesh::save: Failed to open " << filename << " for writing, continue\n"; + return; + } + // fout is open + uint64_t vertex_sz = this->vertex.size(); + uint64_t halfedge_sz = this->halfedge.size(); + uint64_t triangles_sz = this->triangles.size(); + + // Write sizes at head of file, as the first thing + sm::util::binary_write (fout, vertex_sz); + sm::util::binary_write (fout, halfedge_sz); + sm::util::binary_write (fout, triangles_sz); // 3 * 8 = 24 bytes + + // Write the bb range next. + sm::util::binary_write (fout, this->bb.min); + sm::util::binary_write (fout, this->bb.max); // 2 * 3 * 4 = 24 bytes + + // Now loop + for (auto v : this->vertex) { + sm::util::binary_write (fout, v.p); + sm::util::binary_write (fout, v.hi); // 3 * 4 + 4 = 16 bytes per line + } + + for (auto he : this->halfedge) { + sm::util::binary_write (fout, he.vi); + sm::util::binary_write (fout, he.twin); + sm::util::binary_write (fout, he.next); + sm::util::binary_write (fout, he.prev); // 5 * 4 = 20 bytes per line + sm::util::binary_write (fout, he.flags); // plus 4 + } + + for (auto t : this->triangles) { sm::util::binary_write (fout, t.hi); } + } + + void load (const std::string& filename) + { + std::cout << "Load NavMesh from " << filename << std::endl; + + std::ifstream fin (filename, std::ios::binary | std::ios::in); + if (!fin.is_open()) { throw std::runtime_error ("NavMesh::load: Failed to open file"); } + + uint64_t vertex_sz = 0; + uint64_t halfedge_sz = 0; + uint64_t triangles_sz = 0; + + sm::util::binary_read (fin, vertex_sz); + sm::util::binary_read (fin, halfedge_sz); + sm::util::binary_read (fin, triangles_sz); // 3 * 8 = 24 bytes + + sm::util::binary_read (fin, this->bb.min); + sm::util::binary_read (fin, this->bb.max); + + this->vertex.resize (vertex_sz); + this->halfedge.resize (halfedge_sz); + this->triangles.resize (triangles_sz); + + for (auto& v : this->vertex) { + sm::util::binary_read (fin, v.p); + sm::util::binary_read (fin, v.hi); // 3 * 4 + 4 = 16 bytes per line + } + + for (auto& he : this->halfedge) { + sm::util::binary_read (fin, he.vi); + sm::util::binary_read (fin, he.twin); + sm::util::binary_read (fin, he.next); + sm::util::binary_read (fin, he.prev); // 5 * 4 = 20 bytes per line + sm::util::binary_read (fin, he.flags); + } + + for (auto& t : this->triangles) { sm::util::binary_read (fin, t.hi); } + } /*! * Return index of this->vertex that is closest to scene_coord. Can use vertexidx_to_indices @@ -135,7 +183,7 @@ namespace mplot // Brute force it. (But we have a mesh; can this guarantee a faster search? I don't think so) float min_d = std::numeric_limits::max(); for (uint32_t j = 0; j < this->vertex.size(); ++j) { - sm::vec vcoord = (viewmatrix * this->vertex[j]).less_one_dim(); + sm::vec vcoord = (viewmatrix * this->vertex[j].p).less_one_dim(); float d = (scene_coord - vcoord).length(); if (d < min_d) { min_d = d; @@ -145,23 +193,122 @@ namespace mplot return i; } + bool verify_halfedge_chain (const uint32_t _hi, + const uint32_t chain_length = std::numeric_limits::max(), + const bool debug = false) const + { + constexpr uint32_t max = std::numeric_limits::max(); + + if (_hi >= this->halfedge.size()) { return false; } + + uint32_t fcount = 0u; + uint32_t fhi = _hi; + + if (debug) { + // Loop through once showing chain info (index, prev, next, twin) + do { + std::cout << "(prev: " << this->halfedge[fhi].prev << ") halfedge[" + << fhi << "] (next: " << this->halfedge[fhi].next << ") has twin " + << this->halfedge[fhi].twin << " and flags " << this->halfedge[fhi].flags << std::endl; + fhi = this->halfedge[fhi].next; + ++fcount; + } while (fhi != _hi && fhi != max && fcount < this->halfedge.size()); + // Reset for forward again: + fcount = 0u; + fhi = _hi; + } + + // Each boundary should be the same forwards and backwards from every possible start halfedge + do { + //std::cout << "this->halfedge["<halfedge[fhi].flags << " go to next..." << std::endl; + if (this->halfedge[fhi].next == fhi) { throw std::runtime_error ("self next referral!"); } + fhi = this->halfedge[fhi].next; + ++fcount; + } while (fhi != _hi && fhi != max && fcount < this->halfedge.size()); + + if (debug) { + std::cout << "From forwards loop: fcount = " << fcount << " and fhi = " << fhi << " cf _hi = " << _hi << std::endl; + } + + uint32_t rcount = 0u; + uint32_t rhi = _hi; + do { + //std::cout << "this->halfedge["<halfedge[rhi].flags << " go to prev..." << std::endl; + if (this->halfedge[rhi].prev == rhi) { throw std::runtime_error ("self prev referral!"); } + rhi = this->halfedge[rhi].prev; + ++rcount; + } while (rhi != _hi && rhi != max && rcount < this->halfedge.size()); + + if (debug) { + std::cout << "From reverse loop: rcount = " << rcount << " and rhi = " << rhi << " cf _hi = " << _hi << std::endl; + } + + if (fcount == rcount && rhi == _hi && fhi == _hi) { + if (chain_length != max) { + if (fcount == chain_length) { + return true; + } else { + return false; + } + } else { + return true; + } + } else { + return false; + } + } + bool verify_boundary (const uint32_t boundary_hi, const bool debug = false) const + { return verify_halfedge_chain (boundary_hi, std::numeric_limits::max(), debug); } + bool verify_triangle (const uint32_t tri_hi, const bool debug = false) const + { return verify_halfedge_chain (tri_hi, 3, debug); } + // Return the three vertices for the triangle specified as three indices into NavMesh::vertex - sm::vec, 3> triangle_vertices (const std::array& tri_indices) const + sm::vec, 3> triangle_vertices (uint32_t tri_hi) const { - sm::vec, 3> trivert; - if (tri_indices[0] < this->vertex.size()) { trivert[0] = this->vertex[tri_indices[0]]; } - if (tri_indices[1] < this->vertex.size()) { trivert[1] = this->vertex[tri_indices[1]]; } - if (tri_indices[2] < this->vertex.size()) { trivert[2] = this->vertex[tri_indices[2]]; } + sm::vec, 3> trivert = {}; + if (tri_hi == std::numeric_limits::max()) { + std::cout << "tri_hi is unset?\n"; + return trivert; + } + + uint32_t hi = tri_hi; + for (uint32_t i = 0; i < 3; ++i) { + if (this->halfedge[hi].vi[0] < this->vertex.size()) { + trivert[i] = this->vertex[this->halfedge[hi].vi[0]].p; + } + hi = this->halfedge[hi].next; + if (hi >= this->halfedge.size()) { break; } + } + if (hi != tri_hi) { + // Triangle didn't close. This can occur at the edge of a flat model + trivert[0][0] = std::numeric_limits::max(); // to tell client code + } + return trivert; } // Return the three vertices for the triangle specified as three indices into NavMesh::vertex transformed by transform - sm::vec, 3> triangle_vertices (const std::array& tri_indices, const sm::mat& transform) const + sm::vec, 3> triangle_vertices (uint32_t tri_hi, const sm::mat& transform) const { - sm::vec, 3> trivert; - if (tri_indices[0] < this->vertex.size()) { trivert[0] = (transform * this->vertex[tri_indices[0]]).less_one_dim(); } - if (tri_indices[1] < this->vertex.size()) { trivert[1] = (transform * this->vertex[tri_indices[1]]).less_one_dim(); } - if (tri_indices[2] < this->vertex.size()) { trivert[2] = (transform * this->vertex[tri_indices[2]]).less_one_dim(); } + sm::vec, 3> trivert = {}; + if (tri_hi == std::numeric_limits::max()) { + std::cout << "tri_hi is unset?\n"; + return trivert; + } + + uint32_t hi = tri_hi; + for (uint32_t i = 0; i < 3; ++i) { + if (this->halfedge[hi].vi[0] < this->vertex.size()) { + trivert[i] = (transform * this->vertex[this->halfedge[hi].vi[0]].p).less_one_dim(); + } + hi = this->halfedge[hi].next; + if (hi >= this->halfedge.size()) { break; } + } + if (hi != tri_hi) { + // Triangle didn't close. This can occur at the edge of a flat model + trivert[0][0] = std::numeric_limits::max(); // to tell client code + } + return trivert; } @@ -173,204 +320,406 @@ namespace mplot return n; } - sm::vvec neighbours (const uint32_t _idx) const + // Retrieve the halfedge as a vector, transformed by the given transform + sm::vec edge_vector (uint32_t hi, const sm::mat& transform) const { - sm::vvec rtn; - // Search edges to find those that include _idx and then pack up the other ends in a return object - for (auto e : this->edges) { - // we have e[0] and e[1] - if (e[0] == _idx) { - // neighb is e[1] - rtn.push_back (e[1]); - } else if (e[1] == _idx) { - // neighb is e[0] - rtn.push_back (e[0]); - } - } - return rtn; + const sm::vec v0 = (transform * this->vertex[this->halfedge[hi].vi[0]].p).less_one_dim(); + const sm::vec v1 = (transform * this->vertex[this->halfedge[hi].vi[1]].p).less_one_dim(); + return v1 - v0; } - // Determine if ti0 is on the edge of the model (with < 3 edge neighbours), If so, place 1 - // in its final element. Also mark as on edge any nighbours sharing one of its vertices - uint32_t mark_if_on_edge (std::array& _ti0) + // Find all the neighbours of triangle *vertex* index a, throwing exceptions on errors. + // Returns the same vector of halfedge indices as find_neighbours, but without assuming that the navmesh is good. + // \return vector of halfedge indices + std::vector + test_neighbours (const uint32_t a) const { - constexpr bool debug_met = false; - uint32_t n2 = 0; // Neighbours sharing 2 vertices (up to 3) - - std::vector*> neighb_edge_tris; - - for (auto& t: this->triangles) { - auto [ti, tn, tnc, tnd] = t; - auto a0 = _ti0[0]; - auto b0 = _ti0[1]; - auto c0 = _ti0[2]; - auto a = ti[0]; - auto b = ti[1]; - auto c = ti[2]; - if (( a == a0 && ((b == b0 && c != c0) || (c == b0 && b != c0))) - || (b == a0 && ((a == b0 && c != c0) || (c == b0 && a != c0))) - || (c == a0 && ((a == b0 && b != c0) || (b == b0 && a != c0)))) { - ++n2; - neighb_edge_tris.push_back (&ti); - } - else if (( a == b0 && ((b == c0 && c != a0) || (c == c0 && b != a0))) - || (b == b0 && ((a == c0 && c != a0) || (c == c0 && a != a0))) - || (c == b0 && ((a == c0 && b != a0) || (b == c0 && a != a0)))) { - ++n2; - neighb_edge_tris.push_back (&ti); + uint32_t hi = a; + std::vector rtn = {}; + + if (hi > this->halfedge.size()) { return rtn; } + // We have to defensively check for repeated halfedges as we cycle around neighbours. + std::set repeat; + do { + if (repeat.count (hi)) { + // hi is a repeat. This means the mesh isn't perfect. + std::cout << "test_neighbours: Found a repeated halfedge that wasn't the first one\n"; + for (auto h : rtn) { + std::cout << h << " flag: " << this->halfedge[h].flags + << " prev: " << this->halfedge[h].prev + << " prev.twin: " << this->halfedge[this->halfedge[h].prev].twin << std::endl; + } + std::cout << "The repeat was: " << hi << std::endl; + throw std::runtime_error ("Repeated non-start halfedge"); // caused by 2 boundary halfedges with the same 'prev' } - else if (( a == c0 && ((b == a0 && c != b0) || (c == a0 && b != b0))) - || (b == c0 && ((a == a0 && c != b0) || (c == a0 && a != b0))) - || (c == c0 && ((a == a0 && b != b0) || (b == a0 && a != b0)))) { - ++n2; - neighb_edge_tris.push_back (&ti); + // hi emanates from the vertex, so return it. + rtn.push_back (hi); + repeat.insert (hi); + uint32_t pr = this->halfedge[hi].prev; + if (pr == std::numeric_limits::max()) { + throw std::runtime_error ("halfedge.prev was unset!?!"); } - } + hi = this->halfedge[pr].twin; + // or hi = this->halfedge[this->halfedge[hi].twin].next; // Clockwise + } while (hi != a && hi != std::numeric_limits::max()); - if (n2 < 3) { - if constexpr (debug_met) { - std::cout << _ti0[0] << "-" << _ti0[1] << "-" << _ti0[2] << " is on the edge"; - } - _ti0[3] = 1; - for (auto& net : neighb_edge_tris) { - if constexpr (debug_met) { std::cout << " mark vtx neighbour "; } - (*net)[3] = 1; - } - if constexpr (debug_met) { std::cout << std::endl; } + if (hi == std::numeric_limits::max()) { throw std::runtime_error ("A twin was unset!?!"); } + return rtn; + } - return neighb_edge_tris.size() + 1; - } // Meaning that the triangle is 'on the edge' of the model - return 0; + // Find all the neighbours of triangle *vertex* index a. + // Assumes the navmesh is good, and has passed NavMesh::test() + // \return vector of halfedge indices + std::vector find_neighbours (const uint32_t a) const + { + uint32_t hi = a; + std::vector rtn = {}; + if (hi > this->halfedge.size()) { return rtn; } + do { + // hi emanates from the vertex, so return it. + rtn.push_back (hi); + uint32_t pr = this->halfedge[hi].prev; + hi = this->halfedge[pr].twin; + // or hi = this->halfedge[this->halfedge[hi].twin].next; // Clockwise + } while (hi != a); + return rtn; } - // Go through all triangles, marking if they're an 'edge' triangle. A triangle is ALSO on - // the edge if on of its neighbours has < 3 edge neighbours. - void mark_edge_triangles() + // Test the navmesh, to make sure it is perfect + void test() { - constexpr bool debug_met = false; - uint32_t ec = 0; - for (auto& t: this->triangles) { - auto& [ti, tn, tnc, tnd] = t; - ec += mark_if_on_edge (ti); // ALSO loops through triangles - } - if constexpr (debug_met) { - std::cout << ec << " / " << this->triangles.size() << " triangles are on edge\n"; + std::cout << __func__ << " called to test the navmesh\n"; + // 1) Verify that each halfedge is part of a face or hole (boundary) + for (uint32_t hi = 0; hi < this->halfedge.size(); ++hi) { + if ((this->halfedge[hi].flags & 0x1) == 0x1) { + //std::cout << "This halfedge was marked as boundary\n"; + if (this->verify_boundary (hi) == false) { + throw std::runtime_error ("Imperfect halfedge mesh (boundary hole)"); + } + } else { + if (this->verify_triangle (hi) == false) { + throw std::runtime_error ("Imperfect halfedge mesh (face triangle)"); + } + + std::vector nb = this->test_neighbours (hi); + if (nb.size() > 100) { + for (auto n : nb) { + std::cout << n << std::endl; + } + throw std::runtime_error ("too many neighbours?"); + } + } + // Make sure twin is set for every halfedge + if (this->halfedge[hi].twin == std::numeric_limits::max()) { + throw std::runtime_error ("Contains an untwinned halfedge"); + } } } - // Count 2-vertex (i.e. edge) neighbours and also 1-vertex neighbours for triangle _ti0 - std::tuple count_neighbour_triangles (const std::array& _ti0) const + /* + * After making the neighbour relations from the OpenGL mesh, the last step is to fill in + * the boundary halfedges. Find all halfedges with an unset twin and then start creating the + * new half edges to fill in. + */ + void add_boundary_halfedges() { - // Count neighbour triangles - uint32_t n1 = 0; // Neighbour sharing 1 vertex (any number) - uint32_t n2 = 0; // Neighbours sharing 2 vertices (up to 3) - for (auto t: this->triangles) { - auto [ti, tn, tnc, tnd] = t; - auto a0 = _ti0[0]; - auto b0 = _ti0[1]; - auto c0 = _ti0[2]; - auto a = ti[0]; - auto b = ti[1]; - auto c = ti[2]; - - if (( a == a0 && ((b == b0 && c != c0) || (c == b0 && b != c0))) - || (b == a0 && ((a == b0 && c != c0) || (c == b0 && a != c0))) - || (c == a0 && ((a == b0 && b != c0) || (b == b0 && a != c0)))) { ++n2; } - - else if (( a == b0 && ((b == c0 && c != a0) || (c == c0 && b != a0))) - || (b == b0 && ((a == c0 && c != a0) || (c == c0 && a != a0))) - || (c == b0 && ((a == c0 && b != a0) || (b == c0 && a != a0)))) { ++n2; } - - else if (( a == c0 && ((b == a0 && c != b0) || (c == a0 && b != b0))) - || (b == c0 && ((a == a0 && c != b0) || (c == a0 && a != b0))) - || (c == c0 && ((a == a0 && b != b0) || (b == a0 && a != b0)))) { ++n2; } - - else if (( a == a0 && b != b0 && b != c0 && c != b0 && c != c0) - || (b == a0 && c != b0 && c != c0 && a != b0 && a != c0) - || (c == a0 && a != b0 && a != c0 && b != b0 && b != c0)) { ++n1; } + constexpr uint32_t max = std::numeric_limits::max(); + constexpr bool debug_bnd = false; + constexpr bool debug_bnd2 = false; + + const uint32_t sz = this->halfedge.size(); + uint32_t j = 0; + if constexpr (debug_bnd) { std::cout << "BEFORE adding boundary, halfedge.size() = " << halfedge.size() << std::endl; } + for (uint32_t i = 0; i < sz; ++i) { + if (this->halfedge[i].twin == max) { + if constexpr (debug_bnd) { std::cout << "STARTING at i = " << i; } + // This halfedge does not have a twin, walk the boundary from here + const uint32_t j0 = j; // j index at boundary start + if constexpr (debug_bnd) { std::cout << ".... with j0 = " << j0 << std::endl; } + uint32_t bprev = max; + uint32_t cur = i; + uint32_t done = 0u; + while (!done) { + if constexpr (debug_bnd2) { std::cout << "** Search for boundary from cur = " << cur << std::endl; } + uint32_t bcand = cur; // bcand starts as an internal halfedge + uint32_t bcandi = max; + if constexpr (debug_bnd2) { std::cout << "halfedge[" << i << "].twin = " << this->halfedge[i].twin << std::endl; } + uint32_t bcand0 = max; + do { + bcandi = this->halfedge[bcand].prev; + bcand = this->halfedge[bcandi].twin; // if max, it's a boundary, else it's internal + if constexpr (debug_bnd2) { std::cout << "bcandi (inner): " << bcandi << ", bcand: " << bcand << std::endl; } + if (bcand != max && bcand == bcand0) { + // We've looped back without finding a boundary halfedge. halfedge[cur] is probably a rogue halfedge/vertex + ++done; + if constexpr (debug_bnd) { std::cout << "halfedge[cur] is a rogue halfedge/vertex?\n"; } + } + if (bcand0 == max) { bcand0 = bcand; } // bcand0 tests we we looped back, but not to halfedge[i].twin + + } while (bcand != max && bcand != this->halfedge[i].twin && !done); + // && bcandi != cur <-- This last while() test probably crept in during development with out being necessary + + if (done) { + // The bcand we have right now is NOT a boundary halfedge, nor is it the + // twin for cur, so just mark halfedge flags with the 'rogue' flag (0x2) + // which can be used by NormalsVisual to show the offending halfedge + bool rogue_is_tri = this->verify_triangle (cur, true); + std::cout << "Identified a rogue, which " << (rogue_is_tri ? "IS" : "ISN'T") + << " a triangle; navmesh not likely to be usable\n"; + this->halfedge[cur].flags |= 0x2; + } else { + // Now we add the new halfedge twin for cur. + const uint32_t _bnext = sz + j + 1; + const uint32_t newi = halfedge.size(); + if constexpr (debug_bnd) { std::cout << "Push-back to halfedge[" << newi + << "] with prev = " << bprev << " next = " << _bnext + << " and twin = " << cur << std::endl; } + this->halfedge.push_back ({{this->halfedge[cur].vi[1], this->halfedge[cur].vi[0]}, cur, _bnext, bprev, 1}); + this->halfedge[cur].twin = newi; + + if (bcandi == i) { + // We've come all the way around the boundary loop and we are finished. + const uint32_t _first = sz + j0; + const uint32_t _last = sz + j; + this->halfedge[_first].prev = _last; + if constexpr (debug_bnd) { std::cout << "Update final next for halfedge[" << _last << "] to " << _first << std::endl; } + if constexpr (debug_bnd) { std::cout << "Set initial prev for halfedge[" << _first << "] to " << _last << std::endl; } + this->halfedge[_last].next = _first; + ++done; + } else { + // We've only added one new halfedge to the boundary loop, so carry on... + bprev = sz + j; + cur = bcandi; + } + ++j; + } + } + if constexpr (debug_bnd) { std::cout << "Added " << (j - j0) << " halfedges to that boundary\n"; } + } } - return {n2, n1}; + // Lastly - check through for rogues! + std::cout << "Post-search for rogues\n"; + for (uint32_t i = 0; i < sz; ++i) { + if (this->halfedge[i].twin == max) { + bool rogue_is_tri = this->verify_triangle (i, true); + std::cout << "Identified a rogue, which " << (rogue_is_tri ? "IS" : "ISN'T") << " a triangle.\n"; + // How to remove? + } + } + + if constexpr (debug_bnd) { std::cout << __func__ << " returning\n"; } } - sm::vvec> neighbour_triangles (const uint32_t _idx) const + /* + * Determine neighbour relations. That means populating a halfedge data structure. Don't + * think there's any way around the at-worst O(N^2) computation, so save results into an h5 + * file that can be loaded at startup. + * + * The key is the half-edge data structure. + * See: https://jerryyin.info/geometry-processing-algorithms/half-edge/ + */ + void compute_neighbour_relations() { - sm::vvec> rtn; - for (auto t: this->triangles) { - auto [ti, tn, tnc, tnd] = t; - // If it includes _idx, add it to rtn - if (ti[0] == _idx || ti[1] == _idx || ti[2] == _idx) { - rtn.push_back (ti); + constexpr bool debug_nr = true; + uint32_t sz = this->halfedge.size(); + if constexpr (debug_nr) { std::cout << "Finding twins for " << sz << " halfedge\n"; } + + // Search a 'band' either side of i first, assuming that neighbour faces are likely + // to have been nearby in the indices array + const uint32_t band = 3 * 1000; + + uint32_t wider_searches = 0; // Count how many times we make a wider search + + uint64_t twin_meandist = 0; // See how far a search has to search for a twin + uint32_t twins = 0; + + for (uint32_t i = 0; i < sz; ++i) { + + const sm::vec& vi = this->halfedge[i].vi; + + // halfedge[i].twin may already have been set (as we set two twins at a time) + if (this->halfedge[i].twin != std::numeric_limits::max()) { continue; } + + // It's useful to know how long you will have to wait... + if (i % 20000u == 0u) { std::cout << ((100.0f * i)/sz) << " percent...\n" << std::endl; } + + [[maybe_unused]] uint32_t sb = i >= band ? i - band : 0; + [[maybe_unused]] uint32_t eb = i + band < sz ? i + band : sz; + + uint32_t wider = 0; + +#if 0 // Simplest code + for (uint32_t j = 0; j < sz; ++j) { + if (j == i) { continue; } + const sm::vec& vij = this->halfedge[j].vi; + if (vi[0] == vij[1] && vi[1] == vij[0]) { // It's a match. + this->halfedge[i].twin = j; + this->halfedge[j].twin = i; + break; + } + } +#else // Optimize + // First sb to eb, which we hope is most likely to find a twin + for (uint32_t j = sb; j < eb; ++j) { + if (j == i) { continue; } + const sm::vec& vij = this->halfedge[j].vi; + if (vi[0] == vij[1] && vi[1] == vij[0]) { // It's a match. + this->halfedge[i].twin = j; + this->halfedge[j].twin = i; + break; + } + } + + if (this->halfedge[i].twin == std::numeric_limits::max()) { + // Then, if no match, search from 0 to sb + if (sb != 0 && !wider) { wider = 1; } + for (uint32_t j = 0; j < sb; ++j) { + if (j == i) { continue; } + const sm::vec& vij = this->halfedge[j].vi; + if (vi[0] == vij[1] && vi[1] == vij[0]) { // It's a match + this->halfedge[i].twin = j; + this->halfedge[j].twin = i; + break; + } + } } + + if (this->halfedge[i].twin == std::numeric_limits::max()) { + // If still no match search from eb to sz + if (eb != sz && !wider) { wider = 1; } + for (uint32_t j = eb; j < sz; ++j) { + if (j == i) { continue; } + const sm::vec& vij = this->halfedge[j].vi; + if (vi[0] == vij[1] && vi[1] == vij[0]) { // It's a match + this->halfedge[i].twin = j; + this->halfedge[j].twin = i; + break; + } + } + } +#endif + wider_searches += wider; + + if (this->halfedge[i].twin != std::numeric_limits::max()) { + if (wider) { + twin_meandist += i > this->halfedge[i].twin ? i - this->halfedge[i].twin : this->halfedge[i].twin - i; + ++twins; + } + } // else halfedge[i] is an edge of the mesh + } + if constexpr (debug_nr) { + std::cout << "In " << sz << " halfedge searches, had to widen the search in " + << (100.0 * wider_searches) / sz << " percent\n"; + std::cout << "Mean wider twin search distance (in array elements) was " + << static_cast(twin_meandist) / twins << "\n"; } - return rtn; } - std::tuple, sm::vec> - first_triangle_containing (uint32_t _idx) const + // A subroutine for find_triangle_crossing + void test_tri (std::set& tested, const uint32_t ontest, + const sm::vec& vstart, const sm::vec& vdir, const float vdsos, + sm::vec& isect_p, uint32_t& isect_ti, float & isect_d) const { - for (auto t: this->triangles) { - auto [ti, tn, tnc, tnd] = t; - if (ti[0] == _idx || ti[1] == _idx || ti[2] == _idx) { - return {ti, tn}; + if (tested.count (ontest)) { return; } + tested.insert (ontest); + sm::vec, 3> v = this->triangle_vertices (ontest); + auto [isect, p] = sm::geometry::ray_tri_intersection (v[0], v[1], v[2], vstart, vdir); + if (isect) { + float d = (p - vstart).sos(); + if (d < vdsos) { + isect_p = p; + isect_ti = ontest; + isect_d = d; } } - return {}; } /* - * Find the location, and the triangle indices at which a ray starting from coord (scene - * frame) with direction vdir - the 'penetration point' intersects with this NavMesh - * model. The length of vdir is used to avoid finding the intersection at the 'back' of the - * model. + * Find the location, and the triangle indices at which a ray starting from coord with + * direction vdir - the 'penetration point' intersects with this NavMesh model. The length + * of vdir is used to avoid finding the intersection at the 'back' of the model. + * + * \param model_to_scene Transform that is only passed to find_vertex_normal. May in future be unnecessary. * * \param ti_ml The most likely triangle, if you know what it probably is, to reduce the * search time. * - * \return a tuple containing crossing location, triangle identity (three indices) and triangle normal vector + * \return a tuple containing crossing location, halfedge index (which specifies a triangle) */ - std::tuple, std::array, sm::vec> + std::tuple, uint32_t> find_triangle_crossing (const sm::vec& coord_mf, const sm::vec& vdir, - const std::array ti_ml = {std::numeric_limits::max()}) const + const sm::mat& model_to_scene, + const uint32_t ti_ml = std::numeric_limits::max() ) const { - constexpr auto umax = std::numeric_limits::max(); - constexpr auto fmax = std::numeric_limits::max(); + constexpr bool debug_ftc = false; + constexpr float fmax = std::numeric_limits::max(); sm::vec vstart = coord_mf - (vdir / 2.0f); // Return objects sm::vec isect_p = { fmax, fmax, fmax }; - std::array isect_ti = { umax, umax, umax, 0 }; - sm::vec isect_tn = { fmax, fmax, fmax }; + uint32_t isect_ti = std::numeric_limits::max(); - auto isect_d = std::numeric_limits::max(); // distance to intersect + float isect_d = std::numeric_limits::max(); // distance to intersect - const auto vdsos = vdir.sos(); + const float vdsos = vdir.sos(); + + std::set tested; // Have we been passed a 'most likely triangle' to test first? If so, test it. - if (ti_ml[0] != std::numeric_limits::max()) { - sm::vec v0 = this->vertex[ti_ml[0]]; - sm::vec v1 = this->vertex[ti_ml[1]]; - sm::vec v2 = this->vertex[ti_ml[2]]; - auto [isect, p] = sm::geometry::ray_tri_intersection (v0, v1, v2, vstart, vdir); - if (isect) { - float d = (p - vstart).sos(); - if (d < vdsos) { - sm::vec, 3> tverts = { v0, v1, v2 }; - isect_p = p; - isect_ti = ti_ml; - isect_tn = this->triangle_normal (tverts); // compute tn - isect_d = d; + if (ti_ml != std::numeric_limits::max()) { + + test_tri (tested, ti_ml, vstart, vdir, vdsos, isect_p, isect_ti, isect_d); + + if (isect_d != std::numeric_limits::max()) { + // we found it in the first triangle! + //std::cout << "Got a first-tri hit!\n"; + return { isect_p, isect_ti }; + } + + // Next, test the neighbours of ti_ml + std::vector nbs = this->find_neighbours (ti_ml); + //std::cout << "Testing " << nbs.size() << " neighbours of ti_ml for a hit\n"; + for (uint32_t nb : nbs) { + + test_tri (tested, nb, vstart, vdir, vdsos, isect_p, isect_ti, isect_d); + + if (isect_d != std::numeric_limits::max()) { + //std::cout << "Got a neighbour hit!\n"; + return { isect_p, isect_ti }; + } + } + + // Do neighbours of neighbours? + for (uint32_t nb : nbs) { + std::vector nbs2 = this->find_neighbours (nb); + + for (uint32_t nb2 : nbs2) { + test_tri (tested, nb2, vstart, vdir, vdsos, isect_p, isect_ti, isect_d); + + if (isect_d != std::numeric_limits::max()) { + std::cout << "Got a neighbour-neighbour hit!\n"; + return { isect_p, isect_ti }; + } } } - } - if (isect_d != std::numeric_limits::max()) { - // we found it already! - return { isect_p, isect_ti, isect_tn }; } + // Fall back to testing ALL the triangles... + //std::cout << "Oh, no have to test ALL triangles now...\n"; for (auto tri : this->triangles) { - auto [ti, tn, tnc, tnd] = tri; - auto [isect, p] = sm::geometry::ray_tri_intersection (this->vertex[ti[0]], this->vertex[ti[1]], this->vertex[ti[2]], vstart, vdir); + + if constexpr (debug_ftc) { + std::cout << "this->halfedge["<< 0 << "] " << (&this->halfedge[0]) << " contains: vi:" + << this->halfedge[0].vi + << ", twin:" << this->halfedge[0].twin + << ", next:" << this->halfedge[0].next + << ", prev:" << this->halfedge[0].prev << std::endl; + std::cout << "CF. Passing tri.he " << tri.hi << " to triangle_vertices(): with he->vi = " << this->halfedge[tri.hi].vi << std::endl; + } + + sm::vec, 3> v = this->triangle_vertices (tri.hi); + auto [isect, p] = sm::geometry::ray_tri_intersection (v[0], v[1], v[2], vstart, vdir); // What if the triangle is one on the *other side of the model*?? Have to use // vdir.sos() to exclude those that are too far and the distance^2 to find the // closest one that isn't. @@ -378,137 +727,57 @@ namespace mplot float d = (p - vstart).sos(); if (d < isect_d && d < vdsos) { isect_p = p; - isect_ti = ti; - isect_tn = tn; + isect_ti = tri.hi; isect_d = d; } } } - if (isect_p[0] == fmax && this->vertex.size() < 10000) { + if (isect_p[0] == fmax) { // Found no triangle intersection; check vertices, in case vdir points perfectly at a vertex. - // This can be computationally expensive, hence the hacky check, above. - for (uint32_t ti = 0; ti < this->vertex.size(); ++ti) { - sm::vec vertex_n = this->find_vertex_normal (ti); // also loops + std::cout << "Finally, check vertices...\n"; + for (uint32_t vi = 0; vi < this->vertex.size(); ++vi) { + sm::vec vertex_n = this->find_vertex_normal (this->vertex[vi].hi, model_to_scene); vertex_n.renormalize(); vstart = coord_mf + (vertex_n / 2.0f); - if (sm::geometry::ray_point_intersection (this->vertex[ti], vstart, -vertex_n)) { - float d = (this->vertex[ti] - vstart).sos(); + if (sm::geometry::ray_point_intersection (this->vertex[vi].p, vstart, -vertex_n)) { + float d = (this->vertex[vi].p - vstart).sos(); if (d < isect_d && d < vdir.sos()) { - std::cout << "Register vertex triangle_crossing\n"; - isect_p = this->vertex[ti]; - auto [_ti, _tn] = this->first_triangle_containing (ti); - isect_ti = _ti; - isect_tn = _tn; + if constexpr (debug_ftc) { std::cout << "Register vertex triangle_crossing\n"; } + isect_p = this->vertex[vi].p; + isect_ti = this->vertex[vi].hi; isect_d = d; } } } } - return { isect_p, isect_ti, isect_tn }; + return { isect_p, isect_ti }; } - // Find the location, and the triangle indices at which a ray between coord (in model frame) - // and the model centroid cross - the 'penetration point'. - std::tuple, std::array, sm::vec> - find_triangle_crossing (const sm::vec& coord_mf) const + // Find the location, and the triangle indices (by means of a halfedge index) at which a ray + // between coord (in model frame) and the model centroid cross - the 'penetration point'. + std::tuple, uint32_t> + find_triangle_crossing (const sm::vec& coord_mf, const sm::mat& model_to_scene) const { sm::vec vdir = this->bb.mid() - coord_mf; vdir.renormalize(); - return this->find_triangle_crossing (coord_mf, vdir); - } - - // Find a triangle containing indices a and b that isn't 'not_this' and return, along with its normal. - std::tuple, sm::vec> - find_other_triangle_containing (const uint32_t a, const uint32_t b, const std::array& not_this) const - { - constexpr uint32_t umax = std::numeric_limits::max(); - std::array other = {umax, umax, umax, 0}; - constexpr float fmax = std::numeric_limits::max(); - sm::vec other_n = {fmax, fmax, fmax}; - for (auto tri : triangles) { - auto [ti, tn, tnc, tnd] = tri; - if (ti[0] == not_this[0] && ti[1] == not_this[1] && ti[2] == not_this[2]) { - continue; - } - if ((ti[0] == a && (ti[1] == b || ti[2] == b)) - || (ti[1] == a && (ti[0] == b || ti[2] == b)) - || (ti[2] == a && (ti[0] == b || ti[1] == b))) { - other = ti; - other_n = tn; - break; - } - } - return {other, other_n}; - } - - // Find all the one-neighbours of 'of_this' - std::vector, sm::vec>> - find_one_neighbours (const std::array& of_this) const - { - std::vector, sm::vec>> rtn = {}; - auto a = of_this[0]; - auto b = of_this[1]; - auto c = of_this[2]; - for (auto tri : triangles) { - auto [ti, tn, tnc, tnd] = tri; - if ((ti[0] == a && ti[1] != b && ti[1] != c && ti[2] != b && ti[2] != c) - || (ti[1] == a && ti[2] != b && ti[2] != c && ti[0] != b && ti[0] != c) - || (ti[2] == a && ti[0] != b && ti[0] != c && ti[1] != b && ti[1] != c) - || - (ti[0] == b && ti[1] != c && ti[1] != a && ti[2] != c && ti[2] != a) - || (ti[1] == b && ti[2] != c && ti[2] != a && ti[0] != c && ti[0] != a) - || (ti[2] == b && ti[0] != c && ti[0] != a && ti[1] != c && ti[1] != a) - || - (ti[0] == c && ti[1] != a && ti[1] != b && ti[2] != a && ti[2] != b) - || (ti[1] == c && ti[2] != a && ti[2] != b && ti[0] != a && ti[0] != b) - || (ti[2] == c && ti[0] != a && ti[0] != b && ti[1] != a && ti[1] != b)) { - - rtn.push_back ({ti, tn}); - } - } - return rtn; + return this->find_triangle_crossing (coord_mf, vdir, model_to_scene); } - // Find all the neighbours of triangle vertex index a - std::vector, sm::vec>> - find_neighbours (const uint32_t a) const - { - std::vector, sm::vec>> rtn = {}; - for (auto tri : triangles) { - auto [ti, tn, tnc, tnd] = tri; - if (ti[0] == a || ti[1] == a || ti[2] == a) { rtn.push_back({ti, tn}); } - } - return rtn; - } - - sm::vec find_vertex_normal (const uint32_t ti) const + // Find the normal of the vertex specified by halfedge vhe + sm::vec find_vertex_normal (const uint32_t ti, const sm::mat& transform) const { auto neighbs = this->find_neighbours (ti); sm::vec vn = {}; if (neighbs.size() == 0) { return vn; } for (auto nb : neighbs) { - auto [ti, tn] = nb; - vn += tn; + // Turn nb, a half edge index, into a triangle? + vn += this->triangle_normal (this->triangle_vertices (nb, transform)); } return (vn / neighbs.size()); } - // Find the common vertex (ignoring a/b[3]) between a and b - uint32_t common_vertex (const std::array& a, const std::array& b) - { - uint32_t cv = std::numeric_limits::max(); - if (a[0] == b[0] || a[1] == b[0] || a[2] == b[0]) { - cv = b[0]; - } else if (a[0] == b[1] || a[1] == b[1] || a[2] == b[1]) { - cv = b[1]; - } else if (a[0] == b[2] || a[1] == b[2] || a[2] == b[2]) { - cv = b[2]; - } - return cv; - } - // Flags class enum class pm_fl : uint32_t { @@ -660,9 +929,8 @@ namespace mplot */ struct crossing_data { - // edge_idx_a/b are the indices of the triangle vertices on the crossed edge - uint32_t edge_idx_a = 0; - uint32_t edge_idx_b = 0; + // The crossed halfedge + uint32_t halfedge = std::numeric_limits::max(); // The crossed edge as a vector sm::vec tri_edge = {}; // The partial movement. pm.mv is the movement, pm.end is the crossing point @@ -678,128 +946,78 @@ namespace mplot * * All coordinates are in the frame of the model that contains this triangle. * - * \param t_verts *Ordered* vertices of the triangle. Vertices should be in anti-clockwise order - * when viewed with the triangle normal vector coming 'out of the page' + * \param t_verts *Ordered* vertices of the triangle. Vertices should be in anti-clockwise + * order when viewed with the triangle normal vector coming 'out of the page'. These should + * be the vertices that were generated with the param tri (using function + * triangle_vertices()). Could be obtained within this function, but have already been + * computed, and they may be in a different frame of ref that they have in this->vertex * - * \param t_indices The *Ordered* indices of the vertices in t_verts. Used to return the crossed - * edge specified as two common indices. See t_verts for correct order of triangle vertices. + * \param tri The halfedge that gives the triangle * * \param mv_s The start of the planned movement * * \param mv_inplane The planned movement - * - * \param t_norm The triangle normal. While this could be computed from t_verts, it has already - * been computed by the program and so I'm passing it in here. */ crossing_data compute_crossing_location (const sm::vec, 3>& t_verts, - const std::array& t_indices, + const uint32_t tri, const sm::vec& mv_s, - const sm::vec& mv_inplane, - const sm::vec& t_norm) + const sm::vec& mv_inplane) { - constexpr bool debug = false; + constexpr bool debug = true; crossing_data cd; cd.pm.flags.set (pm_fl::no_cross_point, true); - const sm::vec& t0 = t_verts[0]; - const sm::vec& t1 = t_verts[1]; - const sm::vec& t2 = t_verts[2]; - sm::vec p = mv_s + mv_inplane; - sm::vec edge = t1 - t0; - sm::vec ptoe = p - t0; - bool inside01 = (t_norm.dot (edge.cross (ptoe)) >= 0); - if (!inside01) { - partial_movement pm = find_edge_crossing (t0, t1, t_norm, mv_s, mv_inplane); - if constexpr (debug) { - if (pm.flags.test (pm_fl::colinear)) { - std::cout << "ccl: fec returned pm.colinear true for t0t1\n"; - } - } - if (pm.flags.test (pm_fl::no_cross_point) - && pm.flags.test (pm_fl::colinear) == false) { - inside01 = true; - if constexpr (debug) { - std::cout << "ccl: No intersection for edge t0t1 " << t0 << " -- " << t1 - << " and move " << mv_s << " -- " << (mv_s + mv_inplane) << std::endl; - } - } else { - if constexpr (debug) { - if (pm.flags.test (pm_fl::colinear)) { std::cout << "ccl: colinear t0t1\n"; } - std::cout << "ccl: Intersection for edge t0t1 " << t0 << " -- " << t1 - << " and move " << mv_s << " -- " << (mv_s + mv_inplane) << std::endl; - } - cd.pm = pm; - cd.tri_edge = edge; - cd.edge_idx_a = t_indices[0]; - cd.edge_idx_b = t_indices[1]; - } - } + sm::vec tn = this->triangle_normal (t_verts); - edge = t2 - t1; ptoe = p - t1; - bool inside21 = (t_norm.dot (edge.cross (ptoe)) >= 0); - if (!inside21) { - partial_movement pm = find_edge_crossing (t1, t2, t_norm, mv_s, mv_inplane); - if constexpr (debug) { - if (pm.flags.test (pm_fl::colinear)) { - std::cout << "ccl: fec returned pm.colinear true for t1t2\n"; - } - } - if (pm.flags.test (pm_fl::no_cross_point) - && pm.flags.test (pm_fl::colinear) == false) { - inside21 = true; - if constexpr (debug) { - std::cout << "ccl: No intersection for edge t1t2 " << t1 << " -- " << t2 - << " and move " << mv_s << " -- " << (mv_s + mv_inplane) << std::endl; - } - } else { - if constexpr (debug) { - if (pm.flags.test (pm_fl::colinear)) { std::cout << "ccl: colinear t1t2\n"; } - std::cout << "ccl: Intersection for edge t1t2 " << t1 << " -- " << t2 - << " and move " << mv_s << " -- " << (mv_s + mv_inplane) << std::endl; - } - cd.pm = pm; - cd.tri_edge = edge; - cd.edge_idx_a = t_indices[2]; - cd.edge_idx_b = t_indices[1]; - } - } + // do-while with tri + uint32_t hi = tri; + uint32_t a = 0; + sm::vec inside = { false, false, false }; + do { + uint32_t b = (a + 1u) % 3u; - edge = t0 - t2; ptoe = p - t2; - bool inside02 = (t_norm.dot (edge.cross (ptoe)) >= 0); - if (!inside02) { - partial_movement pm = find_edge_crossing (t2, t0, t_norm, mv_s, mv_inplane); - if constexpr (debug) { - if (pm.flags.test (pm_fl::colinear)) { - std::cout << "ccl: fec returned pm.colinear true for t2t0\n"; - } - } - if (pm.flags.test (pm_fl::no_cross_point) - && pm.flags.test (pm_fl::colinear) == false) { - inside02 = true; + sm::vec edge = t_verts[b] - t_verts[a]; + sm::vec ptoe = p - t_verts[a]; + + inside[a] = (tn.dot (edge.cross (ptoe)) >= 0); + if (!inside[a]) { + partial_movement pm = find_edge_crossing (t_verts[a], t_verts[b], tn, mv_s, mv_inplane); if constexpr (debug) { - std::cout << "ccl: No intersection for edge t2t0 " << t2 << " -- " << t0 - << " and move " << mv_s << " -- " << (mv_s + mv_inplane) << std::endl; + if (pm.flags.test (pm_fl::colinear)) { + std::cout << "ccl: fec returned pm.colinear true for t" << a << "t" << b << "\n"; + } } - } else { - if constexpr (debug) { - if (pm.flags.test (pm_fl::colinear)) { std::cout << "ccl: colinear t2t0\n"; } - std::cout << "ccl: Intersection for edge t2t0 " << t2 << " -- " << t0 - << " and move " << mv_s << " -- " << (mv_s + mv_inplane) << std::endl; + if (pm.flags.test (pm_fl::no_cross_point) + && pm.flags.test (pm_fl::colinear) == false) { + inside[a] = true; + if constexpr (debug) { + std::cout << "ccl: No intersection for edge t" << a << "t" << b << " " << t_verts[a] << " -- " << t_verts[b] + << " and move " << mv_s << " -- " << (mv_s + mv_inplane) << std::endl; + } + } else { + if constexpr (debug) { + if (pm.flags.test (pm_fl::colinear)) { std::cout << "ccl: colinear t0t1\n"; } + std::cout << "ccl: Intersection for edge t" << a << "t" << b << " " << t_verts[a] << " -- " << t_verts[b] + << " and move " << mv_s << " -- " << (mv_s + mv_inplane) << std::endl; + } + cd.pm = pm; + cd.tri_edge = edge; + cd.halfedge = hi; } - cd.pm = pm; - cd.tri_edge = edge; - cd.edge_idx_a = t_indices[0]; - cd.edge_idx_b = t_indices[2]; } - } + + ++a; + hi = this->halfedge[hi].next; + + } while (hi != tri && a < 3); + // We've now tested edge crossing for three edges in the triangle. - // if constexpr (debug) { if (cd.pm.flags.test (pm_fl::no_cross_point) == false) { - std::cout << "ccl: Crossed over" << (inside01 ? " " : " 0-1") - << (inside21 ? " " : " 2-1") << (inside02 ? " " : " 0-2") << std::endl; + std::cout << "ccl: Crossed over" << (inside[0] ? " " : " 0-1") + << (inside[1] ? " " : " 2-1") << (inside[2] ? " " : " 0-2") << std::endl; // could test pairs of inside01 etc to detect crossing a vertex } else if (cd.pm.flags.test (pm_fl::colinear) == true) { // Movement was colinear. Set Crossed vertex? @@ -812,8 +1030,8 @@ namespace mplot // cd.pm.no_cross_point will tell if there's a cross point or not } else { // We have NO crossing, which can occur for a variety of reasons - std::cout << "ccl: No crossings " << (inside01 ? " " : "!!0-1") - << (inside21 ? " " : "!!2-1") << (inside02 ? " " : "!!0-2") << std::endl; + std::cout << "ccl: No crossings " << (inside[0] ? " " : "!!0-1") + << (inside[1] ? " " : "!!2-1") << (inside[2] ? " " : "!!0-2") << std::endl; } } @@ -844,21 +1062,20 @@ namespace mplot * \param ti_ml The most likely triangle, if you know what it probably is, to reduce the * search time. * - * \return tuple containing: the hit point in scene coordinates; the triangle normal of the - * triangle we hit; and the indices of the triangle we hit. + * \return tuple containing: the hit point in scene coordinates; --the triangle normal of the + * triangle we hit;-- and the indices of the triangle we hit. */ - std::tuple, sm::vec, std::array> + std::tuple, uint32_t> find_triangle_hit (const sm::mat& model_to_scene, const sm::vec& camloc_mf, const sm::vec& vdir, - const std::array ti_ml = {std::numeric_limits::max()}) + uint32_t ti_ml = std::numeric_limits::max()) { - this->ti0 = {}; - this->tn0 = {}; + this->ti0 = std::numeric_limits::max(); sm::vec hit = {}; // Want to pass 'best tri' to this - std::tie (hit, this->ti0, this->tn0) = this->find_triangle_crossing (camloc_mf, vdir, ti_ml); + std::tie (hit, this->ti0) = this->find_triangle_crossing (camloc_mf, vdir, model_to_scene, ti_ml); - if (this->ti0[0] == std::numeric_limits::max()) { std::cout << __func__ << ": No hit\n"; } + if (this->ti0 == std::numeric_limits::max()) { std::cout << __func__ << ": No hit\n"; } sm::vec hp_scene = (model_to_scene * hit).less_one_dim(); @@ -867,18 +1084,18 @@ namespace mplot std::cout << "found hit at " << hit << " (model); " << hp_scene << " (scene) in direction " << vdir << "\n"; // Check we'll get a hit when we compute_mesh_movement: sm::vec, 3> tv_mf = this->triangle_vertices (this->ti0); - std::cout << "tn0: " << this->tn0 << ", length " << this->tn0.length() << std::endl; - std::cout << "TEST ray_tri_intersection (hit,-tn0): " << (hit + (this->tn0 / 2.0f)) << "," << -this->tn0 << std::endl; - auto [isect, hov_mf] = sm::geometry::ray_tri_intersection (tv_mf[0], tv_mf[1], tv_mf[2], hit + (this->tn0 / 2.0f), -this->tn0); + auto tn = this->triangle_normal (tv_mf); + std::cout << "tn: " << tn << ", length " << tn.length() << std::endl; + std::cout << "TEST ray_tri_intersection (hit,-tn): " << (hit + (tn / 2.0f)) << "," << -tn << std::endl; + auto [isect, hov_mf] = sm::geometry::ray_tri_intersection (tv_mf[0], tv_mf[1], tv_mf[2], hit + (tn / 2.0f), -tn); if (isect) { std::cout << "ray_tri_intersection confirms we would hit at " << hov_mf << "\n"; } else { std::cout << "ray_tri_intersection DOES NOT get a hit\n"; - //throw std::runtime_error ("ray_tri_intersection DOES NOT get a hit!"); } } - return { hp_scene, this->tn0, this->ti0 }; + return { hp_scene, this->ti0 }; } /*! @@ -899,10 +1116,9 @@ namespace mplot * large, flat, one-sided landscape, we want to make vdir long. search_dist_mult is applied * to vdir. * - * \return tuple containing: the hit point in scene coordinates; the triangle normal of the - * triangle we hit; and the indices of the triangle we hit. + * \return tuple containing: the hit point in scene coordinates and the mesh::face we hit. */ - std::tuple, sm::vec, std::array> + std::tuple, uint32_t> find_triangle_hit (const sm::mat& camspace, const sm::mat& model_to_scene, const float search_dist_mult = 1.0f) { @@ -920,15 +1136,16 @@ namespace mplot * _z and its 'x' axis in direction _x. */ sm::mat position_camera (const sm::vec& hp_scene, const sm::mat& model_to_scene, - const sm::vec& _x, const sm::vec& _z, + const sm::vec& _x, const sm::vec& _y, const sm::vec& _z, const float hoverheight) { // I think this positions correctly now (which is all it has to do). It ignores scaling // in model_to_scene. Can be reduced to use fewer mat<>s. sm::mat cam_mv_y; cam_mv_y.translate (sm::vec{0, hoverheight, 0}); - // The basis _x, tn0, _z, where these are vectors in the model frame that define a camera frame - sm::mat cam_to_model_rotn = sm::mat::frombasis (_x, this->tn0, _z); + + // The basis _x, _y, _z, where these are vectors in the model frame that define a camera frame + sm::mat cam_to_model_rotn = sm::mat::frombasis (_x, _y, _z); // Get the rotation from scene frame to model sm::mat m_to_sc_rotn = model_to_scene.rotation_mat44(); sm::mat hp_m; @@ -950,7 +1167,7 @@ namespace mplot const float hoverheight) { // Let's 'draw' the camera towards the model and then arrange its normal upwards wrt to the normal of the model. - if (this->tn0[0] == std::numeric_limits::max()) { + if (this->ti0 == std::numeric_limits::max()) { std::cout << __func__ << ": No hit/triangle normal\n"; return sm::mat{}; } @@ -960,11 +1177,13 @@ namespace mplot // and then set z from this random x and the triangle norm (y). sm::vec rand_vec; rand_vec.randomize(); - sm::vec _x = rand_vec.cross (this->tn0); + sm::vec, 3> tv_sf = this->triangle_vertices (this->ti0, model_to_scene); + sm::vec tn = this->triangle_normal (tv_sf); + sm::vec _x = rand_vec.cross (tn); _x.renormalize(); - sm::vec _z = _x.cross (this->tn0); + sm::vec _z = _x.cross (tn); - return this->position_camera (hp_scene, model_to_scene, _x, _z, hoverheight); + return this->position_camera (hp_scene, model_to_scene, _x, tn, _z, hoverheight); } /*! @@ -975,18 +1194,20 @@ namespace mplot const float hoverheight, const sm::vec& fwds) { // Let's 'draw' the camera towards the model and then arrange its normal upwards wrt to the normal of the model. - if (this->tn0[0] == std::numeric_limits::max()) { + if (this->ti0 == std::numeric_limits::max()) { std::cout << __func__ << ": No hit/triangle normal\n"; return sm::mat{}; } - // Project fwds onto the plane tn0 - sm::vec _z = sm::geometry::vector_plane_projection (tn0, fwds); + // Project fwds onto the plane tn + sm::vec, 3> tv_sf = this->triangle_vertices (this->ti0, model_to_scene); + sm::vec tn = this->triangle_normal (tv_sf); + sm::vec _z = sm::geometry::vector_plane_projection (tn, fwds); _z.renormalize(); - sm::vec _x = -_z.cross (this->tn0); + sm::vec _x = -_z.cross (tn); _x.renormalize(); - return this->position_camera (hp_scene, model_to_scene, _x, _z, hoverheight); + return this->position_camera (hp_scene, model_to_scene, _x, tn, _z, hoverheight); } /*! @@ -1007,28 +1228,29 @@ namespace mplot const sm::mat& model_to_scene, const float hoverheight) { - constexpr bool debug_move = false; + constexpr bool debug_move = true; constexpr bool debug_move2 = true; - // A data-containing exception to throw - mplot::NavException ne (mplot::NavException::type::generic); - ne.tris.push_back (this->ti0); + // In case we throw off-edge, we need to restore ti0's state + const uint32_t ti0_save = this->ti0; // Boolean state flags used in this function enum class cmm_fl : uint32_t { done, detected_crossing, single_movement, vertex_crossing }; sm::flags flags; // Camera location, scene frame - sm::vec camloc_sf = cam_to_scene.translation(); + const sm::vec camloc_sf = cam_to_scene.translation(); // Convert indices to vertices for triangle ti0, converting to the scene frame sm::vec, 3> tv_sf = this->triangle_vertices (this->ti0, model_to_scene); + if (tv_sf[0][0] == std::numeric_limits::max()) { throw std::runtime_error ("ti0 is not a triangle"); } + // Compute the triangle normal in the scene frame - this->tn0 = this->triangle_normal (tv_sf); + sm::vec tn0 = this->triangle_normal (tv_sf); if constexpr (debug_move) { std::cout << "\n# compute_mesh_movement:\n" - << "\nti0: " << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] - << "\nti0 (sf): " << tv_sf << "\nnormal " << this->tn0 + << "\nti0: " << this->ti0 + << "\nti0 (sf): " << tv_sf << "\nnormal " << tn0 << "\nmovement (camframe): " << mv_camframe << "\nInitial camera location (camloc_sf): " << camloc_sf << "\n\n"; } @@ -1043,11 +1265,12 @@ namespace mplot // boundaries and so requires that the start point is *within* the boundary. // if constexpr (debug_move) { - std::cout << "First ray_tri_intersection (raystart,-tn0): " << (camloc_sf + (this->tn0 / 2.0f)) << "," << -this->tn0 << std::endl; + std::cout << "First ray_tri_intersection (raystart,-tn0): " + << (camloc_sf + (tn0 / 2.0f)) << "," << -tn0 << std::endl; } bool isect = false; sm::vec hov_sf = {}; - std::tie (isect, hov_sf) = sm::geometry::ray_tri_intersection (tv_sf[0], tv_sf[1], tv_sf[2], camloc_sf + (this->tn0 / 2.0f), -this->tn0); + std::tie (isect, hov_sf) = sm::geometry::ray_tri_intersection (tv_sf[0], tv_sf[1], tv_sf[2], camloc_sf + (tn0 / 2.0f), -tn0); // Use the detected location, hov_sf to compute the surface location of the camera - its 'hover location' sm::mat cam_to_surface = cam_to_scene; @@ -1055,7 +1278,7 @@ namespace mplot // Try double precision if (isect == false) { - std::tie (isect, hov_sf) = sm::geometry::ray_tri_intersection (tv_sf[0], tv_sf[1], tv_sf[2], camloc_sf + (this->tn0 / 2.0f), -this->tn0); + std::tie (isect, hov_sf) = sm::geometry::ray_tri_intersection (tv_sf[0], tv_sf[1], tv_sf[2], camloc_sf + (tn0 / 2.0f), -tn0); if constexpr (debug_move) { if (isect == false) { std::cout << "No isect at start with ti0 using float OR double internally" << std::endl; @@ -1066,109 +1289,99 @@ namespace mplot } // If that didn't work, try the triangle *vertices* - uint32_t int_vertex = std::numeric_limits::max(); // intersection vertex + uint32_t int_vertex_hi = std::numeric_limits::max(); // intersection vertex if (isect == false) { if constexpr (debug_move) { std::cout << "Try the triangle vertices...\n"; } - for (uint32_t i = 0u; i < 3u; i++) { - + uint32_t hi = this->ti0; + uint32_t i = 0; + do { // We need to use the *vertex* normal for this test - the average of all the adjacent triangle normals! - sm::vec vertex_n = this->find_vertex_normal (this->ti0[i]); + sm::vec vertex_n = this->find_vertex_normal (hi, model_to_scene); vertex_n.renormalize(); - if constexpr (debug_move) { - std::cout << "Vertex normal for triangle index " << ti0[i] << " is " << vertex_n << std::endl; - } - + // How to figure out tv_sf[i]? + // sm::vec v_sf = (model_to_scane * this->vertex[halfedge[hi].vi[0]]).less_one_dim(); + // or, if tv_sf[0] is the start of ti0, then we can do this: if (sm::geometry::ray_point_intersection (tv_sf[i], camloc_sf + (vertex_n / 2.0f), -vertex_n)) { if constexpr (debug_move) { std::cout << "A VERTEX intersection is the start at " << tv_sf[i] << ", compare this with hov_sf = " << hov_sf << "\n"; - // if start is vertex, need to check movement across all the triangle-neighbours of this vertex (see later use of int_vertex) + // if start is vertex, need to check movement across all the triangle-neighbours of this vertex (see later use of int_vertex_hi) } hov_sf = tv_sf[i]; - int_vertex = i; + int_vertex_hi = hi; isect = true; } - } - } + ++i; + hi = this->halfedge[hi].next; - std::vector> trisearched; // the other triangles we search. To place in exception - if (isect == false) { + } while (hi != this->ti0); + } + if (isect == true) { + if constexpr (debug_move) { std::cout << "First ray_tri_intersected. Start of move is IN triangle ti0\n"; } + } else { if constexpr (debug_move2) { - std::cout << "No intersection (at start) with triangle ti0, so correct ti0 and tn0 (if we can)" << std::endl; + std::cout << "No intersection (at start) with triangle ti0, check neighbours (and maybe update ti0)" << std::endl; } // When very close to the boundary, ray_tri_intersection may fail. This triggers a // search for a neighbouring triangle which the camera may instead be hovering over // (this can occur when moving along an edge) - for (uint32_t i = 0u; i < 3u; i++) { - uint32_t i1 = i; - uint32_t i2 = (i + 1) % 3u; - auto [_ti, _tn] = this->find_other_triangle_containing (this->ti0[i1], this->ti0[i2], this->ti0); - if (_ti[0] != std::numeric_limits::max()) { - trisearched.push_back (_ti); - // Test to see if start location was inside a neighbour - sm::vec, 3> tv_lf = this->triangle_vertices (_ti, model_to_scene); - // _tn was returned in model frame coordinates, so recompute in scene frame - _tn = this->triangle_normal (tv_lf); - - auto [is, h] = sm::geometry::ray_tri_intersection (tv_lf[0], tv_lf[1], tv_lf[2], camloc_sf + (_tn / 2.0f), -_tn); - if constexpr (debug_move) { - std::cout << "Start of move " << (is ? "IS" : "is NOT") << " in " << _ti[0] << "," << _ti[1] << "," << _ti[2] << std::endl; + uint32_t hi = this->ti0; + do { + uint32_t twin = this->halfedge[hi].twin; + if (twin != std::numeric_limits::max()) { + // Test to see if start location was inside this twin + sm::vec, 3> tv_lf = this->triangle_vertices (twin, model_to_scene); + if (tv_lf[0][0] == std::numeric_limits::max()) { + throw std::runtime_error ("twin is not a triangle"); } + sm::vec _tn = this->triangle_normal (tv_lf); + auto [is, h] = sm::geometry::ray_tri_intersection (tv_lf[0], tv_lf[1], tv_lf[2], camloc_sf + (_tn / 2.0f), -_tn); + if constexpr (debug_move) { std::cout << "Start of move " << (is ? "IS" : "is NOT") << " in twin " << twin << std::endl; } if (is) { - if constexpr (debug_move) { std::cout << "CORRECT ti0 to " << _ti[0] << "," << _ti[1] << "," << _ti[2] << std::endl; } + if constexpr (debug_move) { std::cout << "CORRECT ti0 to the twin " << twin << std::endl; } // We're in this neighbour, so update ti0/tn0 and mark isect true - this->ti0 = _ti; + this->ti0 = twin; tv_sf = tv_lf; - this->tn0 = _tn; + tn0 = _tn; isect = true; // This requires a number of matrix recomputations: hov_sf = h; cam_to_surface = cam_to_scene; cam_to_surface.pretranslate (hov_sf - camloc_sf); // This is our init pose, placed on the surface - break; + break; // out of do-while } - } // else missing neighbour. Could see if it would land in a neighbour that's just off the edge? - } + } + hi = this->halfedge[hi].next; + } while (hi != this->ti0); if (isect == false) { - if constexpr (debug_move2) { - std::cout << "DBG No intersection (at start) with triangle ti0 OR neighbours" << std::endl; - } + if constexpr (debug_move2) { std::cout << "DBG No intersection (at start) with twins" << std::endl; } // Final test to see if we're on boundary? - float closest_edge_d = sm::geometry::dist_to_tri_edge (tv_sf[0], tv_sf[1], tv_sf[2], camloc_sf - (this->tn0 * hoverheight)); + float closest_edge_d = sm::geometry::dist_to_tri_edge (tv_sf[0], tv_sf[1], tv_sf[2], camloc_sf - (tn0 * hoverheight)); if constexpr (debug_move2) { - std::cout << "Closest distance from " << (camloc_sf - (this->tn0 * hoverheight)) - << " to ti0 edge: " << closest_edge_d << std::endl; + std::cout << "Closest distance from " << (camloc_sf - (tn0 * hoverheight)) << " to ti0 edge: " << closest_edge_d << std::endl; } constexpr float ced_thresh = std::numeric_limits::epsilon() * 50; if (closest_edge_d < ced_thresh) { // make tiny adjustment to camloc_sf so we ARE in the triangle? OR... isect = true; // SAY we are, and proceed? <-- this if it works. } else { - ne.m_type = NavException::type::no_intersection; - ne.tris.insert (ne.tris.end(), trisearched.begin(), trisearched.end()); - throw ne; + throw std::runtime_error ("No intersection (at start) with triangle or neighbours"); } } else { if constexpr (debug_move2) { - std::cout << "Found intersection (at start) with (2-)neighbour triangle " - << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] << std::endl; + std::cout << "Found intersection (at start) with twin triangle " << this->ti0 << std::endl; } } - - } else { - if constexpr (debug_move) { - std::cout << "First ray_tri_intersected. Start of move is IN triangle ti0\n"; - } } // rest of function assumes isect was true (exception otherwise) // Find component of movement that is in the current triangle plane (in the scene frame of reference) sm::vec mv_sf = (cam_to_scene * mv_camframe).less_one_dim() - camloc_sf; - sm::vec mv_orthog = this->tn0 * (mv_sf.dot (this->tn0) / (this->tn0.dot (this->tn0))); + sm::vec mv_orthog = tn0 * (mv_sf.dot (tn0) / (tn0.dot (tn0))); sm::vec mv_inplane = mv_sf - mv_orthog; // scene frame, a relative movement if (mv_inplane.length() == 0.0f) { @@ -1177,38 +1390,36 @@ namespace mplot } // New section to handle the case that we started right on a vertex - if (isect == true && int_vertex != std::numeric_limits::max()) { + if (isect == true && int_vertex_hi != std::numeric_limits::max()) { // We HAVE a vertex intersection. Check if we either cross, or land in one of this vertex's neighbours to correct our starting triangle and normal. - auto onens = this->find_neighbours (this->ti0[int_vertex]); - for (auto onen : onens) { - auto [_ti, _tn] = onen; - sm::vec _mv_orthog = _tn * (mv_sf.dot (_tn) / (_tn.dot (_tn))); - sm::vec _mv_inplane = mv_sf - _mv_orthog; // scene frame, a relative movement + auto onens = this->find_neighbours (int_vertex_hi); + for (auto _ti : onens) { sm::vec, 3> tv_nb = this->triangle_vertices (_ti, model_to_scene); - // _tn = this->triangle_normal (tv_nb); // shouldn't need to recompute - crossing_data cd = this->compute_crossing_location (tv_nb, _ti, hov_sf, _mv_inplane, _tn); + if (tv_nb[0][0] == std::numeric_limits::max()) { + throw std::runtime_error ("_ti is not a triangle"); + } + auto _tn = this->triangle_normal (tv_nb); // gets normal in *scene frame* + sm::vec _mv_orthog = _tn * (mv_sf.dot (_tn) / (_tn.dot (_tn))); // This tn needs to be in scene frame + sm::vec _mv_inplane = mv_sf - _mv_orthog; // scene frame, a relative movement + crossing_data cd = this->compute_crossing_location (tv_nb, _ti, hov_sf, _mv_inplane); if (cd.pm.flags.test (pm_fl::no_cross_point) == false) { this->ti0 = _ti; - this->tn0 = _tn; + tn0 = _tn; tv_sf = tv_nb; mv_orthog = _mv_orthog; mv_inplane = _mv_inplane; - if constexpr (debug_move) { - std::cout << "Break on cross point with triangle (" << _ti[0] << "," << _ti[1] << "," << _ti[2] << ")\n"; - } + if constexpr (debug_move) { std::cout << "Break on cross point with triangle (" << _ti << ")\n"; } break; } else { // No crossing, did we land in the triangle? auto [is, h] = sm::geometry::ray_tri_intersection (tv_nb[0], tv_nb[1], tv_nb[2], hov_sf + _mv_inplane + (_tn / 2.0f), -_tn); if (is) { // then we DID land in this neighbour tri this->ti0 = _ti; - this->tn0 = _tn; + tn0 = _tn; tv_sf = tv_nb; mv_orthog = _mv_orthog; mv_inplane = _mv_inplane; - if constexpr (debug_move) { - std::cout << "Break as we landed in triangle (" << _ti[0] << "," << _ti[1] << "," << _ti[2] << ")\n"; - } + if constexpr (debug_move) { std::cout << "Break as we landed IN triangle (" << _ti << ")\n"; } break; } } @@ -1220,143 +1431,151 @@ namespace mplot // a triangle edge had been crossed, because the original method // (compute_crossing_location, which uses a faster, but numerically fallible approach) // failed. - sm::vec detected_edge = {}; - sm::vec detected_edgevec = {}; - std::array detected_newtri = {}; // new triangle detected as part of a vertex crossing + uint32_t detected_edge = std::numeric_limits::max(); + //sm::vec detected_edgevec = {}; + uint32_t detected_newtri = std::numeric_limits::max(); // new triangle detected as part of a vertex crossing // Now loop while our path may traverse one or more triangles while (!flags.test (cmm_fl::done)) { if constexpr (debug_move) { std::cout << "\nWHILE LOOP\n" - << "ti0 = (" << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] << ")\n" + << "ti0 = (" << this->ti0 << ")\n" << "mv_inplane: " << hov_sf << "," << mv_inplane << "\n" - << "tn0 = " << this->tn0 << ")\n"; + << "tn0 = " << tn0 << ")\n"; } if (mv_inplane.length() == 0) { - ne.m_type = NavException::type::zero_mv; - throw ne; + throw std::runtime_error ("Zero length mv_inplane so stop/freeze/crash"); } if (mv_inplane.has_nan()) { - ne.m_type = NavException::type::nan_mv; - throw ne; + throw std::runtime_error ("mv_inplane contained NaN"); } // Apply the edge crossing algorithm - crossing_data cd = this->compute_crossing_location (tv_sf, this->ti0, hov_sf, mv_inplane, this->tn0); + crossing_data cd = this->compute_crossing_location (tv_sf, this->ti0, hov_sf, mv_inplane); if (cd.pm.flags.test (pm_fl::no_cross_point) == false || flags.test (cmm_fl::detected_crossing) || flags.test (cmm_fl::vertex_crossing)) { - // Then an edge (or vertex)crossing WAS detected (by compute_crossing_location or a prev. 'detected crossing') + // Then an edge (or vertex) crossing WAS detected (by compute_crossing_location or a prev. 'detected crossing') if (flags.test (cmm_fl::detected_crossing)) { - if constexpr (debug_move) { - std::cout << "This is a detected crossing; changing edge_idx_a/b to " << detected_edge << std::endl; - } + if constexpr (debug_move) { std::cout << "This is a detected crossing; changing crossing_data.halfedge to " << detected_edge << std::endl; } // We have to update our crossing data, as we detected a crossing over // an edge (probably while moving along that edge) - cd.edge_idx_a = detected_edge[0]; - cd.edge_idx_b = detected_edge[1]; - cd.tri_edge = detected_edgevec; + cd.halfedge = detected_edge; + cd.tri_edge = this->edge_vector (detected_edge, model_to_scene); + if constexpr (debug_move) { std::cout << "Obtained cd.tri_edge edge_vector (detected_edge=" << detected_edge << ", model_to_scene) = " + << cd.tri_edge << std::endl; } cd.pm.mv = mv_inplane; cd.pm.end = hov_sf + mv_inplane; } else { - if constexpr (debug_move) { - std::cout << "This IS a crossing (compute_crossing_location found it) " << std::endl; - } + if constexpr (debug_move) { std::cout << "This IS a crossing (compute_crossing_location found it) " << std::endl; } } // _ti, _tn are the new triangle sm::vec _tn = {}; - std::array _ti = {}; + uint32_t _ti = std::numeric_limits::max(); if (flags.test (cmm_fl::vertex_crossing)) { - if constexpr (debug_move) { - std::cout << "Setting _ti to over-the-vertex tri " - << detected_newtri[0] << "-" << detected_newtri[0] << "-" << detected_newtri[0] << std::endl; - } + if constexpr (debug_move) { std::cout << "Setting _ti to over-the-vertex tri " << detected_newtri << std::endl; } _ti = detected_newtri; } else { - // Can work out new triangle here across the crossed edge + // new triangle is the twin of the crossed edge + _ti = this->halfedge[cd.halfedge].twin; if constexpr (debug_move) { - std::cout << "find triangle across edge: find_other_triangle_containing (" - << cd.edge_idx_a << ", " << cd.edge_idx_b - << ", [" << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] << "])" << std::endl; + std::cout << "find triangle across edge: halfedge " << cd.halfedge << " twins to: " << _ti << std::endl; } - std::tie (_ti, _tn) = this->find_other_triangle_containing (cd.edge_idx_a, cd.edge_idx_b, this->ti0); } - if (_ti[0] != std::numeric_limits::max()) { - + if (_ti != std::numeric_limits::max()) { // Re-orient onto the new triangle sm::vec, 3> newtv_sf = this->triangle_vertices (_ti, model_to_scene); - _tn = this->triangle_normal (newtv_sf); + if (newtv_sf[0][0] == std::numeric_limits::max()) { + this->ti0 = ti0_save; + throw std::runtime_error ("off-edge: The movement went off the edge of the model"); + continue; + } else { - if constexpr (debug_move) { - std::cout << "RE-ORIENT to _ti: " << _ti[0] << "," << _ti[1] << "," << _ti[2] - << "\n " << newtv_sf << "\n normal " << _tn << "\n"; - } + // Check that _ti is a triangle, else we were on the boundary + // BUT newtv_sf[0][0] check should already have done this?!? - // If a vertex crossing, we have to make an edge that is the cross product of the two triangle normals - if (flags.test (cmm_fl::vertex_crossing)) { cd.tri_edge = this->tn0.cross (_tn); } - - // Compute the reorientation due to the requested movement. - float rotn_angle = 0.0f; - // Rotate by the angle between the normals (if stabilised is false). I think this is constrained to be <= pi - if (stabilised == false) { rotn_angle = this->tn0.angle (_tn, cd.tri_edge); } - // If tn0 and _tn are identical, then rotn_angle will be NaN, but in that case we want no rotation - if (std::isnan (rotn_angle)) { rotn_angle = 0.0f; } - sm::mat reorient_model; // reorientation transformation in sf - reorient_model.rotate (cd.tri_edge, rotn_angle); - sm::vec mv_rest = (reorient_model * (mv_inplane - cd.pm.mv)).less_one_dim(); - reorient_model.pretranslate (hov_sf + cd.pm.mv + mv_rest); - reorient_model.translate (-hov_sf); // r_t_to + r_t1 = -(hov_sf + cd.pm.mv) + cd.pm.mv = -hov_sf - - if (mv_rest.length() == 0) { - // The first movement to edge completed the movement. We actually landed ON the edge. - cam_to_surface = reorient_model * cam_to_surface; - flags.set (cmm_fl::done, true); - } else { - // There's additional movement to complete. - if constexpr (debug_move) { std::cout << "mv_rest length is " << mv_rest.length() << std::endl; } - - // At this point, can test to see if the end point of the movement - // lands in the adjacent triangle. If so, we're done, if not, time - // for another loop. - sm::vec endmv = (reorient_model * cam_to_surface * sm::vec{}).less_one_dim(); - // Is endmv in newtv_sf/_ti? - auto [isect2, isectpoint2] = sm::geometry::ray_tri_intersection (newtv_sf[0], newtv_sf[1], newtv_sf[2], - endmv + (_tn / 2.0f), -_tn); - if constexpr (debug_move) { - std::cout << "endmv = " << endmv << " DOES" << (isect2 ? "" : " NOT") << " land in new triangle\n"; + _tn = this->triangle_normal (newtv_sf); + + if constexpr (debug_move) { std::cout << "RE-ORIENT to _ti: " << _ti << " " << newtv_sf << " norm: " << _tn << "\n"; } + + // If a vertex crossing, we have to make an edge that is the cross product of the two triangle normals + if (flags.test (cmm_fl::vertex_crossing)) { + cd.tri_edge = tn0.cross (_tn); + if constexpr (debug_move) { std::cout << "Setting cd.tri_edge from tn0.cross (_tn) = " + << tn0 << ".cross (" << _tn << ") = " + << cd.tri_edge << std::endl; } + } + + // Compute the reorientation due to the requested movement. + float rotn_angle = tn0.angle (_tn, cd.tri_edge); + // If tn0 and _tn are identical, then rotn_angle will be NaN, but in that case we want no rotation + if (std::isnan (rotn_angle)) { rotn_angle = 0.0f; } + sm::mat reorient_model; // reorientation transformation in sf + // No good if cd.tri_edge is (0,0,0)! + reorient_model.rotate (cd.tri_edge, rotn_angle); + sm::vec mv_rest = (reorient_model * (mv_inplane - cd.pm.mv)).less_one_dim(); + if (std::isnan (mv_rest.length())) { + if constexpr (debug_move) { + std::cout << "Got NaN in mv_rest. mv_inplane: " << mv_inplane << ", cd.pm.mv: " << cd.pm.mv << "reorient_model:" << std::endl; + std::cout << "reorient_model created from tri_edge " << cd.tri_edge << " and rotn_angle " << rotn_angle << std::endl; + std::cout << reorient_model << std::endl; + } + throw std::runtime_error ("NaN in mv_rest"); } - if (isect2) { - // We DID land in the neighbouring triangle. We are done. + reorient_model.pretranslate (hov_sf + cd.pm.mv + mv_rest); + reorient_model.translate (-hov_sf); // r_t_to + r_t1 = -(hov_sf + cd.pm.mv) + cd.pm.mv = -hov_sf + + if (mv_rest.length() == 0) { + // The first movement to edge completed the movement. We actually landed ON the edge. cam_to_surface = reorient_model * cam_to_surface; flags.set (cmm_fl::done, true); } else { - if constexpr (debug_move) { std::cout << "did we sail past or land on the boundary or land in a 1-neighbour?\n"; } - // Incomplete; We've sailed past newtv_sf. Or perhaps landed on the boundary??? - // We need to - // set an end-point that is on newtv_sf, update hov_sf, - // then recurse. also recompute the movement encoded in - // reorient_model - reorient_model.pretranslate (-mv_rest); - cam_to_surface = reorient_model * cam_to_surface; - hov_sf = cd.pm.end; // crossing data planned movement end - // Also update planned move, which is now shorter and in a new direction - tv_sf = newtv_sf; - mv_inplane = mv_rest; + // There's additional movement to complete. + if constexpr (debug_move) { std::cout << "mv_rest length is " << mv_rest.length() << std::endl; } + + // At this point, can test to see if the end point of the movement + // lands in the adjacent triangle. If so, we're done, if not, time + // for another loop. + sm::vec endmv = (reorient_model * cam_to_surface * sm::vec{}).less_one_dim(); + // Is endmv in newtv_sf/_ti? + std::cout << "Crazy? endmv = " << endmv << std::endl; + std::cout << "ray_tri_intersection with _ti: " << (endmv + (_tn / 2.0f)) << "," << -_tn << std::endl; + auto [isect2, isectpoint2] = sm::geometry::ray_tri_intersection (newtv_sf[0], newtv_sf[1], newtv_sf[2], endmv + (_tn / 2.0f), -_tn); + if constexpr (debug_move) { std::cout << "endmv = " << endmv << " DOES" << (isect2 ? "" : " NOT") << " land in new triangle\n"; } + if (isect2) { + // We DID land in the neighbouring triangle. We are done. + cam_to_surface = reorient_model * cam_to_surface; + flags.set (cmm_fl::done, true); + } else { + if constexpr (debug_move) { std::cout << "did we sail past or land on the boundary or land in a 1-neighbour?\n"; } + // Incomplete; We've sailed past newtv_sf. Or perhaps landed on the boundary??? + + // Can now test if we went beyond the boundary. + + // We need to + // set an end-point that is on newtv_sf, update hov_sf, + // then recurse. also recompute the movement encoded in + // reorient_model + reorient_model.pretranslate (-mv_rest); + cam_to_surface = reorient_model * cam_to_surface; + hov_sf = cd.pm.end; // crossing data planned movement end + // Also update planned move, which is now shorter and in a new direction + tv_sf = newtv_sf; + mv_inplane = mv_rest; + } } - } - - this->ti0 = _ti; - ne.tris.push_back (this->ti0); - this->tn0 = _tn; + this->ti0 = _ti; + tn0 = _tn; + } } else { // other triangle not found?! We probably went off the edge of our navigation model mesh - ne.m_type = NavException::type::off_edge; - throw ne; + this->ti0 = ti0_save; + throw std::runtime_error ("off-edge: The movement went off the edge of the model"); continue; } @@ -1370,130 +1589,149 @@ namespace mplot if (cd.pm.flags.test (pm_fl::no_cross_point) == true) { flags.set (cmm_fl::single_movement, true); } else { // We've moved to a vertex, should have captured this case - ne.m_type = NavException::type::mv_to_vertex; - throw ne; + throw std::runtime_error ("We've moved to a vertex, should have captured this case"); } } else { // Test if it was movement-within; the simplest case if constexpr (debug_move) { - std::cout << "No cross point and not colinear.\n Testing if " - << (hov_sf + mv_inplane + (this->tn0 / 2.0f)) << "," << -this->tn0 + std::cout << "No cross point and not colinear.\n Testing if " << (hov_sf + mv_inplane + (tn0 / 2.0f)) << "," << -tn0 << " intersects tv_sf (" << tv_sf << "\n"; } - auto [single_mv, he] = sm::geometry::ray_tri_intersection (tv_sf[0], tv_sf[1], tv_sf[2], hov_sf + mv_inplane + (this->tn0 / 2.0f), -this->tn0); + auto [single_mv, he] = sm::geometry::ray_tri_intersection (tv_sf[0], tv_sf[1], tv_sf[2], hov_sf + mv_inplane + (tn0 / 2.0f), -tn0); flags.set (cmm_fl::single_movement, single_mv); } if (flags.test (cmm_fl::single_movement)) { - if constexpr (debug_move) { std::cout << "End of movement is *still* in ti0, so move mv_inplane/mv_camframe\n"; } // Perform simplest movement, which is just to translate by mv_inplane + if constexpr (debug_move) { std::cout << "End of movement is *still* in ti0, so move mv_inplane\n"; } cam_to_surface.pretranslate (mv_inplane); flags.set (cmm_fl::done, true); } else { - if constexpr (debug_move) { - std::cout << "End of movement is NOT in ti0 " << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] << ". Look for start neighbours\n"; - } + if constexpr (debug_move) { std::cout << "End of movement is NOT in ti0 " << this->ti0 << ". Look for start neighbours\n"; } - // Test 3 neighbours across the edges to find any for which the start location is also within-boundary + // Test neighbours, new scheme using halfedge data structures + // Test neighbours to find any for which the start location is also within-boundary flags.set (cmm_fl::detected_crossing, false); flags.set (cmm_fl::vertex_crossing, false); - std::array _ti_2n = { std::numeric_limits::max() }; - sm::vec_tn_2n = {}; - for (uint32_t i = 0u; i < 3u; i++) { - uint32_t i1 = i; - uint32_t i2 = (i + 1) % 3u; - auto [_ti, _tn] = this->find_other_triangle_containing (this->ti0[i1], this->ti0[i2], this->ti0); - if (_ti[0] != std::numeric_limits::max()) { - // Test to see if start location was inside a neighbour - sm::vec, 3> tv_nb = this->triangle_vertices (_ti, model_to_scene); - _tn = this->triangle_normal (tv_nb); - auto [is, h] = sm::geometry::ray_tri_intersection (tv_nb[0], tv_nb[1], tv_nb[2], hov_sf + (_tn / 2.0f), -_tn); - sm::vec mv_orthog_nb = _tn * (mv_inplane.dot (_tn) / (_tn.dot(_tn))); - sm::vec mv_inplane_nb = mv_inplane - mv_orthog_nb; - if constexpr (debug_move) { - std::cout << "endis? ray_tri_intersection with " << (hov_sf + mv_inplane_nb + (_tn / 2.0f)) << "," << -_tn << std::endl; - } - auto [endis, endh] = sm::geometry::ray_tri_intersection (tv_nb[0], tv_nb[1], tv_nb[2], hov_sf + mv_inplane_nb + (_tn / 2.0f), -_tn); - if constexpr (debug_move) { - std::cout << "Start of move " << (is ? "IS" : "is NOT") - << " in " << _ti[0] << "," << _ti[1] << "," << _ti[2] << " / " << tv_nb << std::endl; - std::cout << "End of move " << (endis ? "IS" : "is NOT") - << " in that triangle" << std::endl; - } + uint32_t _ti_2n = std::numeric_limits::max(); + sm::vec _tn_2n = {}; + sm::vec _tn = {}; - // Here, start is in original, end may not be in original. This - // is an 'intersection detected crossing' of a triangle edge - // which wasn't picked up with compute_crossing_location - if (endis) { - // End is in neighbour so this is a detected crossing - if constexpr (debug_move) { std::cout << "DETECTED crossing! Pass on to next loop!\n"; } - flags.set (cmm_fl::detected_crossing, true); - detected_edge = { this->ti0[i1], this->ti0[i2] }; - detected_edgevec = tv_nb[i2] - tv_nb[i1]; - break; // out of for - } else { // end not in neighbour - if (is) { // start is in neighbour tri (will re-orient to this and re-loop) - _ti_2n = _ti; - _tn_2n = _tn; + // TWO NEIGHBOURS + std::set neighbours_tested; + uint32_t hi = this->ti0; + do { + uint32_t twin = this->halfedge[hi].twin; + if (twin != std::numeric_limits::max()) { + // Test to see if start location was inside a neighbour + sm::vec, 3> tv_nb = this->triangle_vertices (twin, model_to_scene); + if (tv_nb[0][0] == std::numeric_limits::max()) { + // tv_nb is not a triangle + std::cout << "Are we off-edge?"; + throw std::runtime_error ("off-edge: Over a two-neighbour boundary"); + } else { + _tn = this->triangle_normal (tv_nb); + if constexpr (debug_move) { + std::cout << "TN: " << twin << ": START isect vector " << (hov_sf + (_tn / 2.0f)) << "," << -_tn << " with tri " << tv_nb; + } + auto [is, h] = sm::geometry::ray_tri_intersection (tv_nb[0], tv_nb[1], tv_nb[2], hov_sf + (_tn / 2.0f), -_tn); + sm::vec mv_orthog_nb = _tn * (mv_inplane.dot (_tn) / (_tn.dot(_tn))); + sm::vec mv_inplane_nb = mv_inplane - mv_orthog_nb; + if constexpr (debug_move) { + std::cout << "TN: " << twin << ": END isect vector " << (hov_sf + mv_inplane_nb + (_tn / 2.0f)) << "," << -_tn << " with tri " << tv_nb; + } + auto [endis, endh] = sm::geometry::ray_tri_intersection (tv_nb[0], tv_nb[1], tv_nb[2], hov_sf + mv_inplane_nb + (_tn / 2.0f), -_tn); + if constexpr (debug_move) { + std::cout << " Start IN? " << (is ? "Y" : "N") << "; End IN? " << (endis ? "Y" : "N") << std::endl; + } + + neighbours_tested.insert (twin); + + // Here, start is in original, end may not be in original. This is an 'intersection detected crossing' of a triangle edge + // which wasn't picked up with compute_crossing_location + if (endis) { + // End is in neighbour so this is a detected crossing + if constexpr (debug_move) { std::cout << "DETECTED crossing (end of movement intesects two-neighbour). set detected_crossing flag.\n"; } + flags.set (cmm_fl::detected_crossing, true); + detected_edge = hi; + hi = this->ti0; // to end the do-while break; // out of for - } // else end is not in neighbour, and neither is start. This - // occurs if the end is ON the boundary, but precision errors - // mean this location isn't 'in' either start or neighbour - // (according to ray_tri_intersection) + + } else { // end not in neighbour + if (is) { // start is in neighbour tri (will re-orient to this and re-loop) + _ti_2n = twin; + _tn_2n = _tn; + hi = this->ti0; // to end the do-while + break; // out of for + } // Neither end nor start are in neighbour. This occurs if the end is ON the boundary, but precision errors + // mean this location isn't 'in' either start or neighbour (according to ray_tri_intersection) + } } } - } + + hi = this->halfedge[hi].next; + + } while (hi != this->ti0); // Test one-neighbours here if necessary (that is, if the two neighbour test above failed) - if (flags.test (cmm_fl::detected_crossing) == false && - _ti_2n[0] == std::numeric_limits::max()) { - auto onens = this->find_one_neighbours (this->ti0); - for (auto onen : onens) { - // Are we in this one? - auto [_ti, _tn] = onen; - sm::vec, 3> tv_nb = this->triangle_vertices (_ti, model_to_scene); - _tn = this->triangle_normal (tv_nb); - auto [is, h] = sm::geometry::ray_tri_intersection (tv_nb[0], tv_nb[1], tv_nb[2], hov_sf + (_tn / 2.0f), -_tn); - sm::vec mv_orthog_nb = _tn * (mv_inplane.dot (_tn) / (_tn.dot(_tn))); - sm::vec mv_inplane_nb = mv_inplane - mv_orthog_nb; - if constexpr (debug_move) { - std::cout << "endis ONE-n? ray_tri_intersection with " << (hov_sf + mv_inplane_nb + (_tn / 2.0f)) << "," << -_tn << std::endl; - } - auto [endis, endh] = sm::geometry::ray_tri_intersection (tv_nb[0], tv_nb[1], tv_nb[2], hov_sf + mv_inplane_nb + (_tn / 2.0f), -_tn); - if constexpr (debug_move) { - std::cout << "Start of move " << (is ? "IS" : "is NOT") - << " in ONE-neighbour " << _ti[0] << "," << _ti[1] << "," << _ti[2] << " / " << tv_nb << std::endl; - std::cout << "And End of move " << (endis ? "IS" : "is NOT") - << " in that ONE-neighbour " << std::endl; + if (flags.test (cmm_fl::detected_crossing) == false && _ti_2n == std::numeric_limits::max()) { + uint32_t hi = this->ti0; + do { // For each half edge around ti0 + std::vector nbs = this->find_neighbours (hi); + std::cout << "Got " << nbs.size() << " neighbours\n"; + if (nbs.size() > 20) { throw std::runtime_error ("HOW many neighbours??"); } + for (auto _ti : nbs) { + // Already tested? This should avoid us testing any two-neighbours here (already did them above) + if (neighbours_tested.count (_ti)) { continue; } + neighbours_tested.insert (_ti); + + sm::vec, 3> tv_nb = this->triangle_vertices (_ti, model_to_scene); + if (tv_nb[0][0] != std::numeric_limits::max()) { + _tn = this->triangle_normal (tv_nb); + auto [is, h] = sm::geometry::ray_tri_intersection (tv_nb[0], tv_nb[1], tv_nb[2], hov_sf + (_tn / 2.0f), -_tn); + sm::vec mv_orthog_nb = _tn * (mv_inplane.dot (_tn) / (_tn.dot(_tn))); + sm::vec mv_inplane_nb = mv_inplane - mv_orthog_nb; + if constexpr (debug_move) { + std::cout << "endis ONE-n? ray_tri_intersection with " << (hov_sf + mv_inplane_nb + (_tn / 2.0f)) << "," << -_tn << std::endl; + } + auto [endis, endh] = sm::geometry::ray_tri_intersection (tv_nb[0], tv_nb[1], tv_nb[2], hov_sf + mv_inplane_nb + (_tn / 2.0f), -_tn); + if constexpr (debug_move) { + std::cout << "Start of move " << (is ? "IS" : "is NOT") << " in ONE-neighbour " << _ti << " / " << tv_nb << std::endl; + std::cout << "And End of move " << (endis ? "IS" : "is NOT") << " in that ONE-neighbour " << std::endl; + } + + if (endis) { + // End is in one-neighbour so this is a detected crossing + if constexpr (debug_move) { std::cout << "DETECTED crossing over ONE-neighbour! Pass on to next loop!\n"; } + flags.set (cmm_fl::vertex_crossing, true); + detected_edge = hi; + detected_newtri = _ti; + hi = this->ti0; // to end the do-while + break; // out of for + } else { // end not in one-neighbour + if (is) { // start is in one-neighbour tri (will re-orient to this and re-loop) + _ti_2n = _ti; + _tn_2n = _tn; + hi = this->ti0; // to end the do-while + break; // out of for + } // else end is not in one-neighbour, and neither is start. + } + } // else one-neighbour is not a triangle } + hi = this->halfedge[hi].next; - if (endis) { - // End is in one-neighbour so this is a detected crossing - if constexpr (debug_move) { std::cout << "DETECTED crossing over ONE-neighbour! Pass on to next loop!\n"; } - flags.set (cmm_fl::vertex_crossing, true); - detected_edge = { this->common_vertex (this->ti0, _ti), std::numeric_limits::max() }; - detected_edgevec = {}; // to be the cross product of the last-triangle normal and the newtri normal. - detected_newtri = _ti; - break; // out of for - } else { // end not in one-neighbour - if (is) { // start is in one-neighbour tri (will re-orient to this and re-loop) - _ti_2n = _ti; - _tn_2n = _tn; - break; // out of for - } // else end is not in one-neighbour, and neither is start. - } - } + } while (hi != this->ti0); } - if (_ti_2n[0] != std::numeric_limits::max()) { + if (_ti_2n != std::numeric_limits::max()) { // Now we know an alternative start triangle for the movement. Re-orient to this and re-loop + if constexpr (debug_move) { std::cout << "CHANGE ti0 to _ti_2n = " << _ti_2n << "\n"; } this->ti0 = _ti_2n; - ne.tris.push_back (this->ti0); - this->tn0 = _tn_2n; + tn0 = _tn; // recompute mv_inplane for this neighbour triangle - mv_orthog = this->tn0 * (mv_sf.dot (this->tn0) / (this->tn0.dot (this->tn0))); + mv_orthog = tn0 * (mv_sf.dot (tn0) / (tn0.dot (tn0))); mv_inplane = mv_sf - mv_orthog; // sf } else if (flags.test (cmm_fl::detected_crossing)) { // We didn't find an alternative start triangle, but we did detect an edge crossing by intersection, so continue. @@ -1513,7 +1751,7 @@ namespace mplot } // triangle traversing while loop // Raise cam_to_surface up by hoverheight and then return - cam_to_surface.pretranslate (hoverheight * this->tn0); + cam_to_surface.pretranslate (hoverheight * tn0); if constexpr (debug_move) { std::cout << "looping mv_inplanes completed. Final camloc_sf: " << cam_to_surface.translation() << std::endl; } diff --git a/mplot/NormalsVisual.h b/mplot/NormalsVisual.h index dfeb1a48..138554b5 100644 --- a/mplot/NormalsVisual.h +++ b/mplot/NormalsVisual.h @@ -6,11 +6,27 @@ #include #include +#include #include #include namespace mplot { + enum class normalsvisual_flags : uint32_t + { + show_gl_normals, // Show the OpenGL vertex normals? + show_tri_edges, // Show the OpenGLtriangle edge vectors? + show_tri_normals, // Show the OpenGL triangle-derived normal? + show_halfedges, // Show the navmesh halfedges (all of them)? + show_inner_halfedges, // Show the main, internal navmesh halfedges (the blue ones)? + show_boundary_halfedges, // Show the boundary navmesh halfedges (the red ones)? + show_inner_next, // Visualize the 'next' halfedge of each inner halfedge + show_inner_prev, // Visualize the 'prev' halfedge of each inner halfedge + show_boundary_next, // Visualize the 'next' halfedge of each boundary halfedge + show_boundary_prev, // Visualize the 'prev' halfedge of each boundary halfedge + singlecolour // Plot vertex normals in a single colour? + }; + //! A class to visualize normals for another model template class NormalsVisual : public VisualModel @@ -30,49 +46,135 @@ namespace mplot std::cout << "NormalsVisual: I have no model; returning\n"; return; } - + std::cout << "InitializeVertices\n"; const float cone_r = this->thickness * this->scale_factor * 2.0f; const float tube_r = this->thickness * this->scale_factor; // Copy data out of my model... std::vector mymodelPositions = mymodel->getVertexPositions(); std::vector mymodelNormals = mymodel->getVertexNormals(); std::vector mymodelColors = {}; - if (!singlecolour) { mymodelColors = mymodel->getVertexColors(); } + if (this->options.test (normalsvisual_flags::singlecolour) == false) { + mymodelColors = mymodel->getVertexColors(); + } // And interpret it auto vp = reinterpret_cast>*>(&mymodelPositions); auto vn = reinterpret_cast>*>(&mymodelNormals); auto vc = reinterpret_cast>*>(&mymodelColors); - for (uint32_t ii = 0; ii < vn->size(); ++ii) { - // (*vp)[ii] is position, (*vn)[ii] is normal - std::array _clr = clr; - if (!singlecolour) { _clr = (*vc)[ii]; } - this->computeArrow ((*vp)[ii], ((*vp)[ii] + (*vn)[ii] * this->scale_factor), - _clr, tube_r, this->arrowhead_prop, cone_r, this->shapesides); + if (this->options.test (normalsvisual_flags::show_gl_normals)) { + std::cout << "Showing " << vn->size() << " OpenGL vertex normals" << std::endl; + for (uint32_t ii = 0; ii < vn->size(); ++ii) { + // (*vp)[ii] is position, (*vn)[ii] is normal + std::array _clr = clr; + if (this->options.test (normalsvisual_flags::singlecolour) == false) { _clr = (*vc)[ii]; } + this->computeArrow ((*vp)[ii], ((*vp)[ii] + (*vn)[ii] * this->scale_factor), + _clr, tube_r, this->arrowhead_prop, cone_r, this->shapesides); + } } - - // If we also have the navmesh, then use its triangles to show face normals if (mymodel->navmesh) { - std::array ti = {}; sm::vec nv = {}; sm::vec nvc = {}; sm::vec nvd = {}; sm::vec pos = {}; - for (auto t : mymodel->navmesh->triangles) { - std::tie(ti, nv, nvc, nvd) = t; - // Plot tn at mean location of ti - pos = (mymodel->navmesh->vertex[ti[0]] + mymodel->navmesh->vertex[ti[1]] + mymodel->navmesh->vertex[ti[2]]) / 3.0f; - // Mesh triangle normals - this->computeArrow (pos, (pos + nv * this->scale_factor), - clr, tube_r, this->arrowhead_prop, cone_r, this->shapesides); - // Computed triangle normals - this->computeArrow (pos, (pos + nvc * this->scale_factor), - clrnc, tube_r, this->arrowhead_prop, cone_r, this->shapesides); - this->computeArrow (pos, (pos + nvd * scale_factor), - clrnd, tube_r, this->arrowhead_prop, cone_r, this->shapesides); + if (this->options.any_of ({normalsvisual_flags::show_tri_normals, normalsvisual_flags::show_tri_edges})) { + std::cout << "About to show normals and/or edges for " << mymodel->navmesh->triangles.size() << " triangles" << std::endl; + + for (auto t : mymodel->navmesh->triangles) { + + sm::vec, 3> vrts = mymodel->navmesh->triangle_vertices (t.hi); + // Plot normal/edge vectors at mean location of f + pos = vrts.mean(); + if (this->options.test (normalsvisual_flags::show_tri_normals)) { + nv = mymodel->navmesh->triangle_normal (vrts); + // Mesh triangle normals + this->computeArrow (pos, (pos + nv * this->scale_factor), + clr, tube_r, this->arrowhead_prop, cone_r, this->shapesides); + } + if (this->options.test (normalsvisual_flags::show_tri_edges)) { + // Computed triangle edges + nvc = vrts[1] - vrts[0]; + nvd = vrts[2] - vrts[0]; + this->computeArrow (pos, (pos + nvc * this->scale_factor), + clrnc, tube_r, this->arrowhead_prop, cone_r, this->shapesides); + this->computeArrow (pos, (pos + nvd * scale_factor), + clrnd, tube_r, this->arrowhead_prop, cone_r, this->shapesides); + } + } + } + + // Can also show halfedges from the navmesh + if (this->options.any_of ({normalsvisual_flags::show_halfedges, + normalsvisual_flags::show_inner_halfedges, + normalsvisual_flags::show_boundary_halfedges})) { + + if (this->options.test (normalsvisual_flags::show_halfedges) + || this->options.test ({normalsvisual_flags::show_inner_halfedges, normalsvisual_flags::show_boundary_halfedges})) { + std::cout << "About to show " << mymodel->navmesh->halfedge.size() << " halfedges from the model...\n"; + } else { + if (this->options.test (normalsvisual_flags::show_inner_halfedges)) { + std::cout << "Showing only inner halfedges (approx " << mymodel->navmesh->halfedge.size() << ")\n"; + } else if (this->options.test (normalsvisual_flags::show_inner_halfedges)) { + std::cout << "Showing only boundary halfedges (approx " << mymodel->navmesh->halfedge.size() << ")\n"; + } + } + + for (auto h : mymodel->navmesh->halfedge) { + + auto p0 = mymodel->navmesh->vertex[h.vi[0]].p; + auto p1 = mymodel->navmesh->vertex[h.vi[1]].p; + if ((h.flags & 0x2) == 0x2) { + // special/rogue + std::cout << "Showing a rogue!\n"; + if (this->options.any_of ({normalsvisual_flags::show_halfedges, normalsvisual_flags::show_boundary_halfedges, normalsvisual_flags::show_inner_halfedges})) { + + this->computeArrow (p0, p1, mplot::colour::yellow, + tube_r, this->arrowhead_prop, cone_r, this->shapesides); + } + } else if ((h.flags & 0x1) == 0x1) { + // boundary + if (this->options.any_of ({normalsvisual_flags::show_halfedges, normalsvisual_flags::show_boundary_halfedges})) { + + this->computeArrow (p0, p1, mplot::colour::crimson, + tube_r, this->arrowhead_prop, cone_r, this->shapesides); + } + + } else { + // internal + if (this->options.any_of ({normalsvisual_flags::show_halfedges, normalsvisual_flags::show_inner_halfedges})) { + this->computeArrow (p0, p1, mplot::colour::dodgerblue2, + tube_r, this->arrowhead_prop, cone_r, this->shapesides); + } + } + + if ((this->options.test (normalsvisual_flags::show_inner_next) && (h.flags & 0x1) == 0x0) + || + (this->options.test (normalsvisual_flags::show_boundary_next) && (h.flags & 0x1) == 0x1)) { + auto mid = (p0 + p1) / 2.0f + nextprev_offset; + auto nexti = h.next; + if (nexti < mymodel->navmesh->halfedge.size()) { + auto n0 = mymodel->navmesh->vertex[mymodel->navmesh->halfedge[nexti].vi[0]].p; + auto n1 = mymodel->navmesh->vertex[mymodel->navmesh->halfedge[nexti].vi[1]].p; + this->computeArrow (mid, (n0 + n1)/2.0f, mplot::colour::goldenrod2, + tube_r, this->arrowhead_prop, cone_r, this->shapesides); + } + + } + if ((this->options.test (normalsvisual_flags::show_inner_prev) && (h.flags & 0x1) == 0x0) + || + (this->options.test (normalsvisual_flags::show_boundary_prev) && (h.flags & 0x1) == 0x1)) { + auto mid = (p0 + p1) / 2.0f + nextprev_offset; + auto previ = h.prev; + if (previ < mymodel->navmesh->halfedge.size()) { + auto pr0 = mymodel->navmesh->vertex[mymodel->navmesh->halfedge[previ].vi[0]].p; + auto pr1 = mymodel->navmesh->vertex[mymodel->navmesh->halfedge[previ].vi[1]].p; + this->computeArrow (mid, (pr0 + pr1)/2.0f, mplot::colour::springgreen2, + tube_r, this->arrowhead_prop, cone_r, this->shapesides); + } + } + } } } }; @@ -87,11 +189,25 @@ namespace mplot float arrowhead_prop = 0.25f; // How much to linearly scale the size of the vector float scale_factor = 0.1f; + // Options for this VisualModel. Set these with calls like + // vm.options.set (mplot::normalsvisual_flags::show_gl_normals, false) + // from your client code + sm::flags options = options_defaults(); + constexpr sm::flags options_defaults() + { + sm::flags _options; + _options.reset(); + _options.set (normalsvisual_flags::show_gl_normals, true); + _options.set (normalsvisual_flags::show_tri_normals, true); + return _options; + } // Vector single colour - bool singlecolour = false; std::array clr = mplot::colour::grey20; std::array clrnc = mplot::colour::grey60; // computed norm std::array clrnd = mplot::colour::grey90; // computed norm + + // An offset to make the 'next' and 'prev' vectors meaningful. + sm::vec nextprev_offset = sm::vec::uz() * 0.1f; }; } // namespace mplot diff --git a/mplot/VisualModelBase.h b/mplot/VisualModelBase.h index 88cda242..0f6e2eb8 100644 --- a/mplot/VisualModelBase.h +++ b/mplot/VisualModelBase.h @@ -48,6 +48,7 @@ #include #include +#include #include namespace mplot @@ -220,6 +221,23 @@ namespace mplot this->indices.reserve (6u * n_vertices); } + // Make a hash of vertexPositions, etc as an identifier for this model. The hash identifies + // the model's mesh geometry for NavMesh and so the vertexColors are not important. + std::size_t hash() const + { + std::size_t h = 17; + for (std::size_t i = 0u; i < this->vertexPositions.size(); ++i) { + h = (h << 5) - 1 + std::hash{}(this->vertexPositions[i]); + } + for (std::size_t i = 0u; i < this->vertexNormals.size(); ++i) { + h = (h << 5) - 1 + std::hash{}(this->vertexNormals[i]); + } + for (std::size_t i = 0u; i < this->indices.size(); ++i) { + h = (h << 5) - 1 + std::hash{}(this->indices[i]); + } + return h; + } + // Get a single position from vertexPositions, using the index into the vector // interpretation of vertexPositions sm::vec get_position (const uint32_t vec_idx) const @@ -236,6 +254,16 @@ namespace mplot return (*vn)[vec_idx]; } + // Get the area of the triangle whose start index is vec_idx + float get_area (const uint32_t vec_idx0, const uint32_t vec_idx1, const uint32_t vec_idx2) const + { + auto vp = reinterpret_cast>*>(&this->vertexPositions); + auto t0 = (*vp)[vec_idx0]; + auto t1 = (*vp)[vec_idx1]; + auto t2 = (*vp)[vec_idx2]; + return sm::geometry::tri_area (t0, t1, t2); + } + /** * Neighbour vertex mesh code. */ @@ -243,29 +271,12 @@ namespace mplot // Our navigation mesh data struct std::unique_ptr navmesh; - /*! - * Post-process vertices to generate a neighbour relationship mesh. The usual vertices and - * indices may not be useful to help an agent to navigate the surface defined by the - * mesh. This is because vertices may be duplicated at any location, so that adjacent faces - * can have different normals and colours. - * - * To help guide movement across a mesh, it would be useful to have a mesh that always gives - * neighbour relationships. - */ - void make_navmesh() + void build_navmesh() { constexpr bool debug_mn = false; - if constexpr (debug_mn) { std::cout << "make_navmesh: Called" << std::endl; } + if constexpr (debug_mn) { std::cout << __func__ << " called" << std::endl; } - if (this->navmesh) { return; } // already made it - - if (this->flags.test (vm_bools::compute_bb) == false) { - throw std::runtime_error ("make_navmesh requires compute_bb flag to be true"); - } - this->update_bb(); - - // Create a new navmesh - this->navmesh = std::make_unique(); + if (!this->navmesh) { return; } // Copy the bounding box navmesh->bb = this->bb; @@ -281,108 +292,168 @@ namespace mplot for (auto e : equiv_v) { equiv[*e.second.begin()] = e.second; } if constexpr (debug_mn) { for (auto e : equiv) { - std::cout << "make_navmesh: equiv[" << e.first << "] = "; + std::cout << "build_navmesh: equiv[" << e.first << "] = "; for (auto idx : e.second) { std::cout << idx << ","; } std::cout << std::endl; } - std::cout << "make_navmesh: Populated equiv which has " - << equiv.size() << " vvecs" << std::endl; + std::cout << "build_navmesh: Populated equiv which has " << equiv.size() << " vvecs" << std::endl; } // Make inverse of equiv to translate from original (indices, vertexPositions) index to // new topographic mesh index sm::vvec navmesh_idx (vps, 0); - navmesh->vertexidx_to_indices.resize (equiv.size()); - uint32_t vcount = 0; i = 0; for (auto eqs : equiv) { vcount += eqs.second.size(); - navmesh->vertexidx_to_indices[i].resize (eqs.second.size()); - std::copy (eqs.second.begin(), eqs.second.end(), navmesh->vertexidx_to_indices[i].begin()); for (auto ev : eqs.second) { - if constexpr (debug_mn) { std::cout << "make_navmesh: set navmesh_idx[" << ev << "] = " << i << std::endl; } + if constexpr (debug_mn) { + std::cout << "build_navmesh: set navmesh_idx[" << ev << "] = " << i << std::endl; + } navmesh_idx[ev] = i; } ++i; } - if constexpr (debug_mn) { - std::cout << "make_navmesh: Created equiv inverse" << std::endl; - } + if constexpr (debug_mn) { std::cout << "build_navmesh: Created equiv inverse" << std::endl; } + if (vcount != vps) { - std::cout << "make_navmesh: WARNING: Vertex count from equiv is " << vcount + std::cout << "build_navmesh: WARNING: Vertex count from equiv is " << vcount << " which should (but does not) equal " << vps << std::endl; } // Can now populate vertex, a vector of coordinates, if required, or simply access (*vp) // as needed using equiv.first - navmesh->vertex.resize (equiv.size(), {0}); + navmesh->vertex.resize (equiv.size(), mesh::vertex{}); i = 0; - for (auto eq : equiv) { navmesh->vertex[i++] = (*vp)[eq.first]; } // FIXME? + for (auto eq : equiv) { + navmesh->vertex[i++] = { (*vp)[eq.first], std::numeric_limits::max() }; + } + + // We're turing a triangle mesh into a navmesh. Don't know what to do if there are stray vertices. + if (this->indices.size() % 3u != 0u) { + throw std::runtime_error ("Uh oh, indices size not divisible by 3!!!! Call the cops!"); + } // Lastly, generate edges. For which we require use of indices, which is expressed in // terms of the old indices. That lookup is navmesh_idx. for (uint32_t i = 0; i < this->indices.size(); i += 3) { - // Each three entries in indices is a triangle containing 3 edges. NB: Edges must be listed in ascending order! - std::array e = { navmesh_idx[indices[i]], navmesh_idx[indices[i+1]] }; - if (e[0] > e[1]) { - uint32_t t = e[0]; - e[0] = e[1]; - e[1] = t; - } - navmesh->edges.insert (e); - e = { navmesh_idx[indices[i]], navmesh_idx[indices[i+2]] }; - if (e[0] > e[1]) { - uint32_t t = e[0]; - e[0] = e[1]; - e[1] = t; - } - navmesh->edges.insert (e); + // Add three halfedges for the triangle + const uint32_t hesz = navmesh->halfedge.size(); + const uint32_t he0 = hesz; + const uint32_t he1 = hesz + 1; + const uint32_t he2 = hesz + 2; - e = { navmesh_idx[indices[i+1]], navmesh_idx[indices[i+2]] }; - if (e[0] > e[1]) { - uint32_t t = e[0]; - e[0] = e[1]; - e[1] = t; + if constexpr (debug_mn) { + std::cout << "setting halfedge["<< he0 << "] to { {" + << navmesh_idx[indices[i]] << ", " << navmesh_idx[indices[i + 1]] + << "}, nullptr, " << he1 << ", " << he2 << " }" << std::endl; + + std::cout << "setting halfedge[" << he1 << "] to { {" + << navmesh_idx[indices[i + 1]] << ", " << navmesh_idx[indices[i + 2]] + << "}, nullptr, " << he2 << ", " << he0 << " }" << std::endl; + + std::cout << "setting halfedge[" << he2 << "] to { {" + << navmesh_idx[indices[i + 2]] << ", " << navmesh_idx[indices[i]] + << "}, nullptr, " << he0 << ", " << he1 << " }" << std::endl; } - navmesh->edges.insert (e); - // Direct population of triangles. Three indices and a 4th number to hold flags (with bit0 meaning edge-triangle) - std::array t = { navmesh_idx[indices[i]], navmesh_idx[indices[i+1]], navmesh_idx[indices[i+2]], 0 }; + navmesh->halfedge.resize (hesz + 3, {}); + + // Now, could also try to identify LINES + navmesh->halfedge[he0] = { {navmesh_idx[indices[i ]], navmesh_idx[indices[i + 1]]}, std::numeric_limits::max(), he1, he2, 0u }; + navmesh->halfedge[he1] = { {navmesh_idx[indices[i + 1]], navmesh_idx[indices[i + 2]]}, std::numeric_limits::max(), he2, he0, 0u }; + navmesh->halfedge[he2] = { {navmesh_idx[indices[i + 2]], navmesh_idx[indices[i ]]}, std::numeric_limits::max(), he0, he1, 0u }; + + if constexpr (debug_mn) { + std::cout << "halfedge["<< hesz << "] contains: vi:" + << navmesh->halfedge[hesz].vi + << ", twin:" << navmesh->halfedge[hesz].twin + << ", next:" << navmesh->halfedge[hesz].next + << ", prev:" << navmesh->halfedge[hesz].prev << std::endl; + } + // A face contains just the first half edge index + mesh::face<> t = { he0 }; // The normal vector for this triangle could be obtained from the mesh normals, but // we can't trust them (though they're easy to get, as we're dealing with indices // already). However, use this to ensure that our triangle indices order is in // agreement with mesh normal as far as direction goes. - sm::vec trinorm = this->get_normal (indices[i]) + this->get_normal (indices[i+1]) + this->get_normal (indices[i+2]) ; - trinorm.renormalize(); + sm::vec tn = this->get_normal (indices[i]) + this->get_normal (indices[i + 1]) + this->get_normal (indices[i + 2]) ; + tn.renormalize(); // Compute trinorm as well and compare with the one from the mesh - perhaps it's // different? We really want the right normal. - const sm::vec& tv0 = navmesh->vertex[t[0]]; - const sm::vec& tv1 = navmesh->vertex[t[1]]; - const sm::vec& tv2 = navmesh->vertex[t[2]]; + const sm::vec& tv0 = navmesh->vertex[navmesh_idx[indices[i]]].p; + const sm::vec& tv1 = navmesh->vertex[navmesh_idx[indices[i + 1]]].p; + const sm::vec& tv2 = navmesh->vertex[navmesh_idx[indices[i + 2]]].p; sm::vec nx = (tv1 - tv0); sm::vec ny = (tv2 - tv0); sm::vec n = nx.cross (ny); n.renormalize(); - // Check rotational sense of triangles? - if (n.dot (trinorm) < 0.0f) { - // need to swap order in t: - uint32_t ti = t[2]; - t[2] = t[1]; - t[1] = ti; - n = -n; // Also reverse n + // Check rotational sense of triangles + if (n.dot (tn) < 0.0f) { + std::cout << "Swap order of triangle with he " << he0 << std::endl; + // Swap first and last half edge + navmesh->halfedge[he0].vi.rotate(); + navmesh->halfedge[he1].vi.rotate(); + navmesh->halfedge[he2].vi.rotate(); } + navmesh->triangles.push_back (t); + } + if constexpr (debug_mn) { + std::cout << "build_navmesh: Created triangles (" << navmesh->halfedge.size() << " halfedges)" << std::endl; + } + + navmesh->compute_neighbour_relations(); // finds the halfedge twins + } + + /*! + * Post-process vertices to generate a neighbour relationship mesh suitable for navigation. + * + * \param navmesh_dir The directory into which to store/read the navmesh data file. + */ + void make_navmesh (std::string navmesh_dir = "") + { + if (this->navmesh) { return; } // already made it - navmesh->triangles.push_back ({t, n, nx, ny}); // n is computed normal + if (this->flags.test (vm_bools::compute_bb) == false) { + throw std::runtime_error ("make_navmesh requires compute_bb flag to be true"); + } + this->update_bb(); + + // Create a new navmesh + this->navmesh = std::make_unique(); + + // Have we got a pre-computed navmesh file for the halfedge twin relationships? + uint64_t h = this->hash(); + if (navmesh_dir.empty()) { + navmesh_dir = mplot::tools::getTmpPath(); + } else { + if (navmesh_dir.back() != '/') { navmesh_dir += "/"; } } - if constexpr (debug_mn) { std::cout << "make_navmesh: Created triangles" << std::endl; } + std::string filename = navmesh_dir + std::string("navmesh_") + std::to_string (h); + std::string filename_pre_boundary = filename + ".pre"; + + if (mplot::tools::fileExists (filename)) { + this->navmesh->load (filename); + this->navmesh->test(); - //navmesh->mark_edge_triangles(); - //if constexpr (debug_mn) { std::cout << "make_navmesh: Marked edge triangles and done." << std::endl; } + } else if (mplot::tools::fileExists (filename_pre_boundary)) { + std::cout << "Pre-boundary navmesh\n"; + this->navmesh->load (filename_pre_boundary); + this->navmesh->add_boundary_halfedges(); + this->navmesh->test(); + this->navmesh->save (filename); + } else { + std::cout << "Building NavMesh to save into file " << filename << std::endl; + this->build_navmesh(); + this->navmesh->save (filename_pre_boundary); + this->navmesh->add_boundary_halfedges(); + this->navmesh->test(); + this->navmesh->save (filename); + } } /** @@ -1510,11 +1581,11 @@ namespace mplot size_t i0 = this->indices.size(); this->indices.resize (i0 + 6, 0); this->indices[i0++] = this->idx; - this->indices[i0++] = this->idx + 1; this->indices[i0++] = this->idx + 2; + this->indices[i0++] = this->idx + 1; this->indices[i0++] = this->idx; - this->indices[i0++] = this->idx + 2; this->indices[i0++] = this->idx + 3; + this->indices[i0++] = this->idx + 2; this->idx += 4; } diff --git a/mplot/tools.h b/mplot/tools.h index 8966c0b2..11a95e2c 100644 --- a/mplot/tools.h +++ b/mplot/tools.h @@ -399,6 +399,20 @@ namespace mplot return std::make_pair (fpath, fname); } + // Get the correct path to the temporary file store directory. /tmp/ on Unix systems. + std::string getTmpPath() + { +#ifdef _MSC_VER + char* userprofile = getenv ("USERPROFILE"); + std::string uppath(""); + if (userprofile != nullptr) { uppath = std::string (userprofile); } + std::string tmp = uppath + "\\AppData\\Local\\Temp\\"; +#else + std::string tmp = "/tmp/"; +#endif + return tmp; + } + /*! * Given a path like /path/to/file.ext or just file.ext in str, remove the file * suffix.