diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index c5fff3b6..a3f30007 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -380,6 +380,9 @@ 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/model_crawler.cpp b/examples/model_crawler.cpp new file mode 100644 index 00000000..3f282591 --- /dev/null +++ b/examples/model_crawler.cpp @@ -0,0 +1,134 @@ +/* + * Move one model (mplot::CoordArrows) around another (mplot::GeodesicVisual), as if it were + * crawling over it. Demonstrates mplot::NavMesh, which allows you to move over a triangular + * landscape model, following the exact contour defined by the landscape's mesh. + */ + +#include +#include +#include +#include +#include +#include + +#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"); + v.rotateAboutNearest (true); + + // How big to make the sphere? + constexpr float radius = 2.0f; + // How many iterations for the geodesic? Try 2, 3 and 4 + int geo_itrns = 4; + if (argc > 1) { + geo_itrns = std::atoi (argv[1]); + if (geo_itrns > 6) { + std::cout << "Warning: GeodesicVisual takes a long time to build for iterations > 6!" << std::endl; + } + } + // How high to hover the arrows + constexpr float hoverheight = 0.05f; + // Model locations within the scene + //sm::vec arrows_loc = { 0.01f, radius + 1.5f * hoverheight, 0.2f }; + sm::vec arrows_loc = { 0.0f, radius + 1.5f * hoverheight, 0.0f }; + sm::vec sphere_loc = {}; + + // A CoordArrows is our "crawling" agent + 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) + + // 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); + 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->colour_bb = cl; + gv->finalize(); + auto gvp = v.addVisualModel (gv); + // re-colour sphere with sequential colouring after construction + gvp->data.linspace (0.0f, 1.0f, gvp->data.size()); + gvp->reinitColours(); + // 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 + sm::vec mv_ca = sm::vec::uz() * move_step; + + // The viewmatrices have to be passed to mplot::NavMesh::compute_mesh_movement + sm::mat44 ca_view = cap->getViewMatrix(); + sm::mat44 sph_view = gvp->getViewMatrix(); + + // 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; + + int move_counter = 0; + constexpr int move_max = 1000; + while (!v.readyToFinish()) { + + // Wait .018 s and also poll for mouse/keyboard events + v.waitevents (0.018); + + // Compute a new movement over the landscape mesh (the sphere) + try { + ca_view = gvp->navmesh->compute_mesh_movement (mv_ca, ca_view, sph_view, hoverheight); + } catch (mplot::NavException& e) { + if (e.m_type == mplot::NavException::type::off_edge) { + std::cout << "You can handle movements that go off the edge of a flat model\n"; + } + 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 + cap->setViewMatrix (ca_view); + + // Compute the new location + arrows_loc = (ca_view * sm::vec{}).less_one_dim(); + + // 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); + } + + // Re-render the scene + v.render(); + } + + v.keepOpen(); + + return rtn; +} diff --git a/maths b/maths index 32af2f0e..7d3174d9 160000 --- a/maths +++ b/maths @@ -1 +1 @@ -Subproject commit 32af2f0e421ecfe3f3f4c23966804a1953b6ae61 +Subproject commit 7d3174d91fe1b93f76dc284d3bba9e29b312bde6 diff --git a/mplot/NavMesh.h b/mplot/NavMesh.h index 72facec4..19449ada 100644 --- a/mplot/NavMesh.h +++ b/mplot/NavMesh.h @@ -105,6 +105,15 @@ namespace mplot //! 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 = {}; + + /*! + * The normal of ti0. This is the current triangle normal (in our mesh's frame of + * reference) that our agent/camera is 'next to' + */ + sm::vec tn0 = {}; + /*! * 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 @@ -176,7 +185,7 @@ namespace mplot // 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) + uint32_t mark_if_on_edge (std::array& _ti0) { constexpr bool debug_met = false; uint32_t n2 = 0; // Neighbours sharing 2 vertices (up to 3) @@ -185,9 +194,9 @@ namespace mplot for (auto& t: this->triangles) { auto [ti, tn, tnc, tnd] = t; - auto a0 = ti0[0]; - auto b0 = ti0[1]; - auto c0 = ti0[2]; + auto a0 = _ti0[0]; + auto b0 = _ti0[1]; + auto c0 = _ti0[2]; auto a = ti[0]; auto b = ti[1]; auto c = ti[2]; @@ -213,9 +222,9 @@ namespace mplot if (n2 < 3) { if constexpr (debug_met) { - std::cout << ti0[0] << "-" << ti0[1] << "-" << ti0[2] << " is on the edge"; + std::cout << _ti0[0] << "-" << _ti0[1] << "-" << _ti0[2] << " is on the edge"; } - ti0[3] = 1; + _ti0[3] = 1; for (auto& net : neighb_edge_tris) { if constexpr (debug_met) { std::cout << " mark vtx neighbour "; } (*net)[3] = 1; @@ -242,17 +251,17 @@ namespace mplot } } - // 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 + // 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 { // 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 a0 = _ti0[0]; + auto b0 = _ti0[1]; + auto c0 = _ti0[2]; auto a = ti[0]; auto b = ti[1]; auto c = ti[2]; @@ -290,26 +299,78 @@ namespace mplot return rtn; } + std::tuple, sm::vec> + first_triangle_containing (uint32_t _idx) 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}; + } + } + return {}; + } + /* * Find the location, and the triangle indices at which a ray starting from coord (scene - * frame) with direction vdir - the 'penetration point'. + * 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. * * \return a tuple containing crossing location, triangle identity (three indices) and triangle normal vector */ std::tuple, std::array, sm::vec> find_triangle_crossing (const sm::vec& coord_mf, const sm::vec& vdir) const { - for (auto tri : triangles) { + constexpr auto umax = std::numeric_limits::max(); + constexpr auto 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 + + 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]], coord_mf - (vdir / 2.0f), vdir); - if (isect) { return {p, ti, tn}; } + 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. + if (isect) { + float d = (p - vstart).sos(); + if (d < isect_d && d < vdir.sos()) { + isect_p = p; + isect_ti = ti; + isect_tn = tn; + isect_d = d; + } + } } - // Failed to find, return container full of maxes - sm::vec p = {}; - p.set_from (std::numeric_limits::max()); - constexpr uint32_t umax = std::numeric_limits::max(); - return {p , std::array{umax, umax, umax, 0}, p}; + if (isect_p[0] == fmax) { + // Found no triangle intersection; check vertices, in case vdir points perfectly at a vertex + for (uint32_t ti = 0; ti < this->vertex.size(); ++ti) { + sm::vec vertex_n = 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] = first_triangle_containing (ti); + isect_ti = _ti; + isect_tn = _tn; + isect_d = d; + } + } + } + } + + return { isect_p, isect_ti, isect_tn }; } // Find the location, and the triangle indices at which a ray between coord (in model frame) @@ -374,6 +435,30 @@ namespace mplot 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; + } + + sm::vec find_vertex_normal (const uint32_t ti) 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; + } + 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) { @@ -715,39 +800,27 @@ namespace mplot std::tuple, sm::vec, std::array> find_triangle_hit (const sm::mat44& camspace, const sm::mat44& model_to_scene) { - constexpr bool debug = false; sm::mat44 scene_to_model = model_to_scene.inverse(); // use camera location in gltf to start from, then find model surface. sm::vec camloc_mf = (scene_to_model * camspace * sm::vec{}).less_one_dim(); - std::array ti0; - sm::vec tn0 = {}; + this->ti0 = {}; + this->tn0 = {}; sm::vec hit = {}; sm::vec vdir = this->bb.mid() - camloc_mf; - float bb_len = this->bb.span().longest(); // lengthscale of model - // Make vdir long - float vdl = vdir.length() * 2.0f; // Twice the distance from camera to BB centroid - vdl += bb_len * 2.0f; // plus twice the longest axis from the BB - vdir.renormalize(); - vdir *= vdl; - std::tie (hit, ti0, tn0) = this->find_triangle_crossing (camloc_mf - (vdir / 2.0f), vdir); - if (ti0[0] == std::numeric_limits::max()) { - std::cout << __func__ << ": No hit\n"; - } - // Can I make hit the centre of the triangle? - constexpr bool hit_tri_centre = false; - if constexpr (hit_tri_centre) { - sm::vec, 3> tv_mf = this->triangle_vertices (ti0); - hit = tv_mf.mean(); - } + std::tie (hit, this->ti0, this->tn0) = this->find_triangle_crossing (camloc_mf , vdir); + + if (this->ti0[0] == std::numeric_limits::max()) { std::cout << __func__ << ": No hit\n"; } + sm::vec hp_scene = (model_to_scene * hit).less_one_dim(); + constexpr bool debug = false; if constexpr (debug) { std::cout << "found hit at " << hit << " (model); " << hp_scene << " (scene)\n"; // Check we'll get a hit when we compute_mesh_movement: - sm::vec, 3> tv_mf = this->triangle_vertices (ti0); - std::cout << "tn0: " << tn0 << ", length " << tn0.length() << std::endl; - std::cout << "TEST ray_tri_intersection (hit,-tn0): " << (hit + (tn0 / 2.0f)) << "," << -tn0 << std::endl; - auto [isect, hov_mf] = sm::geometry::ray_tri_intersection (tv_mf[0], tv_mf[1], tv_mf[2], hit + (tn0 / 2.0f), -tn0); + 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); if (isect) { std::cout << "ray_tri_intersection confirms we would hit at " << hov_mf << "\n"; } else { @@ -756,7 +829,7 @@ namespace mplot } } - return { hp_scene, tn0, ti0 }; + return { hp_scene, this->tn0, this->ti0 }; } /*! @@ -768,11 +841,11 @@ namespace mplot * and z axes randomly oriented. The frame is set to hover hoverheight 'above' the triangle */ sm::mat44 position_camera (const sm::vec& hp_scene, const sm::mat44& model_to_scene, - const sm::vec& tn0, const float hoverheight) + 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 (tn0[0] == std::numeric_limits::max()) { - std::cout << __func__ << ": No hit\n"; + if (this->tn0[0] == std::numeric_limits::max()) { + std::cout << __func__ << ": No hit/triangle normal\n"; return sm::mat44{}; } @@ -781,16 +854,16 @@ 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 (tn0); + sm::vec _x = rand_vec.cross (this->tn0); _x.renormalize(); - sm::vec _z = _x.cross (tn0); + sm::vec _z = _x.cross (this->tn0); // 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 mat44s. sm::mat44 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::mat44 cam_to_model_rotn = sm::mat44::frombasis (_x, tn0, _z); + sm::mat44 cam_to_model_rotn = sm::mat44::frombasis (_x, this->tn0, _z); // Get the rotation from scene frame to model sm::mat44 m_to_sc_rotn = model_to_scene.rotation_mat44(); sm::mat44 hp_m; @@ -809,7 +882,6 @@ namespace mplot * \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 ti0 Triangle indices. Will be updated if movement passed to another triangle * \param hoverheight * * \return The re-positioned camera transform matrix @@ -817,7 +889,6 @@ namespace mplot sm::mat44 compute_mesh_movement (const sm::vec& mv_camframe, const sm::mat44& cam_to_scene, const sm::mat44& model_to_scene, - std::array& ti0, const float hoverheight) { constexpr bool debug_move = false; @@ -825,7 +896,7 @@ namespace mplot // A data-containing exception to throw mplot::NavException ne (mplot::NavException::type::generic); - ne.tris.push_back (ti0); + 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 }; @@ -834,13 +905,13 @@ namespace mplot // 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 (ti0, model_to_scene); + sm::vec, 3> tv_sf = this->triangle_vertices (this->ti0, model_to_scene); // Compute the triangle normal in the scene frame - sm::vec tn0 = this->triangle_normal (tv_sf); + this->tn0 = this->triangle_normal (tv_sf); if constexpr (debug_move) { - std::cout << "\n# compute_mesh_movement: ti0 " << ti0[0] << "," << ti0[1] << "," << ti0[2] - << " has vertices (sf) at " << tv_sf << " and normal " << tn0 + std::cout << "\n# compute_mesh_movement: ti0 " << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] + << " has vertices (sf) at " << tv_sf << " and normal " << this->tn0 << ". upcoming movement (camframe) is " << mv_camframe << std::endl; std::cout << "Initial camera location (camloc_sf): " << camloc_sf << std::endl; } @@ -855,35 +926,61 @@ namespace mplot // boundaries and so requires that the start point is *within* the boundary. // if constexpr (debug_move) { - std::cout << "First ray_tri_intersection (raystart,-tn0): " << (camloc_sf + (tn0 / 2.0f)) << "," << -tn0 << std::endl; + std::cout << "First ray_tri_intersection (raystart,-tn0): " << (camloc_sf + (this->tn0 / 2.0f)) << "," << -this->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 + (tn0 / 2.0f), -tn0); + 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); // Use the detected location, hov_sf to compute the surface location of the camera - its 'hover location' sm::mat44 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 + (tn0 / 2.0f), -tn0); - if constexpr (debug_move2) { + 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); + if constexpr (debug_move) { if (isect == false) { - std::cout << "No isect at start with " << ti0[0] << "," << ti0[1] << "," << ti0[2] + std::cout << "No isect at start with " << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] << " using float OR double internally" << std::endl; } else { - std::cout << "Intersection at start with " << ti0[0] << "," << ti0[1] << "," << ti0[2] + std::cout << "Intersection at start with " << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] << " using *double* internally" << std::endl; } } } + // 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++) { + + // 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]); + 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) + } + hov_sf = tv_sf[i]; + int_vertex = i; + isect = true; + } + } + } + std::vector> trisearched; // the other triangles we search. To place in exception if (isect == false) { if constexpr (debug_move2) { std::cout << "No intersection (at start) with triangle " - << ti0[0] << "," << ti0[1] << "," << ti0[2] + << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] << ", so correct ti0 and tn0 (if we can)" << std::endl; } @@ -893,7 +990,7 @@ namespace mplot 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 (ti0[i1], ti0[i2], ti0); + 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 @@ -908,9 +1005,9 @@ namespace mplot if (is) { if constexpr (debug_move) { std::cout << "*** Correcting!\n"; } // We're in this neighbour, so update ti0/tn0 and mark isect true - ti0 = _ti; + this->ti0 = _ti; tv_sf = tv_lf; - tn0 = _tn; + this->tn0 = _tn; isect = true; // This requires a number of matrix recomputations: hov_sf = h; @@ -924,13 +1021,13 @@ namespace mplot if (isect == false) { if constexpr (debug_move2) { std::cout << "No intersection (at start) with triangle " - << ti0[0] << "," << ti0[1] << "," << ti0[2] << " OR neighbours" << std::endl; + << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] << " OR neighbours" << 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 - (tn0 * hoverheight)); + 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 - (tn0 * hoverheight)) + std::cout << "Closest distance from " << (camloc_sf - (this->tn0 * hoverheight)) << " to ti0 edge: " << closest_edge_d << std::endl; } constexpr float ced_thresh = std::numeric_limits::epsilon() * 50; @@ -945,15 +1042,15 @@ namespace mplot } else { if constexpr (debug_move2) { std::cout << "Found intersection (at start) with (2-)neighbour triangle " - << ti0[0] << "," << ti0[1] << "," << ti0[2] << std::endl; + << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] << std::endl; } } } else { if constexpr (debug_move) { std::cout << "First ray_tri_intersected. Start of move is IN triangle " - << ti0[0] << "," << ti0[1] << "," << ti0[2] - << " from coord " << camloc_sf << " and dirn " << -tn0 << std::endl; + << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] + << " from coord " << camloc_sf << " and dirn " << -this->tn0 << std::endl; } } @@ -961,7 +1058,7 @@ namespace mplot // 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 = tn0 * (mv_sf.dot (tn0) / (tn0.dot (tn0))); + 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 if (mv_inplane.length() == 0.0f) { @@ -969,6 +1066,45 @@ namespace mplot return cam_to_scene; } + // 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; + } + } + } + } // Now carry on with corrected mv_inplane, tn0 and ti0 + // 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 @@ -982,7 +1118,8 @@ namespace mplot while (!flags.test (cmm_fl::done)) { if constexpr (debug_move) { - std::cout << "\n* loopstart: Processing mv_inplane: " << hov_sf << "," << mv_inplane << std::endl; + std::cout << "\n* loopstart: Processing mv_inplane: " << hov_sf << "," << mv_inplane << " ti0 = (" + << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] << ") with tn0 = " << this->tn0 << std::endl; } if (mv_inplane.length() == 0) { @@ -995,7 +1132,7 @@ namespace mplot } // Apply the edge crossing algorithm - crossing_data cd = this->compute_crossing_location (tv_sf, ti0, hov_sf, mv_inplane, tn0); + crossing_data cd = this->compute_crossing_location (tv_sf, this->ti0, hov_sf, mv_inplane, this->tn0); 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') @@ -1031,9 +1168,9 @@ namespace mplot if constexpr (debug_move) { std::cout << "find triangle across edge: find_other_triangle_containing (" << cd.edge_idx_a << ", " << cd.edge_idx_b - << ", [" << ti0[0] << "," << ti0[1] << "," << ti0[2] << "])" << std::endl; + << ", [" << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] << "])" << std::endl; } - std::tie (_ti, _tn) = this->find_other_triangle_containing (cd.edge_idx_a, cd.edge_idx_b, ti0); + 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()) { @@ -1048,11 +1185,11 @@ namespace mplot } // If a vertex crossing, we have to make an edge that is the cross product of the two triangle normals - if (flags.test (cmm_fl::vertex_crossing)) { cd.tri_edge = tn0.cross (_tn); } + if (flags.test (cmm_fl::vertex_crossing)) { cd.tri_edge = this->tn0.cross (_tn); } // Compute the reorientation due to the requested movement. // Rotate by the angle between the normals. I think this is constrained to be <= pi - float rotn_angle = tn0.angle (_tn, cd.tri_edge); + float rotn_angle = this->tn0.angle (_tn, cd.tri_edge); // If tn0 and _tn are identical, then rotn_angle will be NaN, but in that case we want no rotation if (std::isnan (rotn_angle)) { rotn_angle = 0.0f; } sm::mat44 reorient_model; // reorientation transformation in sf @@ -1099,9 +1236,9 @@ namespace mplot } } - ti0 = _ti; - ne.tris.push_back (ti0); - tn0 = _tn; + 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 @@ -1127,10 +1264,10 @@ namespace mplot // 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 + (tn0 / 2.0f)) << "," << -tn0 + << (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 + (tn0 / 2.0f), -tn0); + 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); } @@ -1142,7 +1279,7 @@ namespace mplot } else { if constexpr (debug_move) { - std::cout << "End of movement is NOT in ti0 " << ti0[0] << "," << ti0[1] << "," << ti0[2] << ". Look for start neighbours\n"; + std::cout << "End of movement is NOT in ti0 " << this->ti0[0] << "," << this->ti0[1] << "," << this->ti0[2] << ". Look for start neighbours\n"; } // Test 3 neighbours across the edges to find any for which the start location is also within-boundary @@ -1153,7 +1290,7 @@ namespace mplot 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 (ti0[i1], ti0[i2], ti0); + 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); @@ -1180,7 +1317,7 @@ namespace mplot // 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 = { ti0[i1], ti0[i2] }; + 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 @@ -1199,7 +1336,7 @@ namespace mplot // 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 (ti0); + auto onens = this->find_one_neighbours (this->ti0); for (auto onen : onens) { // Are we in this one? auto [_ti, _tn] = onen; @@ -1223,7 +1360,7 @@ namespace mplot // 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 (ti0, _ti), std::numeric_limits::max() }; + 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 @@ -1239,11 +1376,11 @@ namespace mplot 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 - ti0 = _ti_2n; - ne.tris.push_back (ti0); - tn0 = _tn_2n; + this->ti0 = _ti_2n; + ne.tris.push_back (this->ti0); + this->tn0 = _tn_2n; // recompute mv_inplane for this neighbour triangle - mv_orthog = tn0 * (mv_sf.dot (tn0) / (tn0.dot (tn0))); + 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. @@ -1263,7 +1400,7 @@ namespace mplot } // triangle traversing while loop // Raise cam_to_surface up by hoverheight and then return - cam_to_surface.pretranslate (hoverheight * tn0); + cam_to_surface.pretranslate (hoverheight * this->tn0); if constexpr (debug_move) { std::cout << "looping mv_inplanes completed. Final camloc_sf: " << cam_to_surface.translation() << std::endl; } diff --git a/mplot/SphericalProjectionVisual.h b/mplot/SphericalProjectionVisual.h index a8c2ed3e..95f12003 100644 --- a/mplot/SphericalProjectionVisual.h +++ b/mplot/SphericalProjectionVisual.h @@ -15,9 +15,8 @@ namespace mplot //! This class creates a flat projection of spherical data provided as vvecs of //! latitude-longitude pairs and scalar or vector values. Use VisualDataModel? template requires std::is_floating_point_v - class SphericalProjectionVisual : public mplot::VisualModel + struct SphericalProjectionVisual : public mplot::VisualModel { - public: SphericalProjectionVisual() {} SphericalProjectionVisual (const sm::vec _offset) : mplot::VisualModel(_offset) {}