diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 8168f544..37493591 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -260,6 +260,9 @@ if(NOT APPLE) target_link_libraries(scatter_instanced OpenGL::GL glfw Freetype::Freetype) add_executable(breadcrumbs breadcrumbs.cpp) target_link_libraries(breadcrumbs OpenGL::GL glfw Freetype::Freetype) + + add_executable(model_crawler model_crawler.cpp) + target_link_libraries(model_crawler OpenGL::GL glfw Freetype::Freetype) endif() add_executable(scatter_dynamic scatter_dynamic.cpp) @@ -393,9 +396,6 @@ if(NOT APPLE) target_link_libraries(geodesic_ce OpenGL::GL glfw Freetype::Freetype) endif() -add_executable(model_crawler model_crawler.cpp) -target_link_libraries(model_crawler OpenGL::GL glfw Freetype::Freetype) - add_executable(tri tri.cpp) target_link_libraries(tri OpenGL::GL glfw Freetype::Freetype) diff --git a/examples/README.md b/examples/README.md index 107578fa..287716cf 100644 --- a/examples/README.md +++ b/examples/README.md @@ -1,15 +1,15 @@ # Example programs -This folder contains a set of example morphologica programs to help +This folder contains a set of example mathplot programs to help new users to get started using the library. These examples will build alongside the unit tests when you do a -morphologica build like this: +mathplot build like this: ```bash -# Clone morphologica if you didn't already -git clone git@github.com/ABRG-Models/morphologica -cd morphologica +# Clone mathplot if you didn't already +git clone git@github.com/sebsjames/mathplot --recurse-submodules +cd mathplot mkdir build cd build cmake .. @@ -18,153 +18,10 @@ cd .. ``` You'll find the example program binaries in `build/examples`. -The examples are almost all built on the morph::Visual environment, +The examples are almost all built on the mplot::Visual environment, which means you can interact with the mouse. Right-button down allows you to drag, Left-button down allows you to rotate. Press 't' to change the axis of rotations. Press 'h' and have a look at stdout to see some other key presses. 'x' exits. 'a' Resets the view. -## Computational and scientific model examples - -These are examples of models that we've re-implemented from the -literature. These examples make use of most of the basic facilities in -morphologic; morph::Config, morph::HdfData and morph::Visual. - -### convolve.cpp - -An example demonstrating the use of morph::HexGrid, its -HexGrid::convolve function, the morph::Random class and a -visualization of the input, the convolution kernel and the resulting -output. - -![Screenshot of the convolve program](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/convolve.png?raw=true) - -### logisticmap.cpp - -Computes the logistic map and displays with a morph::GraphVisual, using diamond shaped markers. - -![Screenshot of the logisticmap program](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/logisticmap.png?raw=true) - -### rosenbrock.cpp - -This program find the minimum of the Rosenbrock banana function using -the Nelder-Mead simplex optimization method (coded as the class -morph::NMSimplex). The walk of the simplex down the function surface -is animated. Note that the scaling of the colour map is set so that -only the lowest part of the surface is resolved in the green to blue -part of the map (which is Inferno) - -![Screenshot of Rosenbrock banana function with Nelder-Mead triangle](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/rosenbrock.png?raw=true) - -### Ermentrout2009 - -Contains an implementation of a Keller-Segel reaction diffusion system. - -**From the base of morphologica**, run like this (the program needs to access the file ./boundaries/whiskerbarrels.svg): - -```bash -/build/examples/Ermentrout2009/erm ./examples/Ermentrout2009/configs/erm.json -``` - -![Screenshot of the erm.cpp program, showing a bullseye mode](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/erm.png?raw=true) - -### LotkaVolterra - -Contains an implementation of the Lotka-Volterra population model cast as a reaction diffusion system. - -![Shows two surfaces for two population variables, u and v](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/lotkavolterra.png?raw=true) - -### SimpsonGoodhill - -Contains an implementation of the model described in the paper 'A -simple model can unify a broad range of phenomena in retinotectal map -development', Biological Cybernetics, 2011, by Hugh Simpson and Geoffrey Goodhill. This one is notable because it is an **agent based model**, rather than a reaction diffusion model. Proof that morphologica is useful for many different types of model! The image shows results for the wildtype model, Figs 2B to 2D in the paper, with parameters exactly as given in the paper. - -**From the base of morphologica**, run like this: - -```bash -/build/examples/SimpsonGoodhill/sg ./examples/SimpsonGoodhill/sg.json -``` -![Screenshot of the sg.cpp program, showing the wildtype model (cf. Figs 2B-2D)](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/sg.png?raw=true) - -## morph::Visual examples - -These simple examples showcase the features in morphologica's -morph::Visual code. They're a useful place to see what the code can do -for data visualization. - -### visual.cpp - -An example morph::Visual program which shows a morph::HexGrid and some -text labels. - -![Screenshot of a morph::Visual scene](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/visual.png?raw=true) - -### fps.cpp - -A dynamic HexGrid program showing an animated surface - -![Screenshot of the animated hexgrid program, fps.cpp](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/fps.png?raw=true) - -### hexgrid.cpp - -Example HexGridVisual - -![Screenshot of hexgrid.cpp showing a sinusoidal landscape in the jet colour map](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/hexgrid.png?raw=true) - -### hexgrid_image - -Creates a HexGrid, then uses `HexGrid::resampleImage` to load a picture. - -![Screenshot of hexgrid_image.cpp showing a resampled image of a touring bicycle](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/hexgrid_image.png?raw=true) - -### cartgrid.cpp - -Example Cartesian grid (CartGridVisual) - -![Screenshot of cartgrid.cpp showing a sinusoidal landscape in the jet colour map](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/cartgrid.png?raw=true) - -### quiver.cpp - -An example quiver plot using morph::QuiverVisual. - -![Screenshot of a 3D quiver plot](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/quiver.png?raw=true) - -### scatter.cpp - -An example three dimensional scatter plot of spheres using -morph::ScatterVisual. Note that in this example, the coordinate arrows -are set within the scene (and so move with the model). - -![Screenshot of a 3D scatter plot](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/scatter.png?raw=true) - -### graph1.cpp to graph4.cpp - -Various examples of the use of morph::GraphVisual. - -![Screenshot of graph3.cpp, showing some example graphs](https://github.com/ABRG-Models/morphologica/blob/main/examples/screenshots/graph3.png?raw=true) - -### twowindows.cpp - -An example to show how to create two morph::Visuals, and hence two -windows, in your program. - -### rods.cpp - -An example of the very simple VisualModel, morph::RodVisual, which -simply draws a polygonal rod. You can specify how many sides, so this -can be used to draw rods of square section, or rods which appear to be -cylindrical. - -### quads.cpp - -An example of morph::QuadsVisual. - -### pointrows.cpp - -An example of morph::PointRowsVisual, which is used to render a -surface made of adjacent rows of points. - -This program is also conditially compiled into the exectuable -pointrows_mesh, which renders the same points as a ball-and-stick -mesh. +There's also a [gallery of the example programs](https://sebsjames.github.io/mathplot/) on the reference site. \ No newline at end of file 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..0037a8bc 100644 --- a/examples/model_crawler.cpp +++ b/examples/model_crawler.cpp @@ -14,18 +14,22 @@ #include #include #include +#include + +#include +constexpr int32_t glver = mplot::gl::version_4_3; #include #include #include -#include +#include #include int main (int argc, char** argv) { int rtn = -1; - mplot::Visual v(1024, 768, "Crawling a surface with NavMesh features"); + mplot::Visual v(1024, 768, "Crawling a surface with NavMesh features"); v.rotateAboutNearest (true); // How big to make the sphere? @@ -46,33 +50,32 @@ int main (int argc, char** argv) sm::vec sphere_loc = {}; // A CoordArrows is our "crawling" agent - auto ca = std::make_unique> (arrows_loc); + auto ca = std::make_unique> (arrows_loc); v.bindmodel (ca); ca->finalize(); [[maybe_unused]] auto cap = v.addVisualModel (ca); - // A ScatterVisual will show the agent's trail - sm::vvec> sv_points; - sm::vvec sv_data; - auto sv = std::make_unique> (sphere_loc); - v.bindmodel (sv); - sv->setDataCoords (&sv_points); - sv->setScalarData (&sv_data); - sv->radiusFixed = 0.015f; - sv->cm.setType (mplot::ColourMapType::Plasma); - sv->colourScale.compute_scaling (0.0f, 1.0f); - sv->finalize(); - [[maybe_unused]] auto svp = v.addVisualModel (sv); // use svp->add (coord, value) + // Breadcrumb trail + uint64_t move_counter = 0u; + uint64_t max_bc = 1000; + sm::vvec> sv_points = {}; + sm::vvec sv_data = {}; + auto isv = std::make_unique> (sphere_loc); + v.bindmodel (isv); + isv->max_instances = max_bc; + isv->radiusFixed = 0.01f; + isv->finalize(); + auto isvp = v.addVisualModel (isv); // A sphere, approximated by an icosahedral geodesic, is our landscape mplot::ColourMap cm (mplot::ColourMapType::Jet); auto cl = cm.convert (0.5f); - auto gv = std::make_unique> (sphere_loc, radius); + auto gv = std::make_unique> (sphere_loc, radius); v.bindmodel (gv); gv->iterations = geo_itrns; std::string lbl = "GeodesicVisual with computed NavMesh"; gv->addLabel (lbl, {0, -(radius + 0.1f), 0}, mplot::TextFeatures (0.06f)); - gv->cm.setType (mplot::ColourMapType::Jet); + gv->cm.setType (mplot::ColourMapType::NaviaW); gv->colour_bb = cl; gv->finalize(); auto gvp = v.addVisualModel (gv); @@ -82,10 +85,15 @@ int main (int argc, char** argv) // Make the navmesh for the geodesic, this doesn't occur automatically and has to come after finalize() gvp->make_navmesh(); - // We're going to move the coordinate arrows forwards (along its z-axis), so that it 'orbits' - float move_step = 0.1f; // 0.075 <= move_step and iterations 6 to fail + // We're going to move the coordinate arrows forwards (along its z-axis) on each step + float move_step = 0.01f; sm::vec mv_ca = sm::vec::uz() * move_step; + // We'll also rotate by a small amount on each step, drawn from a Von Mises distribution + constexpr float mu = 0.0f; + constexpr float kappa = 3.0f; + sm::rand_vonmises rvm (mu, kappa); + // The viewmatrices have to be passed to mplot::NavMesh::compute_mesh_movement sm::mat ca_view = cap->getViewMatrix(); sm::mat sph_view = gvp->getViewMatrix(); @@ -93,25 +101,26 @@ 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; + + cap->setHide (true); - int move_counter = 0; - constexpr int move_max = 1000; while (!v.readyToFinish()) { + // Render the scene. Make sure this happens before first call to set_instance_data + v.render(); + // Wait .018 s and also poll for mouse/keyboard events - v.waitevents (0.018); + v.waitevents (0.002); // Compute a new movement over the landscape mesh (the sphere) try { + // rotate ca_view each time by a little (randomly) + ca_view.rotate (sm::vec<>::uy(), rvm.get()); 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 @@ -122,12 +131,15 @@ int main (int argc, char** argv) // We're adding and rebuilding the not-very-optimized ScatterVisual, so if move_max is too // high, the program will slow down (too many tiny spheres!) - if (move_counter++ < move_max) { - svp->add (arrows_loc, static_cast(move_counter) / move_max); + move_counter++; + // This should be the right place to update breadcrumbs + if (sv_points.size() < max_bc) { + sv_points.push_back (arrows_loc); + sv_data.push_back (0.0f); // dummy for now + } else { + sv_points[move_counter % max_bc] = arrows_loc; } - - // Re-render the scene - v.render(); + isvp->set_instance_data (sv_points); } v.keepOpen(); 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..ecc1c258 160000 --- a/maths +++ b/maths @@ -1 +1 @@ -Subproject commit db4ab32e0bc8b98fbd58ac4eb6289dae4cbf1247 +Subproject commit ecc1c2581272c51b102c9e3c6c668d3a82b671fa 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..5a103745 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,52 +73,110 @@ namespace mplot * Minimum set of vertices to generate a topological mesh. populated by * VisualModel::make_navmesh() */ - 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() - */ - std::set> edges; + std::vector> vertex = {}; - /*! - * Triangles too. Might be more useful than edges. Triangle given as indices into - * this->vertex. populated by VisualModel::make_navmesh() - */ - sm::vvec, sm::vec, sm::vec, sm::vec>> triangles; + //! The vector of half edges in the mesh + std::vector> halfedge = {}; - /*! - * Maps index in vertex to the original parent->indices index. populated by - * VisualModel::make_navmesh() - */ - sm::vvec> vertexidx_to_indices; + //! Triangle mesh faces. populated by VisualModel::make_navmesh() + 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' + * Save this navmesh into a binary file. */ - sm::vec tn0 = {}; + void save (const std::string& filename) const + { + std::cout << "Save NavMesh to " << filename << std::endl; + + 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); } + } /*! - * 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. + * Load the navmesh from the binary format produced by NavMesh::save() */ - bool stabilised = false; + 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 * to find the indices into vertexPositions and vertexNormals that this index in the * topographic mesh relates to. * - * \param scene_coord Supplied coordinate in scene frame of referencea + * \param scene_coord Supplied coordinate in scene frame of reference * \param viewmatrix The viewmatrix of the model which converts model frame coordinates to the scene frame */ uint32_t find_vertex_nearest (const sm::vec& scene_coord, const sm::mat& viewmatrix) const @@ -135,7 +185,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,27 +195,195 @@ namespace mplot return i; } - // 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 + /*! + * Performs the work required to verify a triangle or a boundary made of halfedges. + */ + 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; + } + + bool zlen_halfedge = false; + bool zlen_halfedge_to_halfedge = false; + bool approx_zlen_halfedge_to_halfedge = false; + // Each boundary should be the same forwards and backwards from every possible start halfedge + do { + // No self-referrals please + if (this->halfedge[fhi].next == fhi) { throw std::runtime_error ("self next referral!"); } + // Make sure that the vertices are not the same (no triangles-that-are-lines) + const uint32_t fhi_nx = this->halfedge[fhi].next; + // Check the length of the edge + auto hlen = (this->vertex[halfedge[fhi].vi[1]].p - this->vertex[halfedge[fhi_nx].vi[1]].p).length(); + if (halfedge[fhi].vi[0] == halfedge[fhi].vi[1]) { zlen_halfedge = true; } + if (halfedge[fhi].vi[1] == halfedge[fhi_nx].vi[1]) { zlen_halfedge_to_halfedge = true; } + if (hlen < (10.0f * std::numeric_limits::epsilon())) { + approx_zlen_halfedge_to_halfedge = true; + } + // Move to the next one + 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; + if (zlen_halfedge == true) { + std::cout << "test fails because we have zlen_halfedge\n"; + } + if (zlen_halfedge_to_halfedge == true) { + std::cout << "test fails because we have zlen_halfedge_to_halfedge\n"; + } + if (approx_zlen_halfedge_to_halfedge == true) { + std::cout << "test fails because we have at least one zero or ~zero length halfedge\n"; + } + } + + if (zlen_halfedge || zlen_halfedge_to_halfedge || approx_zlen_halfedge_to_halfedge) { + return false; + } + + // Check continuity in the reverse direction now + uint32_t rcount = 0u; + uint32_t rhi = _hi; + do { + 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; + } + } + /*! + * Verify a boundary that is not expected to be a triangle (its loop is likely to contain >3 + * halfedges) + */ + bool verify_boundary (const uint32_t boundary_hi, const bool debug = false) const + { + return this->verify_halfedge_chain (boundary_hi, std::numeric_limits::max(), debug); + } + + /*! + * Verify a halfedge loop that is expected to be a triangle with 3 halfedges + */ + bool verify_triangle (const uint32_t tri_hi, const bool debug = false) const + { + return this->verify_halfedge_chain (tri_hi, 3, debug); + } + + /*! + * Return the three indices in the triangle containing halfedge hi + * + * A single max value in the set indicates an error + */ + std::set triangle_indices (uint32_t 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]]; } + std::set indices; + if (hi == std::numeric_limits::max()) { + indices.insert (hi); + return indices; + } + for (uint32_t i = 0; i < 3; ++i) { + indices.insert (hi); + hi = this->halfedge[hi].next; + if (hi >= this->halfedge.size()) { break; } + } + return indices; + } + + /*! + * Return the three vertex coordinates, in the NavMesh model frame for the triangle of which + * the halfedge index tri_hi is a part. + */ + sm::vec, 3> triangle_vertices (uint32_t tri_hi) const + { + 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 + /*! + * Return the three vertex coordinates, transformed from the NavMesh model frame by + * 'transform' for the triangle of which the halfedge index tri_hi is a part. + */ + sm::vec, 3> triangle_vertices (const 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; } - // Compute the triangle normal for the ordered triplet of triangle vertices, tverts + /*! + * Compute the triangle normal for the ordered triplet of triangle vertices, tverts + */ sm::vec triangle_normal (const sm::vec, 3>& tverts) const { sm::vec n = (tverts[1] - tverts[0]).cross (tverts[2] - tverts[0]); @@ -173,349 +391,479 @@ 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 + { + 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; + } + + /*! + * Retrieve the coordinate of the start of the halfedge, transformed by the given transform + */ + sm::vec edge_start (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; + return (transform * this->vertex[this->halfedge[hi].vi[0]].p).less_one_dim(); } - // 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 the triangle *vertex* found at the start (position 0) of the + * halfedge 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, including all neighbour triangles AND self (a) + */ + 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 (const bool just_mark_bad = false) { - 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 << "NavMesh verification test running...\n"; + // 1) Verify that each halfedge is part of a face or hole (boundary) and is not a line? + for (uint32_t hi = 0; hi < this->halfedge.size(); ++hi) { + if ((this->halfedge[hi].flags & 0x1) == 0x1) { // 0x1 flag means 'on boundary' + if (this->verify_boundary (hi) == false) { + if (just_mark_bad == false) { + throw std::runtime_error ("Imperfect halfedge mesh (boundary hole)"); + } + } + } else { + if (this->verify_triangle (hi) == false) { + if (just_mark_bad == true) { + this->halfedge[hi].flags |= 0x2; // 0x2 means 'rogue halfedge' + } else { + throw std::runtime_error ("Imperfect halfedge mesh (face triangle)"); + } + } + + std::vector nb = this->test_neighbours (hi); + // Don't expect a very large number of neighbours + if (nb.size() > 100) { 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 + /*! + * During the construction of the halfedge mesh, 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 + << "]: " << this->vertex[this->halfedge[cur].vi[1]].p + << "," << (this->vertex[this->halfedge[cur].vi[0]].p - this->vertex[this->halfedge[cur].vi[1]].p) + << " with prev = " << bprev << " next = " << _bnext + << " and twin = " << cur << std::endl; } + uint32_t fl = 1; + if ((this->vertex[this->halfedge[cur].vi[0]].p - this->vertex[this->halfedge[cur].vi[1]].p).length() < 10.0f * std::numeric_limits::epsilon()) { + fl |= 2; + } + + this->halfedge.push_back ({{this->halfedge[cur].vi[1], this->halfedge[cur].vi[0]}, cur, _bnext, bprev, fl}); + 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! + if constexpr (debug_bnd) { 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? Answer: Preprocess the model in Blender (or similar) + } + } } - 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 a + * binary file that can be re-loaded at each 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 = false; + 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; + +#pragma omp parallel for + 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 + // The Simplest code would be a single loop + 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; + } + } +#endif + // But it's worth optimizing: + // 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; + } + } } + + 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 + */ + bool test_tri (std::set& tested, const uint32_t ontest, + const sm::vec& vstart, const sm::vec& vdir, + sm::vec& isect_p, uint32_t& isect_ti) 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 false; } + 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) { + isect_p = p; + isect_ti = ontest; } - return {}; + return isect; } - /* - * 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. + * + * \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 }; - - auto isect_d = std::numeric_limits::max(); // distance to intersect + uint32_t isect_ti = std::numeric_limits::max(); - const auto 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()) { + if (test_tri (tested, ti_ml, vstart, vdir, isect_p, isect_ti)) { + // we found it in the first triangle! + if constexpr (debug_ftc) { 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); + if constexpr (debug_ftc) { std::cout << "Testing " << nbs.size() << " neighbours of ti_ml for a hit\n"; } + for (uint32_t nb : nbs) { + if (test_tri (tested, nb, vstart, vdir, isect_p, isect_ti)) { + if constexpr (debug_ftc) { 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) { + if (test_tri (tested, nb2, vstart, vdir, isect_p, isect_ti)) { + 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... + if constexpr (debug_ftc) { 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); - // 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. + 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); if (isect) { - float d = (p - vstart).sos(); - if (d < isect_d && d < vdsos) { - isect_p = p; - isect_ti = ti; - isect_tn = tn; - isect_d = d; - } + isect_p = p; + isect_ti = tri.hi; + break; } } - 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 - 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 (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; - isect_d = d; + if constexpr (debug_ftc) { std::cout << "Finally, check vertices...\n"; } + + constexpr float dist_thresh = 2.0f * std::numeric_limits::epsilon(); + for (auto tri : this->triangles) { + sm::vec, 3> v = this->triangle_vertices (tri.hi); + for (uint32_t vi = 0; vi < 3; ++vi) { + if (sm::geometry::ray_point_intersection ((model_to_scene * v[vi]).less_one_dim(), vstart, vdir, dist_thresh)) { + isect_p = v[vi]; + isect_ti = tri.hi; + break; } } } } - 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; - } - - // 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; + return this->find_triangle_crossing (coord_mf, vdir, model_to_scene); } - sm::vec find_vertex_normal (const uint32_t ti) const + /*! + * Find the normal of the vertex specified by halfedge ti + */ + 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 + /*! + * Flags class for partial_movement + */ enum class pm_fl : uint32_t { - no_cross_point, // Means 'there was no crossing' - colinear // Means the movement was colinear with an edge + crossed, // Means the partial movement crossed an edge + colinear, // Means the movement was colinear with an edge + near_vertex_0, // The partial movement crossed very close to vertex 0 of the crossed edge + near_vertex_1 // The partial movement crossed very close to vertex 0 of the crossed edge }; - /* + + /*! * The partial movement that takes us to the crossing point, specified as movement + endpoint * (rather than startpoint + movement) */ @@ -525,11 +873,18 @@ namespace mplot sm::vec mv = {}; // The end coordinate of the movement sm::vec end = {}; + constexpr sm::flags default_flags() + { + sm::flags _flags; + _flags.reset(); + _flags.set (pm_fl::crossed, true); // assume crossed in new partial_movement + return _flags; + } // boolean state - sm::flags flags; + sm::flags flags = default_flags(); }; - /* + /*! * Find the part of mv_inplane that gets us to the triangle boundary defined by edge_s and * edge_e * @@ -616,12 +971,12 @@ namespace mplot if (to_v.length() <= mv_inplane3d.length()) { if constexpr (debug) { std::cout << "fec: partial colinear move to vertex\n"; } - pm.flags.set (pm_fl::no_cross_point, false); + pm.flags.set (pm_fl::crossed, true); pm.mv = (from_triangle_frame * to_v).less_one_dim(); // need to know if we were to go over a vertex pm.end = (from_triangle_frame * edge_e_3d).less_one_dim(); } else { if constexpr (debug) { std::cout << "fec: partial colinear along/within edge\n"; } - pm.flags.set (pm_fl::no_cross_point, true); + pm.flags.set (pm_fl::crossed, false); // Compute end from mv_inplane4d pm.mv = (from_triangle_frame * mv_inplane4d).less_one_dim(); pm.end = (from_triangle_frame * (h_4d + mv_inplane4d)).less_one_dim(); @@ -634,6 +989,27 @@ namespace mplot // Now go from cross point 2d to a point in model coordinates? pm.end = (from_triangle_frame * cp2d.plus_one_dim(edge_s_4d[2])).less_one_dim(); if constexpr (debug) { std::cout << "fec: Cross point in mdl frame: " << pm.end << std::endl; } + + // Check if cross point is close to a vertex + sm::vec e1 = cp2d - edge_s_2d; + float d2_e1 = e1.length(); + if (d2_e1 < 100.0f * std::numeric_limits::epsilon()) { + if constexpr (debug) { + std::cout << "Set near_vertex flag due to end 0 (within " << 100.0f * std::numeric_limits::epsilon() << ")\n"; + } + pm.flags.set (pm_fl::near_vertex_0); + } + e1 += edge_2d; + float d2_e2 = e1.length(); + if (d2_e2 < 100.0f * std::numeric_limits::epsilon()) { + if constexpr (debug) { std::cout << "Set near_vertex flag due to end 1\n"; } + pm.flags.set (pm_fl::near_vertex_1); + } + + if constexpr (debug) { + std::cout << "fec: Distance to edge end 1: " << d2_e1 << ", and to end 2: " << d2_e2 << std::endl; + } + pm.mv = pm.end - mv_s; } else { @@ -645,7 +1021,7 @@ namespace mplot << h_2d << " -- " << (h_2d + mv_inplane2d) << std::endl; } // Mark that there was no intersection - pm.flags.set (pm_fl::no_cross_point, true); + pm.flags.set (pm_fl::crossed, false); pm.mv = sm::vec{}; pm.end = mv_s; } @@ -654,22 +1030,25 @@ namespace mplot return pm; } - /* + /*! * After testing up to all three edges of a triangle, we return information about the crossing * location and the indices of the triangle that form the crossed edge in this structure. */ 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 crossed = std::numeric_limits::max(); + // A halfedge in the triangle that we cross into. Usually set to this->halfedge[halfedge].twin + uint32_t into = 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 + // The remaining movement after the crossing, with direction rotated about tri_edge. max means unset + sm::vec mv_rest = { std::numeric_limits::max() }; + // The partial movement. pm.mv is the movement up to the crossing point, pm.end is the crossing point partial_movement pm = {}; }; - /* + /*! * Find the location at which a movement from mv_s in the direction mv_inplane crosses one of * the edges of the triangle specified by the three vertices in t_verts/t_indices. * @@ -678,142 +1057,99 @@ 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; 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]; + cd.pm.flags.set (pm_fl::crossed, false); 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"; + } + } + if (pm.flags.test (pm_fl::crossed) == false && 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] - t_verts[a]) + << " and move " << mv_s << "," << mv_inplane << std::endl; + } + } else { + if constexpr (debug) { + if (pm.flags.test (pm_fl::colinear)) { std::cout << "ccl: colinear t" << a << "t" << b << "\n"; } + if (pm.flags.test (pm_fl::crossed) == false) { std::cout << "ccl: no cross point t" << a << "t" << b << "\n"; } + std::cout << "ccl: Intersection for edge t" << a << "t" << b << " " << t_verts[a] << "," << (t_verts[b] - t_verts[a]) + << " and move " << mv_s << "," << mv_inplane << std::endl; + } + cd.pm = pm; + cd.tri_edge = edge; + cd.crossed = hi; } } 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; + std::cout << "inside[" << a << "] is true for edge t" << a << "t" << b << " " + << t_verts[a] << "," << (t_verts[b] - t_verts[a]) << "\n"; } - 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; + if (cd.pm.flags.test (pm_fl::crossed) == true) { + std::cout << "ccl: Crossed over" << (inside[0] ? " " : " 0-1") + << (inside[1] ? " " : " 2-1") << (inside[2] ? " " : " 0-2"); + if (cd.pm.flags.any_of ({pm_fl::near_vertex_0, pm_fl::near_vertex_1}) == true) { std::cout << " near a vertex"; } + std::cout << 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? std::cout << "ccl: movement was colinear!\n"; - if (cd.pm.flags.test (pm_fl::no_cross_point)) { + if (cd.pm.flags.test (pm_fl::crossed) == false) { std::cout << "ccl: Colinear along edge" << std::endl; } else { std::cout << "ccl: Colinear to vertex" << std::endl; } - // cd.pm.no_cross_point will tell if there's a cross point or not + // cd.pm.crossed 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; } } @@ -831,34 +1167,25 @@ namespace mplot * \param camloc_mf The camera location in the model frame. This gives us the start location * for the ray. * - * \param vdir The direction of the ray. - * - * \param search_dist_mult a multiplier on the search distance. The length of vdir in this - * function should cross the landscape model. By default it's the vector from the camera - * location in the model frame of reference to the middle of the bounding box. If the vector - * is too long when finding the surface of a convex hull, such as a model of a rock, it is - * possible to mis-identify the back side of the model. However, for finding a location on a - * large, flat, one-sided landscape, we want to make vdir long. search_dist_mult is applied - * to vdir. + * \param vdir The direction of the ray (its length is also significant). * * \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 index of the triangle + * we hit (also set into this->ti0). */ - 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 +1194,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 +1226,10 @@ 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 halfedge referring to + * the triangle 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) { @@ -916,19 +1243,23 @@ namespace mplot } /*! - * Position the camera hoverheight above the location hp_scene, with its forward direction - * _z and its 'x' axis in direction _x. + * Position the camera a distance hoverheight above the location hp_scene, with its forward + * direction _z and its 'x' axis in direction _x. _x, _y and _z are used to form a basis + * that creates a coordinate transform matrix. + * + * \return the transformation matrix that positions the camera */ 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 +1281,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 +1291,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,87 +1308,73 @@ 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); } /*! - * Compute a movement over this navigation mesh. + * Return true: intersection found; false: no intersection found. Also returns outputs in + * the args hov_sf and cam_to_surface. * - * We convert the triangle vertices from the model frame to the scene frame before computing - * reorientations, so that non-uniform scalings in the model do not fox us. + * \param tv_sf triangle vertices as coordinates. Input, may be modified? * - * \param mv_camframe A movement vector in the camera's own frame of reference (an ego-motion) - * \param cam_to_scene The transformation matrix to bring the camera coordinates to the scene frame - * \param model_to_scene The transformation matrix to convert model coordinates to the scene frame - * \param hoverheight + * \param tn0 Triangle normal of tv_sf. In/out? * - * \return The re-positioned camera transform matrix + * \param hov_sf An output. Hit location on triangle + * + * \param cam_to_surface An output. pose matrix for hov_sf (is hov_sf in the end just cam_to_surface.translation()?) + * + * \param cam_to_scene Transforms camera's ego-frame to scene coordinate frame + * + * \param model_to_scene Transforms landscape/model space to scene coordinate frame + * + * \param hoverheight Camera height-above-model-surface */ - sm::mat compute_mesh_movement (const sm::vec& mv_camframe, - const sm::mat& cam_to_scene, - const sm::mat& model_to_scene, - const float hoverheight) + bool find_first_intersection (sm::vec, 3>& tv_sf, + sm::vec& tn0, + sm::vec& hov_sf, + sm::mat& cam_to_surface, + const sm::mat& cam_to_scene, + const sm::mat& model_to_scene, + const float hoverheight) { constexpr bool debug_move = false; - constexpr bool debug_move2 = true; - - // A data-containing exception to throw - mplot::NavException ne (mplot::NavException::type::generic); - ne.tris.push_back (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(); - // 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); - // Compute the triangle normal in the scene frame - this->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 - << "\nmovement (camframe): " << mv_camframe - << "\nInitial camera location (camloc_sf): " << camloc_sf << "\n\n"; - } + const sm::vec camloc_sf = cam_to_scene.translation(); - // Does camloc_sf in dirn tn0 intersect the tv_sf triangle? This - // returns true if camloc_sf is on the edge of the triangle or on a - // vertex. Assumes we're above the model and within the length of tn0 of the - // surface. + // Does camloc_sf in dirn tn0 intersect the tv_sf triangle? This returns true if + // camloc_sf is on the edge of the triangle or on a vertex. Assumes we're above the + // model and within the length of tn0 of the surface. // // IF we're on an edge, then this intersection algo may disagree with // compute_crossing_location, which currently looks for crossing each of the three - // boundaries and so requires that the start point is *within* the boundary. - // + // boundaries and so expects 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; + cam_to_surface = cam_to_scene; cam_to_surface.pretranslate (hov_sf - camloc_sf); // This is now our init pose; the camera is now at the surface // 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,454 +1385,579 @@ namespace mplot } // If that didn't work, try the triangle *vertices* - uint32_t int_vertex = 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; - } - 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 } hov_sf = tv_sf[i]; - int_vertex = i; 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 constexpr (debug_move2) { - std::cout << "No intersection (at start) with triangle ti0, so correct ti0 and tn0 (if we can)" << std::endl; + 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_move) { + 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()) { + // This probably means we've attempted to go over a boundary. client code should turn around + throw std::runtime_error ("off-edge: 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 << ", " << tv_lf << 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? - } - - if (isect == false) { - if constexpr (debug_move2) { - std::cout << "DBG No intersection (at start) with triangle ti0 OR neighbours" << std::endl; } + hi = this->halfedge[hi].next; + } while (hi != this->ti0); + if (isect == false) { + if constexpr (debug_move) { 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)); - if constexpr (debug_move2) { - std::cout << "Closest distance from " << (camloc_sf - (this->tn0 * hoverheight)) - << " to ti0 edge: " << closest_edge_d << std::endl; + 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_move) { + 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; + constexpr float ced_thresh = std::numeric_limits::epsilon() * 50; // FIX this 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; + // If we still have no intersection, throw an exception + 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; + if constexpr (debug_move) { + std::cout << "Found intersection (at start) with twin triangle " << this->ti0 << std::endl; } } + } + + return isect; + } + + /*! + * For the two triangles t0 and t1, find if t0 has a twin in t1 and return that halfedge. + */ + uint32_t test_twin (const uint32_t t0, const uint32_t t1) + { + uint32_t rtn = std::numeric_limits::max(); + uint32_t hi0 = t0; + do { + uint32_t hi1 = t1; + do { + if (this->halfedge[hi1].twin == hi0) { rtn = hi0; } + hi1 = this->halfedge[hi1].next; + } while (hi1 != t1 && rtn == std::numeric_limits::max()); + hi0 = this->halfedge[hi0].next; + } while (hi0 != t0 && rtn == std::numeric_limits::max()); + return rtn; + } + + /*! + * Find any vertex in t0 that shares a vertex with t1. Return the halfedge in t0 for which + * that vertex is vi[0] + */ + uint32_t test_vertex_twin (const uint32_t t0, const uint32_t t1) + { + uint32_t rtn = std::numeric_limits::max(); + uint32_t hi0 = t0; + do { + uint32_t hi1 = t1; + do { + if (this->halfedge[hi0].vi[0] == this->halfedge[hi1].vi[0] + || this->halfedge[hi0].vi[0] == this->halfedge[hi1].vi[1]) { rtn = hi0; } + hi1 = this->halfedge[hi1].next; + } while (hi1 != t1 && rtn == std::numeric_limits::max()); + hi0 = this->halfedge[hi0].next; + } while (hi0 != t0 && rtn == std::numeric_limits::max()); + return rtn; + } + + /*! + * Did the movement pass through the given neighbour triangle, which is probably a + * neighbour-over-a-vertex? + * + * \param nb The halfedge index of the 'to' or neighbour triangle. + * + * \param tn_frm The normal of the 'from' triangle. + * + * \param mv is the movement, assumed to start in the boundary of the from triangle, and to + * lie in the plane of the from triangle + * + * \param start is the start the movement in the new triangle (and the end in the 'from' + * triangle). i.e. it's on the border between the two. + * + * \param model_to_scene Transform required to obtain triangle vertices in scene frame + * + * \return tuple containing true/false 'was a crossing found?'; the rotation axis and the rest + * of the movement, reoriented to the neighbour + */ + std::tuple, sm::vec> + detect_movement_in_neighbour (const uint32_t nb, + const sm::vec& tn_frm, + const sm::vec& mv, + const sm::vec& start, + const sm::mat& model_to_scene) + { + constexpr bool debug_move = false; + + bool found = false; + + // Does mv_rest pass through this neighbour? + sm::vec, 3> tv_nb = this->triangle_vertices (nb, model_to_scene); + + // Have to reorient to each neighbour to test + auto _tn = this->triangle_normal (tv_nb); + if constexpr (debug_move) { + std::cout << __func__ << " called:\n"; + std::cout << "Test candidate movement in nb: " << nb << " " << tv_nb; + std::cout << "\n norm: " << tv_nb.mean() << "," << (_tn * 0.05f) << "\n"; + std::cout << "start/mv: " << start << "," << mv << "\n"; + } + sm::vec r_axis = tn_frm.cross (_tn); + if constexpr (debug_move) { std::cout << "Rotation axis: " << start << "," << r_axis << std::endl; } + r_axis.renormalize(); + // Compute the reorientation due to the requested movement into this neighbour + float rotn_angle = tn_frm.angle (_tn, r_axis); + // 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 between these two triangles + reorient_model.rotate (r_axis, rotn_angle); + // The 'new' mv_rest + sm::vec mv_reoriented = (reorient_model * mv).less_one_dim(); + const float rl = mv_reoriented.length(); + if (std::isnan (rl)) { return {found, r_axis, mv_reoriented}; } + // Now test points along mv_rest to be in + if constexpr (debug_move) { std::cout << "candidate mv_reoriented is " << start << "," << mv_reoriented << std::endl; } + // The far edge will be the next edge + uint32_t faredge = this->halfedge[nb].next; + const sm::vec fes = (model_to_scene * this->vertex[this->halfedge[nb].vi[1]].p).less_one_dim(); + const sm::vec fee = (model_to_scene * this->vertex[this->halfedge[faredge].vi[1]].p).less_one_dim(); + if constexpr (debug_move) { std::cout << "Far edge is " << fes << "," << (fee - fes) << std::endl; } + partial_movement pm = find_edge_crossing (fes, fee, _tn, start, mv_reoriented); + if (pm.flags.test (pm_fl::crossed) == true) { + if constexpr (debug_move) { std::cout << "Return 'found'; cross point over faredge (" << faredge << ") of nb " << nb << "\n"; } + found = true; } else { if constexpr (debug_move) { - std::cout << "First ray_tri_intersected. Start of move is IN triangle ti0\n"; + std::cout << "NO cross point with faredge (" << faredge << ") of " << nb << ", did we land in nb " << nb << "?\n"; + } + // 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], start + mv_reoriented + (_tn / 2.0f), -_tn); + if (is) { // then we DID land in this neighbour tri + if constexpr (debug_move) { std::cout << "Return found; landed IN nb " << nb << "\n"; } + found = true; + } else { + if constexpr (debug_move) { std::cout << "NO ray_tri_intersection in nb " << nb << "\n"; } } } - // rest of function assumes isect was true (exception otherwise) + return {found, r_axis, mv_reoriented}; + } - // 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_inplane = mv_sf - mv_orthog; // scene frame, a relative movement + /*! + * By searching all over-the-vertex neighbours (i.e. neighbours over all 3 vertices), find a + * boundary crossing. + * + * Sub-calls detect_movement_in_neighbour. + */ + crossing_data find_nearest_boundary_crossing (const sm::vec& hov_sf, + const sm::vec& mv_inplane, + const sm::mat& model_to_scene) + { + constexpr bool debug_move = false; + crossing_data cd; - if (mv_inplane.length() == 0.0f) { - if constexpr (debug_move) { std::cout << "No movement, so return unchanged camera viewmatrix\n"; } - return cam_to_scene; + // Get the 'from' triangle normal + sm::vec, 3> tv_frm = this->triangle_vertices (this->ti0, model_to_scene); + sm::vec tn_frm = this->triangle_normal (tv_frm); + if constexpr (debug_move) { + std::cout << "FROM triangle: " << tv_frm << std::endl; + std::cout << "FROM normal: " << tv_frm.mean() << "," << tn_frm << std::endl; } - // New section to handle the case that we started right on a vertex - if (isect == true && int_vertex != 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 - 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 (cd.pm.flags.test (pm_fl::no_cross_point) == false) { - this->ti0 = _ti; - this->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"; - } - 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; - 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"; - } - break; - } + // for each vertex in ti0... + uint32_t _ti = this->ti0; + + std::set> tested; + tested.insert (this->triangle_indices (this->ti0)); // never test ti0 + + for (uint32_t i = 0; i < 3; ++i) { + + const sm::vec vtx_loc = this->edge_start (_ti, model_to_scene); + const sm::vec to_vtx = vtx_loc - hov_sf; + // to_vtx.cross (mv_inplane) should be very short + auto cp = to_vtx.cross (mv_inplane); + + if constexpr (debug_move) { + std::cout << "i = " << i << ", cp.length(): " << cp.length() + << " and to_vtx.dot (mv_inplane) / to_vtx.length() = " + << (to_vtx.dot (mv_inplane) / to_vtx.length()) << std::endl; + } + + // Check to see if vtx_loc is in the direction mv_inplane... + if (to_vtx.dot (mv_inplane) < 0.0f) { + // Then mv_inplane points away from this vtx + _ti = this->halfedge[_ti].next; + continue; + } + + // Get neighbours of _ti + auto nbs = this->find_neighbours (_ti); + const sm::vec mv_rest = mv_inplane - to_vtx; + + // For each neighbour, test to see if start location was inside a neighbour + for (auto nb : nbs) { + std::set ind = this->triangle_indices (nb); + if (tested.count (ind) > 0) { continue; } // Don't test already-tested + tested.insert (ind); + auto[found_in, r_axis, _mv_rest] = detect_movement_in_neighbour (nb, tn_frm, mv_rest, vtx_loc, model_to_scene); + if (found_in) { + cd.crossed = _ti; // the vtx halfedge + cd.into = nb; + cd.tri_edge = r_axis; + cd.mv_rest = _mv_rest; + cd.pm.mv = to_vtx; + cd.pm.end = vtx_loc; + cd.pm.flags.set (pm_fl::crossed); + //cd.pm.flags.set (pm_fl::near_vertex_0); // of _ti. If I set this, then it triggers find_triangle_over... + + break; // also need to break out of outer loop } } - } // Now carry on with corrected mv_inplane, tn0 and ti0 + if (cd.into != std::numeric_limits::max()) { break; } + _ti = this->halfedge[_ti].next; + } + + // Did we get a result? + if (cd.into != std::numeric_limits::max()) { + if constexpr (debug_move) { std::cout << "detected a neighbour with detect_movement_in_neighbour()\n"; } + // cd should have been set up... + } // else cd.into is max, that means we're off-edge. + + return cd; + } + + /*! + * Find which triangle we crossed into over the vertex. Update cd with new + * tri_edge. crossing_data required here for initial halfedge and also to be populated. + * + * The triangle into which we cross is placed into cd.into + * + * Sub-calls detect_movement_in_neighbour. + */ + void find_triangle_over_vertex (crossing_data& cd, + const sm::vec& mv_inplane, + const sm::mat& model_to_scene) + { + constexpr bool debug_move = false; + + if (cd.pm.flags.any_of ({pm_fl::near_vertex_0, pm_fl::near_vertex_1}) == false) { + if constexpr (debug_move) { std::cout << "Crossing cd does not go near a vertex, returning\n"; } + return; + } + + if constexpr (debug_move) { + std::cout << "Finding triangle after crossing halfedge " << cd.crossed << " near vertex " + << (cd.pm.flags.test (pm_fl::near_vertex_0) ? "0" : "1") << "\n"; + } + + uint32_t cv = cd.pm.flags.test (pm_fl::near_vertex_0) ? cd.crossed : this->halfedge[cd.crossed].next; + if constexpr (debug_move) { std::cout << "Vertex is represented by halfedge " << cv << std::endl; } + + std::vector nbs = find_neighbours (cv); - // A 'detected crossing' is one where we had to use a secondary method (comparing the - // triangle containing the start and the triangle containing the end) to determine that - // 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 + // Get the 'from' triangle normal + sm::vec, 3> tv_frm = this->triangle_vertices (cv, model_to_scene); + sm::vec tn_frm = this->triangle_normal (tv_frm); + if constexpr (debug_move) { + std::cout << "FROM triangle: " << tv_frm << std::endl; + std::cout << "FROM normal: " << tv_frm.mean() << "," << tn_frm << std::endl; + } + + const sm::vec mv_rest = mv_inplane - cd.pm.mv; + const sm::vec end_at_border = cd.pm.end; + for (auto nb : nbs) { + if (nb == cv) { continue; } // Don't test crossing into self + auto[found_in, r_axis, _mv_rest] = detect_movement_in_neighbour (nb, tn_frm, mv_rest, end_at_border, model_to_scene); + + if (found_in) { + if constexpr (debug_move) { std::cout << "Found movement into neighbour " << nb << "\n"; } + cd.into = nb; + cd.crossed = cv; + cd.tri_edge = r_axis; + cd.mv_rest = _mv_rest; + // Don't update cd.pm.mv/cd.pm.end here, they're already set + cd.pm.flags.set (pm_fl::crossed); + break; + } + } + } + /*! + * Move across triangles, until at the end of mv_inplane. + * + * This is the main subroutine of compute_mesh_movement. + * + * On each loop, we move either to the end of the movement, if it is within the current + * triangle (ti0) OR we move to the boundary that we cross, and adjust mv_inplane. + */ + void traverse_triangles (sm::vec& mv_inplane, + sm::vec, 3>& tv_sf, + sm::vec& tn0, + sm::vec& hov_sf, + sm::mat& cam_to_surface, + const sm::mat& model_to_scene) + { + constexpr bool debug_move = false; + // In case we throw off-edge, we need to restore ti0's state + const uint32_t ti0_save = this->ti0; + bool done = false; // Now loop while our path may traverse one or more triangles - while (!flags.test (cmm_fl::done)) { + while (!done) { if constexpr (debug_move) { - std::cout << "\nWHILE LOOP\n" - << "ti0 = (" << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] << ")\n" + std::cout << "\nWHILE LOOP, TRAVERSING TRIANGLES\n" + << "ti0 = (" << this->ti0 << ") = " << tv_sf << "\n" << "mv_inplane: " << hov_sf << "," << mv_inplane << "\n" - << "tn0 = " << this->tn0 << ")\n"; - } - - if (mv_inplane.length() == 0) { - ne.m_type = NavException::type::zero_mv; - throw ne; - } - if (mv_inplane.has_nan()) { - ne.m_type = NavException::type::nan_mv; - throw ne; + << "tn0 = " << tn0 << ")\n"; } - // Apply the edge crossing algorithm - crossing_data cd = this->compute_crossing_location (tv_sf, this->ti0, hov_sf, mv_inplane, this->tn0); + // zero length mv_inplane should have been tested before calling this function + if (mv_inplane.length() == 0) { throw std::runtime_error ("Zero length mv_inplane"); } + if (mv_inplane.has_nan()) { throw std::runtime_error ("mv_inplane contained NaN"); } - 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') + // 1. Apply the fast edge crossing algorithm + crossing_data cd = this->compute_crossing_location (tv_sf, this->ti0, hov_sf, mv_inplane); - 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; - } - // 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.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 it failed to find a cross point, then we test inside the triangle and neighbours + // (Also if we moved colinearly along edge past a vertex) + if ((cd.pm.flags.test (pm_fl::colinear) == true && cd.pm.flags.test (pm_fl::crossed) == true) + || (cd.pm.flags.test (pm_fl::colinear) == false && cd.pm.flags.test (pm_fl::crossed) == false)) { + // Now test if our movement stays within the triangle + sm::vec endmv = (cam_to_surface * sm::vec{}).less_one_dim() + mv_inplane; + if constexpr (debug_move) { + std::cout << "endmv intersection with ti0 " << ti0 << " test vector " << (endmv + (tn0 / 2.0f)) << "," << -tn0 << " with tri " << tv_sf << std::endl; + } + auto [isect, isectpoint] = sm::geometry::ray_tri_intersection (tv_sf[0], tv_sf[1], tv_sf[2], endmv + (tn0 / 2.0f), -tn0); + if constexpr (debug_move) { std::cout << "End lands in ? " << (isect ? "Y" : "N") << std::endl; } + if (isect == false) { + // Didn't find edge crossing or that the end point is within ti0, so now search neighbours for an end point or boundary crossing. + cd = find_nearest_boundary_crossing (hov_sf, mv_inplane, model_to_scene); + if (cd.into == std::numeric_limits::max()) { + this->ti0 = ti0_save; + throw std::runtime_error ("off-edge: The movement went off the edge of the model over a vertex"); } } + } // else We HAVE a crossing of some sort. + + // crossing_data gives us info about if there is NO cross point in the partial mv crossed flag + if (cd.pm.flags.test (pm_fl::colinear) == true && cd.pm.flags.test (pm_fl::crossed) == false) { + if constexpr (debug_move) { std::cout << "A: Movement stays inside triangle ti0 (colinear within boundary\n"; } + cam_to_surface.pretranslate (mv_inplane); + done = true; + } else if (cd.pm.flags.test (pm_fl::crossed) == false) { // move a bit, shorten mv_inplane + // mv_inplane moved camera inside triangle. + if constexpr (debug_move) { std::cout << "A: Movement stays inside triangle ti0\n"; } + cam_to_surface.pretranslate (mv_inplane); + done = true; + } else { - // _ti, _tn are the new triangle - sm::vec _tn = {}; - std::array _ti = {}; - if (flags.test (cmm_fl::vertex_crossing)) { + if (cd.pm.flags.any_of ({pm_fl::near_vertex_0, pm_fl::near_vertex_1})) { 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 (cd.pm.flags.test (pm_fl::near_vertex_0)) { + std::cout << "B: Crossed near vertex 0 of crossed edge\n"; + } else if (cd.pm.flags.test (pm_fl::near_vertex_1)) { + std::cout << "B: Crossed near vertex 1 of crossed edge\n"; + } } - _ti = detected_newtri; + // The right triangle to reorient onto may not be the twin across the crossed edge + // Check all neighbours of the crossed vertex to find out if our path travels through. + find_triangle_over_vertex (cd, mv_inplane, model_to_scene); // This could set cd itself + } else { - // Can work out new triangle here across the crossed edge + // If compute_crossing_location found boundary, then new triangle is the twin of the crossed edge. + if (cd.into == std::numeric_limits::max()) { + cd.into = this->halfedge[cd.crossed].twin; + } // else: BUT if find_nearest_boundary_crossing found boundary, then new triangle _ti has been set into cd.into + 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 << "B: Crossed a boundary halfedge " << cd.crossed << " which twins/crosses to: " << cd.into << 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 (cd.into == std::numeric_limits::max()) { + // We probably went off the edge of our navigation model mesh + this->ti0 = ti0_save; + throw std::runtime_error ("off-edge: The movement went off the edge of the model"); + } - // Re-orient onto the new triangle - sm::vec, 3> newtv_sf = this->triangle_vertices (_ti, model_to_scene); - _tn = this->triangle_normal (newtv_sf); + // Re-orient onto the new triangle + sm::vec, 3> newtv_sf = this->triangle_vertices (cd.into, model_to_scene); + 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 { + sm::vec _tn = this->triangle_normal (newtv_sf); if constexpr (debug_move) { - std::cout << "RE-ORIENT to _ti: " << _ti[0] << "," << _ti[1] << "," << _ti[2] - << "\n " << newtv_sf << "\n normal " << _tn << "\n"; + std::cout << "RE-ORIENT to cd.into: " << cd.into << " " << newtv_sf << " norm: " << newtv_sf.mean() << "," << _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 = 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); } + 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(); - reorient_model.pretranslate (hov_sf + cd.pm.mv + mv_rest); + + // cd.mv_rest may have been set, or it may need setting + if (cd.mv_rest[0] == std::numeric_limits::max()) { + if constexpr (debug_move) { std::cout << "cd.mv_rest was unset; setting it from mv_inplane and cd.pm.mv\n"; } + cd.mv_rest = (reorient_model * (mv_inplane - cd.pm.mv)).less_one_dim(); + } else { + if constexpr (debug_move) { std::cout << "cd.mv_rest was set up already\n"; } + } + if constexpr (debug_move) { std::cout << "mv_rest: " << cd.pm.end << "," << cd.mv_rest << std::endl; } + + const float rl = cd.mv_rest.length(); + if (std::isnan (rl)) { + 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"); + } + reorient_model.pretranslate (hov_sf + cd.pm.mv + cd.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) { + if (rl <= std::numeric_limits::epsilon()) { // 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); + 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"; - } - 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??? - // 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; - } + if constexpr (debug_move) { std::cout << "cd.mv_rest length is " << rl << std::endl; } + // Loop To Next. Final movement should be exclusively within a triangle. + reorient_model.pretranslate (-cd.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 = cd.mv_rest; } - this->ti0 = _ti; - ne.tris.push_back (this->ti0); - this->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; - continue; + this->ti0 = cd.into; + tn0 = _tn; } - } else { // NO triangle edge crossing was detected with compute_crossing_location - - // We had intersection in ti0, but no apparent crossing over its edges. - // We may have moved entirely within the starting triangle or colinear with an edge. Test for these cases. - - // Check if it was a colinear movement - if (cd.pm.flags.test (pm_fl::colinear)) { - 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; - } - } 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 - << " 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); - flags.set (cmm_fl::single_movement, single_mv); - } + } // compute_crossing_location if/else - 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 - cam_to_surface.pretranslate (mv_inplane); - flags.set (cmm_fl::done, true); + } // triangle traversing while loop + } - } 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"; - } + /*! + * Compute a movement over this navigation mesh. + * + * We convert the triangle vertices from the model frame to the scene frame before computing + * reorientations, so that non-uniform scalings in the model do not fox us. + * + * \param mv_camframe A movement vector in the camera's own frame of reference (an + * ego-motion) + * + * \param cam_to_scene The transformation matrix to bring the camera coordinates to the + * scene frame + * + * \param model_to_scene The transformation matrix to convert model coordinates to the scene + * frame + * + * \param hoverheight The height at which we want the camera to 'hover' over the + * model/landscape surface + * + * \return The re-positioned camera transform matrix + */ + sm::mat compute_mesh_movement (const sm::vec& mv_camframe, + const sm::mat& cam_to_scene, + const sm::mat& model_to_scene, + const float hoverheight) + { + constexpr bool debug_move = false; - // Test 3 neighbours across the edges 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; - } - - // 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; - 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) - } - } - } + // 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"); } - // 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 (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. - } - } - } + // Compute the triangle normal in the scene frame + sm::vec tn0 = this->triangle_normal (tv_sf); - if (_ti_2n[0] != std::numeric_limits::max()) { - // Now we know an alternative start triangle for the movement. Re-orient to this and re-loop - this->ti0 = _ti_2n; - ne.tris.push_back (this->ti0); - this->tn0 = _tn_2n; - // recompute mv_inplane for this neighbour triangle - mv_orthog = this->tn0 * (mv_sf.dot (this->tn0) / (this->tn0.dot (this->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. - } else if (flags.test (cmm_fl::vertex_crossing)) { - // We didn't find an alternative start triangle, but we did detect a vertex crossing, so continue. - } else { - // End of move not evidently in self or neighbours, so assume it's bang on the boundary - if constexpr (debug_move2) { std::cout << "Movement complete on boundary ASSUMPTION\n"; } - cam_to_surface.pretranslate (mv_inplane); - flags.set (cmm_fl::done, true); - } + if constexpr (debug_move) { + std::cout << "\n# compute_mesh_movement:\n" + << "\nti0: " << this->ti0 + << "\nti0 (sf): " << tv_sf << "\nnormal " << tv_sf.mean() << "," << tn0 + << "\nmovement (camframe): " << mv_camframe + << "\nInitial camera location: " << cam_to_scene.translation() << "\n\n"; + } - } // single movement if/else + // Find the intersection point with the triangle ti0: + sm::vec hov_sf = {}; + sm::mat cam_to_surface; + bool isect = this->find_first_intersection (tv_sf, tn0, hov_sf, cam_to_surface, + cam_to_scene, model_to_scene, hoverheight); + if (!isect) { throw std::runtime_error ("No initial intersection found"); } - } // compute_crossing_location if/else + // 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() - cam_to_scene.translation(); + 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) { + if constexpr (debug_move) { std::cout << "No movement, so return unchanged camera viewmatrix\n"; } + return cam_to_scene; + } - } // triangle traversing while loop + // Now traverse those triangles! The output of this function is cam_to_surface + this->traverse_triangles (mv_inplane, tv_sf, tn0, hov_sf, cam_to_surface, model_to_scene); // 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..59356caf 100644 --- a/mplot/NormalsVisual.h +++ b/mplot/NormalsVisual.h @@ -6,11 +6,28 @@ #include #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 +47,144 @@ 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 + sm::vec<> rpos = ((p0 + p1) / 2.0f); + // output text position of rogue half edge in Blender (not glTF/OpenGL y-up) z-up coordinates, so need this transform: + auto to_blender = sm::mat::frombasis (sm::vec::ux(), sm::vec::uz(), -sm::vec::uy()); + std::cout << "Showing a rogue at " + << (to_blender * p0).less_one_dim() + << " " << rpos.length() << " from origin\n" + << " otherend: " << (to_blender * p1).less_one_dim() << std::endl; + + + 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); + } + this->computeSphere (rpos + sm::vec<>::uy() * 0.2f, mplot::colour::yellow, 0.1f); + } 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 +199,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..b190f448 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,170 @@ 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; + + 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; - 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; + 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 + } - navmesh->triangles.push_back ({t, n, nx, ny}); // n is computed normal + /*! + * 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 + + if (this->flags.test (vm_bools::compute_bb) == false) { + throw std::runtime_error ("make_navmesh requires compute_bb flag to be true"); } - if constexpr (debug_mn) { std::cout << "make_navmesh: Created triangles" << std::endl; } + this->update_bb(); + + // Create a new navmesh + this->navmesh = std::make_unique(); - //navmesh->mark_edge_triangles(); - //if constexpr (debug_mn) { std::cout << "make_navmesh: Marked edge triangles and done." << std::endl; } + // 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 += "/"; } + } + std::string filename = navmesh_dir + std::string("navmesh_") + std::to_string (h); + std::string filename_pre_boundary = filename + ".pre"; + + constexpr bool just_mark = true; + if (mplot::tools::fileExists (filename)) { + this->navmesh->load (filename); + std::cout << "Full test...\n"; + this->navmesh->test(); + + } 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 (just_mark); + 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 (just_mark); + this->navmesh->save (filename); + } } /** @@ -1510,11 +1583,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/VisualResourcesMX.h b/mplot/VisualResourcesMX.h index b275b5db..75ba13fe 100644 --- a/mplot/VisualResourcesMX.h +++ b/mplot/VisualResourcesMX.h @@ -135,6 +135,7 @@ namespace mplot void insert_instance_data (const unsigned int instance_idx, const sm::vec& coord) { + // If this function fails, make sure to call v.render before calling set_instance_data :) if (instance_idx >= this->max_instances) { throw std::runtime_error ("insert_instance_data: bad instance_idx"); } diff --git a/mplot/compoundray/interop.h b/mplot/compoundray/interop.h index 134abc9f..c97fc8c4 100644 --- a/mplot/compoundray/interop.h +++ b/mplot/compoundray/interop.h @@ -36,15 +36,20 @@ namespace mplot::compoundray // Blender has a transformation to convert the native y-up OpenGL/GLTF coordinate system into a // z-up coordinate system. To work in Blender, we need a "match blender" mode in which we apply - // the same transform. This function returns the matrix that should be passed to + // the same transform. This function returns the matrix (as sm::mat) that should be passed to // libEyeRenderer's loadGlTFscene + sm::mat blender_transform_mat() + { + return sm::mat::frombasis (sm::vec::ux(), sm::vec::uz(), -sm::vec::uy()); + } + + // Blender has a transformation to convert the native y-up OpenGL/GLTF coordinate system into a + // z-up coordinate system. To work in Blender, we need a "match blender" mode in which we apply + // the same transform. This function returns the matrix (as sutil::Matrix4x4) that should be + // passed to libEyeRenderer's loadGlTFscene sutil::Matrix4x4 blender_transform() { - constexpr sm::vec ux = { 1.0f, 0.0f, 0.0f }; - constexpr sm::vec uy = { 0.0f, 1.0f, 0.0f }; - constexpr sm::vec uz = { 0.0f, 0.0f, 1.0f }; - sm::mat world_transform = sm::mat::frombasis (ux, uz, -uy); - return mplot::compoundray::mat44_to_Matrix4x4 (world_transform); + return mplot::compoundray::mat44_to_Matrix4x4 (mplot::compoundray::blender_transform_mat()); } /*! 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.