Step Type Query

src/CMakeSettings.json

@@ -175,6 +175,39 @@
           "type": "BOOL"
         }
       ]
+    },
+    {
+      "name": "x64-Debug-C",
+      "generator": "Visual Studio 17 2022 Win64",
+      "configurationType": "Debug",
+      "buildRoot": "${projectDir}\\out\\build\\${name}",
+      "installRoot": "${projectDir}\\out\\install\\${name}",
+      "cmakeCommandArgs": "",
+      "buildCommandArgs": "",
+      "ctestCommandArgs": "",
+      "inheritEnvironments": [ "msvc_x64_x64" ],
+      "variables": [
+        {
+          "name": "DHARTAPI_Config",
+          "value": "All",
+          "type": "STRING"
+        },
+        {
+          "name": "DHARTAPI_EnableTests",
+          "value": "True",
+          "type": "BOOL"
+        },
+        {
+          "name": "DHARTAPI_BuildCSharpTests",
+          "value": "False",
+          "type": "BOOL"
+        },
+        {
+          "name": "DHARTAPI_EnableCSharp",
+          "value": "False",
+          "type": "BOOL"
+        }
+      ]
     }
   ]
 }

src/Cinterface/analysis_C.cpp

@@ -105,3 +105,33 @@ C_INTERFACE GenerateGraphObstacles(
 	*out_graph = G;
 	return OK;
 }
+
+C_INTERFACE CalculateAndStoreStepTypes(
+	HF::SpatialStructures::Graph* g, 
+	HF::RayTracer::EmbreeRayTracer* ray_tracer,
+	float up_step,
+	float down_step,
+	float up_slope,
+	float down_slope,
+	float ground_offset,
+	float node_z,
+	float node_spacing
+	) {
+	HF::GraphGenerator::Precision precision = { node_z, node_spacing, ground_offset };
+	HF::GraphGenerator::GraphParams params;
+
+	//Setup params struct
+	params.up_step = up_step; params.down_step = down_step;
+	params.up_slope = up_slope; params.down_slope = down_slope;
+	params.precision.ground_offset = ground_offset;
+	params.precision.node_z = node_z;
+	params.precision.node_spacing = node_spacing;
+
+	g->Compress();
+
+	auto result = CalculateStepType(*g, HF::RayTracer::MultiRT(ray_tracer), params);
+
+	g->AddEdges(result, "step_type");
+
+	return OK;
+}

src/Cinterface/analysis_C.h

@@ -223,6 +223,45 @@ C_INTERFACE GenerateGraphObstacles(
 	HF::SpatialStructures::Graph** out_graph
 );
 
+/*!
+	\brief		Query the graph and identify the step types of all edges, adding them to the graph.
+
+	\param		g						Graph used to query step types.
+
+	\param		ray_tracer				Raytracer containing the geometry to use for graph generation.
+
+	\param		up_step					Maximum height of a step the graph can traverse.
+										Any steps higher this will be considered inaccessible.
+
+	\param		up_slope				Maximum upward slope the graph can traverse in degrees.
+										Any slopes steeper than this will be considered inaccessible.
+
+	\param		down_step				Maximum step down the graph can traverse.
+										Any steps steeper than this will be considered inaccessible.
+
+	\param		down_slope				The maximum downward slope the graph can traverse.
+										Any slopes steeper than this will be considered inaccessible.
+
+	\param		ground_offset			Distance to offset nodes from the ground before checking line of sight.
+
+	\param		node_z					Precision to round the z-component of nodes after a raycast is performed.
+
+	\param		node_spacing			Precision to round nodes after spacing is calculated.
+
+	\returns	\link HF_STATUS::OK \endlink if query was successful and edges corresponding to step types have been added to the graph.
+*/
+
+C_INTERFACE CalculateAndStoreStepTypes(
+	HF::SpatialStructures::Graph* g,
+	HF::RayTracer::EmbreeRayTracer* ray_tracer,
+	float up_step,
+	float down_step,
+	float up_slope,
+	float down_slope,
+	float ground_offset,
+	float node_z,
+	float node_spacing
+);
 /**@}*/
 
 #endif /* ANALYSIS_C_H */

src/Cpp/analysismethods/src/graph_generator.h

@@ -12,6 +12,7 @@
 #include <vector>
 #include <array>
 #include <Node.h>
+#include <graph.h>
 #include <cassert>
 #include <variant>
 #include <MultiRT.h>
@@ -26,6 +27,8 @@ namespace HF::SpatialStructures {
 	class Graph;
 	struct Edge;
 	enum STEP;
+	struct Subgraph;
+	struct EdgeSet;
 }
 
 /*! \brief Generate a graph of accessible space from a given start point. 
@@ -782,6 +785,24 @@ namespace HF::GraphGenerator {
 		const GraphParams & params
 	);
 
+	HF::SpatialStructures::EdgeSet CalculateStepType(
+		const HF::SpatialStructures::Subgraph& sg,
+		HF::RayTracer::MultiRT& rt,
+		const GraphParams& params
+	);
+
+	std::vector<HF::SpatialStructures::EdgeSet> CalculateStepType(
+		const HF::SpatialStructures::Graph& g,
+		HF::RayTracer::MultiRT& rt,
+		const GraphParams& params
+	);
+
+	void CalculateAndStoreStepType(
+		HF::SpatialStructures::Graph& g,
+		HF::RayTracer::MultiRT& rt,
+		const GraphParams& params
+	);
+
 	/*! 
 		\brief Determine if there is a valid line of sight between parent and child
 

src/Cpp/analysismethods/src/graph_utils.cpp

@@ -300,6 +300,88 @@ namespace HF::GraphGenerator {
 		return calc_slope > -1.0 * gp.down_slope && calc_slope < gp.up_slope;
 	}
 
+	HF::SpatialStructures::EdgeSet CalculateStepType(const HF::SpatialStructures::Subgraph& sg, HF::RayTracer::MultiRT& rt, const GraphParams& params) {
+		// Step type will be stored here and returned from this function
+
+		//Initialize output
+		std::vector<HF::SpatialStructures::EdgeSet> edge_set;
+
+		//From Subgraph sg
+		Node parent_node = sg.m_parent;
+		std::vector<Edge> edge_list = sg.m_edges;
+
+		// We can preallocate this container to have edge_list.size()
+		// blocks since we know how many children are in the subgraph.
+		std::vector<HF::SpatialStructures::IntEdge> children(edge_list.size());
+		auto it_children = children.begin();
+
+		//For every edge...
+		for (Edge link_a : edge_list) {
+			Node curr_child = link_a.child;
+
+			//Type Casting
+			real_t parent_x = CastToReal(parent_node.x);
+			real_t parent_y = CastToReal(parent_node.y);
+			real_t parent_z = CastToReal(parent_node.z);
+
+			real_t child_x = CastToReal(curr_child.x);
+			real_t child_y = CastToReal(curr_child.y);
+			real_t child_z = CastToReal(curr_child.z);
+
+			std::vector<real_t> parent_loc = { parent_x, parent_y, parent_z };
+			std::vector<real_t> child_loc = { child_x, child_y, child_z };
+
+			real3 parent_cast = CastToReal3(parent_loc);
+			real3 child_cast = CastToReal3(child_loc);
+
+			//Get the step type between parent and child
+			STEP step_type = CheckConnection(parent_cast, child_cast, rt, params);
+
+			//Construct an intedge for this parent-child pair to add to edgeset
+			HF::SpatialStructures::IntEdge ie = { link_a.child.id, static_cast<float>(step_type) };
+
+			*(it_children++) = ie;
+		}
+
+		HF::SpatialStructures::EdgeSet es = { parent_node.id, children };
+
+		return es;
+	}
+
+	std::vector<HF::SpatialStructures::EdgeSet> CalculateStepType(const HF::SpatialStructures::Graph& g, HF::RayTracer::MultiRT& rt, const GraphParams& params) {
+		// Retrieve all nodes from g so we can obtain subgraphs.
+		std::vector<HF::SpatialStructures::Node> nodes = g.Nodes();
+
+		// The result container will always be, at most, the node count of g. 
+		// We can preallocate this memory so we do not have to resize during the loop below.
+		std::vector<HF::SpatialStructures::EdgeSet> result(nodes.size());
+		auto it_result = result.begin();
+
+		for (Node parent_node : nodes) {
+			// Get subgraph via parent_node
+			HF::SpatialStructures::Subgraph sg = g.GetSubgraph(parent_node);
+
+			// Call CalculateStepType using the subgraph
+			HF::SpatialStructures::EdgeSet step_types = CalculateStepType(sg, rt, params);
+
+			*(it_result++) = step_types;
+		}
+		return result;
+	}
+
+	void CalculateAndStoreStepType(HF::SpatialStructures::Graph& g, HF::RayTracer::MultiRT& rt, const GraphParams& params) {
+		// calculates and stores step types of all edges and stores it in the graph
+
+		// Get all edges with weights corresponding to step type
+		auto result = HF::GraphGenerator::CalculateStepType(g, rt, params);
+
+		// Compression needed before adding edges of alternate cost
+		g.Compress();
+
+		// Add edges to the graph
+		g.AddEdges(result, "step_type");
+	}
+
 	HF::SpatialStructures::STEP CheckConnection(
 		const real3& parent,
 		const real3& child,

src/Cpp/tests/src/GraphAlgorithms.cpp

@@ -12,7 +12,6 @@
 #include<ray_data.h>
 
 #include "analysis_C.h"
-#include "graph.h"
 
 namespace HF {
 

src/Cpp/tests/src/GraphGenerator.cpp

@@ -9,6 +9,7 @@
 #include <edge.h>
 #include <node.h>
 #include <Constants.h>
+#include <analysis_C.h>
 #include <graph_generator.h>
 #include <objloader.h>
 #include <unique_queue.h>
@@ -663,5 +664,128 @@ TEST(_GraphGenerator, OcclusionCheck) {
 	ASSERT_FALSE(occlusion_check_child_2);
 }
 
+TEST(_GraphGenerator, CalculateStepType) {
+	EmbreeRayTracer ray_tracer = CreateGGExmapleRT();
+	//! [EX_CheckStepTypes]
+
+	// Create graph parameters
+	HF::GraphGenerator::GraphParams params;
+	params.up_step = 2; params.down_step = 2;
+	params.up_slope = 45; params.down_slope = 45;
+	params.precision.node_z = 0.01f;
+	params.precision.ground_offset = 0.01f;
+
+	Graph g;
+	Node N0 = Node(0, 0, 1);
+	Node N1 = Node(0, 2, 0);
+	Node N2 = Node(1, 0, 0);
+	Node N3 = Node(0, 1, 0);
+	Node N4 = Node(2, 0, 0);
+
+	g.addEdge(N0, N1);
+	g.addEdge(N0, N2);
+	g.addEdge(N0, N3);
+	g.addEdge(N0, N4);
+
+	g.Compress();
+
+	std::vector<HF::SpatialStructures::EdgeSet> step_types = HF::GraphGenerator::CalculateStepType(g, HF::RayTracer::MultiRT(&ray_tracer), params);
+
+	HF::SpatialStructures::EdgeSet node_one_edgeset = step_types[0];
+	std::vector<HF::SpatialStructures::IntEdge> node_one_inteedges = node_one_edgeset.children;
+
+	std::vector<float> expected_step_types = { 1,0,0,1 };
+
+	for (int i = 0; i < expected_step_types.size(); i++) {
+		float step_type = node_one_inteedges[i].weight;
+		ASSERT_EQ(step_type, expected_step_types[i]);
+	}
+	//! [EX_CheckStepTypes]
+}
+
+TEST(_GraphGenerator, CalculateAndStoreStepType) {
+	EmbreeRayTracer ray_tracer = CreateGGExmapleRT();
+
+	// Create graph parameters
+	HF::GraphGenerator::GraphParams params;
+	params.up_step = 2; params.down_step = 2;
+	params.up_slope = 45; params.down_slope = 45;
+	params.precision.node_z = 0.01f;
+	params.precision.ground_offset = 0.01f;
+
+	// Construct graph
+	Graph g;
+	Node N0 = Node(0, 0, 1);
+	Node N1 = Node(0, 2, 0);
+	Node N2 = Node(1, 0, 0);
+	Node N3 = Node(0, 1, 0);
+	Node N4 = Node(2, 0, 0);
+
+	g.addEdge(N0, N1);
+	g.addEdge(N0, N2);
+	g.addEdge(N0, N3);
+	g.addEdge(N0, N4);
+
+	g.Compress();
+
+	// Compute and store step types in graph
+	CalculateAndStoreStepType(g, HF::RayTracer::MultiRT(&ray_tracer), params);
+
+	std::vector<HF::SpatialStructures::EdgeSet> result = g.GetEdges("step_type");
+
+	// Testing node 0's edges
+	HF::SpatialStructures::EdgeSet node_zero_edgeset = result[0];
+	std::vector<HF::SpatialStructures::IntEdge> node_zero_inteedges = node_zero_edgeset.children;
+
+	std::vector<float> expected_step_types = { 1,0,0,1 };
+
+	for (int i = 0; i < expected_step_types.size(); i++) {
+		float step_type = node_zero_inteedges[i].weight;
+		ASSERT_EQ(step_type, expected_step_types[i]);
+	}
+}
+
+TEST(_GraphGenerator, CalculateAndStoreStepTypes) {
+	EmbreeRayTracer ray_tracer = CreateGGExmapleRT();
+
+	// Create graph parameters
+	HF::GraphGenerator::GraphParams params;
+	params.up_step = 2; params.down_step = 2;
+	params.up_slope = 45; params.down_slope = 45;
+	params.precision.node_z = 0.01f;
+	params.precision.ground_offset = 0.01f;
+
+	// Construct graph
+	Graph g;
+	Node N0 = Node(0, 0, 1);
+	Node N1 = Node(0, 2, 0);
+	Node N2 = Node(1, 0, 0);
+	Node N3 = Node(0, 1, 0);
+	Node N4 = Node(2, 0, 0);
+
+	g.addEdge(N0, N1);
+	g.addEdge(N0, N2);
+	g.addEdge(N0, N3);
+	g.addEdge(N0, N4);
+
+	g.Compress();
+
+	// Compute and store step types in graph
+
+	CalculateAndStoreStepTypes(&g, &ray_tracer, params.up_step, params.down_step, params.up_slope, params.down_slope, params.precision.ground_offset, params.precision.node_z, params.precision.node_spacing);
+
+	std::vector<HF::SpatialStructures::EdgeSet> result = g.GetEdges("step_type");
+
+	// Testing node 0's edges
+	HF::SpatialStructures::EdgeSet node_zero_edgeset = result[0];
+	std::vector<HF::SpatialStructures::IntEdge> node_zero_inteedges = node_zero_edgeset.children;
+
+	std::vector<float> expected_step_types = { 1,0,0,1 };
+
+	for (int i = 0; i < expected_step_types.size(); i++) {
+		float step_type = node_zero_inteedges[i].weight;
+		ASSERT_EQ(step_type, expected_step_types[i]);
+	}
+}