Bugfix - Agent can't find path while colliding with an obstacle.

The process of finding a path on a roadmap for an agent consists of finding
"visible" links from the agent position to a road map vertex. The
visibility query w.r.t. a *single* obstacle essentially tests for
intersection between the obstacle's line segment and a capsule formed by
the desired link (agent position to vertex position) convolved with a
disk of the agent's radius. For a given vertex, if any obstacle intersects
the vertex is not "visible". If the agent is intersecting an obstacle,
no vertex will be considered visible.

The fix:
  1. Add new query to SpatialQuery; linkIsTraversible(). It is closely
     related to the queryVsibility() method with one extra detail; it
     handles the case where the agent is intersecting an obstacle.
  2. Extend the API through the two implementations of the interface
     (the nav mesh implementation is *not* implemented at all.) The kd-tree
     implementaiton is, essentially, a copy of the visibility query with
     the last test modified.
  3. Change the calls in the roadmap from visibility to traversibility.
  4. Add some stray clarifying comments.
  5. Remove asserts and replace them with more appropriate exceptions.

releaseNotes.txt

@@ -5,6 +5,8 @@ Release 0.9.3
     Bug fixes
         OBBShape had inconsistent definitions for getXBasis() and getYBasis().
             Documentation clarified, usage unified.
+		RoadMap velocity component couldn't create a path for agents who were in collision with an
+		    obstacle. Addressed in [PR 118](https://github.com/MengeCrowdSim/Menge/pull/118)
 
 	Features
 		State population event trigger - upgrade the event triggers that depend on state population.

src/Menge/MengeCore/Agents/SpatialQueries/ObstacleKDTree.cpp

@@ -83,6 +83,13 @@ namespace Menge {
 			queryTreeRecursive(filter, filter->getQueryPoint(), range, _tree );
 		}
 
+    /////////////////////////////////////////////////////////////////////////////
+
+    bool ObstacleKDTree::linkIsTraversible(const Vector2& q1, const Vector2& q2, 
+                                           float radius) const {
+      return linkIsTraversibleRecursive(q1, q2, radius, _tree);
+    }
+
 		/////////////////////////////////////////////////////////////////////////////
 
 		bool ObstacleKDTree::queryVisibility(const Vector2& q1, const Vector2& q2,
@@ -262,11 +269,80 @@ namespace Menge {
 			}
 		}
 
-		/////////////////////////////////////////////////////////////////////////////
-
-		bool ObstacleKDTree::queryVisibilityRecursive(const Vector2& q1, const Vector2& q2,
+    /////////////////////////////////////////////////////////////////////////////
+
+    bool ObstacleKDTree::linkIsTraversibleRecursive(
+      const Vector2& q1, const Vector2& q2, float radius, const ObstacleTreeNode* node) const {
+      if (node == nullptr) {
+        return true;
+      } else {
+        const Obstacle* const obstacle1 = node->_obstacle;
+        const Obstacle* const obstacle2 = obstacle1->_nextObstacle;
+
+        // Scaled signed distance to the obstacle's line; positive values are on the left of the
+        // obstacle. The distance is scaled by the obstacle's length.
+        const float q1LeftOfObst = leftOf(obstacle1->_point, obstacle2->_point, q1);
+        const float q2LeftOfObst = leftOf(obstacle1->_point, obstacle2->_point, q2);
+        const float invObstLengthSqd = 1.0f / absSq(obstacle2->_point - obstacle1->_point);
+        const float rad_sqd = sqr(radius);
+
+        // In all of these tests we hope for a proof of non-traversibility -- such a proof is
+        // sufficient to return without evaluating any more of the tree.
+
+        if (q1LeftOfObst >= 0.0f && q2LeftOfObst >= 0.0f) {
+          // The link lies completely on the "left" side of the obstacle. To be traversible, it must
+          //   - be traversible w.r.t. all the obstacles on the left side AND
+          //   - be at least radius distance away from the obstacle's *line* OR
+          //     be traversible w.r.t. all the obstacles on the right side.
+          //     The "at least radius distance away from the line" is merely a performance
+          //     optimization.
+          // TODO(curds01): Neither this, nor the "completely-on-the-right" case do further tests
+          // against *this* obstacle and it is not clear why. Confirm in testing that this is
+          // correct.
+          return linkIsTraversibleRecursive(q1, q2, radius, node->_left) &&
+              ((sqr(q1LeftOfObst) * invObstLengthSqd >= rad_sqd &&
+                sqr(q2LeftOfObst) * invObstLengthSqd >= rad_sqd) ||
+               linkIsTraversibleRecursive(q1, q2, radius, node->_right));
+        } else if (q1LeftOfObst <= 0.0f && q2LeftOfObst <= 0.0f) {
+          // The link lies completely on the "right" side of the obstacle. See note on the
+          // "completely-on-the-left-side" case.
+          return linkIsTraversibleRecursive(q1, q2, radius, node->_right) &&
+            ((sqr(q1LeftOfObst) * invObstLengthSqd >= rad_sqd &&
+              sqr(q2LeftOfObst) * invObstLengthSqd >= rad_sqd) ||
+             linkIsTraversibleRecursive(q1, q2, radius, node->_left));
+        } else if (q1LeftOfObst >= 0.0f && q2LeftOfObst <= 0.0f) {
+          // One can traverse through obstacle from left to right.
+          return linkIsTraversibleRecursive(q1, q2, radius, node->_left) &&
+            linkIsTraversibleRecursive(q1, q2, radius, node->_right);
+        } else {
+          // q1 on right, q2 on left. This crosses the *line* from outside to inside. Now it depends
+          // on where on the line the obstacle lies -- with radius of the crossing point?
+          const float point1LeftOfQ = leftOf(q1, q2, obstacle1->_point);
+          const float point2LeftOfQ = leftOf(q1, q2, obstacle2->_point);
+          const float invQLengthSqd = 1.0f / absSq(q2 - q1);
+
+          // Several conditions which make this traversible:
+          //  1. If the obstacle lies entirely on one side of the query link's line AND
+          //  2. The obstacle lies at least radius distance away from the line OR
+          //     q1 is closer than radius to the obstacle and q2 is greater than radius distance AND
+          //  3. It's traversible w.r.t. the right- and left-hand sides of the tree.
+          return (point1LeftOfQ * point2LeftOfQ >= 0.0f &&                     // test condition 1
+                  ((sqr(point1LeftOfQ) * invQLengthSqd > rad_sqd &&            // test condition 2
+                    sqr(point2LeftOfQ) * invQLengthSqd > rad_sqd) ||           //        |
+                   (sqr(q1LeftOfObst) * invObstLengthSqd <= rad_sqd &&         //        |
+                    sqr(q2LeftOfObst) * invObstLengthSqd >= rad_sqd)) &&       //        |
+                  linkIsTraversibleRecursive(q1, q2, radius, node->_left) &&   // test condition 3
+                  linkIsTraversibleRecursive(q1, q2, radius, node->_right));   //        |
+        }
+      }
+    }
+
+    /////////////////////////////////////////////////////////////////////////////
+
+    bool ObstacleKDTree::queryVisibilityRecursive(const Vector2& q1, const Vector2& q2,
 										  float radius, 
 										  const ObstacleTreeNode* node) const {
+      // NOTE: See linkIsTraversible for explanation of this code.
 			if (node == 0) {
 				return true;
 			} else {

src/Menge/MengeCore/Agents/SpatialQueries/ObstacleKDTree.h

@@ -96,6 +96,10 @@ namespace Menge {
 			 */
 			void obstacleQuery( ProximityQuery *query) const;
 
+      /*! @brief  Implementation of SpatialQuery::linkIsTraversible().  */
+      bool linkIsTraversible(const Math::Vector2& q1, const Math::Vector2& q2,
+                             float radius) const;
+
 			/*!
 			 *  @brief      Queries the visibility between two points within a
 			 *              specified radius.
@@ -134,6 +138,9 @@ namespace Menge {
 			void queryTreeRecursive( ProximityQuery *query, Math::Vector2 pt, float& rangeSq,
 									 const ObstacleTreeNode* node ) const;
 
+      /*! @brief  Implementation of linkIsTraversible() via recursion.  */
+      bool linkIsTraversibleRecursive(const Math::Vector2& q1, const Math::Vector2& q2,
+                                      float radius, const ObstacleTreeNode* node) const;
 			/*!
 			 *	@brief		Perform the work, recursively, to determine if q1 can see q2, w.r.t.
 			 *				the obstacles.

src/Menge/MengeCore/Agents/SpatialQueries/SpatialQuery.h

@@ -158,6 +158,37 @@ namespace Menge {
 			 */
 			virtual void obstacleQuery( ProximityQuery * query) const = 0;
 
+      /*!
+       @brief  Reports if an agent can traverse the straight-line path from `q1` to `q2`.
+
+       This query performs a per-obstacle test to determine if the obstacle limits traversibility.
+       This test is closely related to an intersection test between a line segment (an individual
+       obstacle) and a capsule (the traversible link between `q1` and `q2` convolved with a disk of
+       the given `radius`). Generally, if the line segment intersects the link, the link is _not_
+       traversible. However, this isn't universally true.
+
+       First, obstacles have "sides". An obstacle doesn't obstruct if the path from `q1` to `q2
+       passes from _inside_ the obstacle to _outside_ the obstacle.
+
+       Second, even an agent whose center lies _outside_ an obstacle (but otherwise intersects the
+       obstacle) may not be considered obstructed. This query link represents the path of an agent.
+       We cannot guarantee that the starting position is collision free. As such, an obstacle might
+       intersect the capsule in an otherwise meaningless way. These meaningless collisions do not
+       prevent the link from being traversible.
+
+       To be a "meaningless" collision the following must be true:
+
+         - The line segment must intersect a circle centered at `q1` with the given `radius`.
+         - The direction of the link (`q2 - q1`) must point _away_ from the line segment.
+
+      @param  q1      The start point of the link to be tested.
+      @param  q2      The end point of the link to be tested.
+      @param  radius  The radius of the agent to traverse the link.
+      @returns  True if the link is deemed a traversible path.
+      */
+      virtual bool linkIsTraversible(const Math::Vector2& q1, const Math::Vector2& q2,
+                                     float radius) const = 0;
+
 			/*!
 			 *  @brief      Queries the visibility between two points within a
 			 *              specified radius.

src/Menge/MengeCore/Agents/SpatialQueries/SpatialQueryKDTree.h

@@ -98,6 +98,12 @@ namespace Menge {
 				_obstTree.obstacleQuery( query);
 			}
 
+      /*! @brief  Implementation of SpatialQuery::linkIsTraversible().  */
+      bool linkIsTraversible(const Math::Vector2& q1, const Vector2& q2, float radius) const
+        override {
+        return _obstTree.linkIsTraversible(q1, q2, radius);
+      }
+
 			/*!
 			 *  @brief      Queries the visibility between two points within a
 			 *              specified radius.

src/Menge/MengeCore/Agents/SpatialQueries/SpatialQueryNavMesh.cpp

@@ -325,6 +325,12 @@ namespace Menge {
 			}	
 		}
 
+    bool NavMeshSpatialQuery::linkIsTraversible(const Math::Vector2& q1, const Vector2& q2,
+                                                float radius) const {
+      // TODO(curds01): Not implemented.
+      return true;
+    }
+
 		////////////////////////////////////////////////////////////////
 
 		bool NavMeshSpatialQuery::queryVisibility(const Vector2& q1, const Vector2& q2,

src/Menge/MengeCore/Agents/SpatialQueries/SpatialQueryNavMesh.h

@@ -98,6 +98,9 @@ namespace Menge {
 			 */
 			virtual void obstacleQuery( ProximityQuery *query, float rangeSq) const;
 
+      /*! @brief  Implementation of SpatialQuery::linkIsTraversible().  */
+      bool linkIsTraversible(const Math::Vector2& q1, const Vector2& q2, float radius) const
+        override;
 
 			/*!
 			 *  @brief      Queries the visibility between two points within a

src/Menge/MengeCore/BFSM/VelocityComponents/VelCompRoadMap.cpp

@@ -107,8 +107,11 @@ namespace Menge {
 				_lock.releaseRead();
 				// compute the path and add it to the map
 				//	Create path for the agent
-				Vector2 goalPoint = goal->getCentroid();
-				path = _roadmap->getPath( agent, goal ); 
+				path = _roadmap->getPath( agent, goal );
+        if (path == nullptr) {
+          throw VelCompFatalException("Agent " + std::to_string(agent->_id) + 
+                                      " was unable to find a path to its goal");
+        }
 				_lock.lockWrite();
 				_paths[ agent->_id ] = path;
 				_lock.releaseWrite();
@@ -123,8 +126,12 @@ namespace Menge {
 				delete path;
 				Vector2 goalPoint = goal->getCentroid();
 				path = _roadmap->getPath(agent, goal);
-				// While this operation doesn't change the structure of the map (agent->_id is already a key),
-				// we lock it to prevent any *other* write operation from interfering.
+        if (path == nullptr) {
+          throw VelCompFatalException("Agent " + std::to_string(agent->_id) + 
+                                      " lost roadmap path and was unable to create a new path");
+        }
+				// While this operation doesn't change the structure of the map (agent->_id is already a
+        // key), we lock it to prevent any *other* write operation from interfering.
 				_lock.lockWrite();
 				_paths[agent->_id] = path;
 				_lock.releaseWrite();

src/Menge/MengeCore/BFSM/VelocityComponents/VelCompRoadMap.h

@@ -126,6 +126,7 @@ namespace Menge {
 			 *	@param		agent		The agent for which a preferred velocity is computed.
 			 *	@param		goal		The agent's goal (although this may be ignored).
 			 *	@param		pVel		The instance of Agents::PrefVelocity to set.
+       *  @throws   VelCompFatalException if a path cannot be found from agent position to goal.
 			 */
 			virtual void setPrefVelocity( const Agents::BaseAgent * agent, const Goal * goal,
 										  Agents::PrefVelocity & pVel ) const;

src/Menge/MengeCore/resources/Graph.cpp

@@ -195,10 +195,16 @@ namespace Menge {
 
 	RoadMapPath * Graph::getPath( const Agents::BaseAgent * agent, const BFSM::Goal * goal ) {
 		// Find the closest visible node to agent position
-		size_t startID = getClosestVertex( agent->_pos, agent->_radius );
+		size_t startID = getClosestVertex( agent->_pos, agent->_radius, Clearance::Partial );
 		// Find the closest visible node to goal position
 		Vector2 goalPos = goal->getCentroid();
-		size_t endID = getClosestVertex( goalPos, agent->_radius );
+    // TODO(curds01): Investigate finding a path to the goal *area* rather than centroid; more
+    // difficult, but provides the possibility to get more efficient paths. Not just efficient, but
+    // may also eliminate false negatives. See https://github.com/MengeCrowdSim/Menge/issues/121.
+		size_t endID = getClosestVertex( goalPos, agent->_radius, Clearance::Full );
+    if (startID == -1 || endID == -1) {
+      return nullptr;
+    }
 		// Compute the path based on those nodes
 		RoadMapPath * path = getPath( startID, endID );
 		if ( path ) { 
@@ -216,24 +222,24 @@ namespace Menge {
 
 	//////////////////////////////////////////////////////////////////////////////////////
 
-	size_t Graph::getClosestVertex( const Vector2 & point, float radius ) {
+	size_t Graph::getClosestVertex( const Vector2 & point, float radius, Clearance clearance) {
 		assert( _vCount > 0 && "Trying to operate on an empty roadmap" );
-		// TODO: Make this faster
+		// TODO(curds01): Make this faster via a spatial hash - in other words, test them in the order
+    // of closest to farthest.
 
 		float bestDistSq = INFTY;
 		size_t bestID = -1;
 		for ( size_t i = 0; i < _vCount; ++i ) {
 			float testDistSq = absSq( _vertices[i].getPosition() - point );
 			if ( testDistSq < bestDistSq ) {
-				if ( Menge::SPATIAL_QUERY->queryVisibility( point, _vertices[i].getPosition(),
-															radius ) ) {
+				if ((clearance == Clearance::Full &&
+             Menge::SPATIAL_QUERY->queryVisibility(point, _vertices[i].getPosition(), radius)) ||
+            Menge::SPATIAL_QUERY->linkIsTraversible(point, _vertices[i].getPosition(), radius)) {
 					bestDistSq = testDistSq;
 					bestID = i;
 				}
 			}
 		}
-
-		assert( bestID != -1 && "Roadmap Graph was unable to find a visible vertex" );
 
 		return bestID;
 	}

src/Menge/MengeCore/resources/Graph.h

@@ -98,7 +98,7 @@ namespace Menge {
 		 *	@param		agent		The agent for whom to compute the path.
 		 *	@param		goal		The agent's goal.
 		 *	@returns	A pointer to a RoadMapPath.  If there is an error,
-		 *				the poitner will be NULL.
+		 *				the pointer will be NULL.
 		 */
 		RoadMapPath * getPath( const Agents::BaseAgent * agent, const BFSM::Goal * goal );
 
@@ -127,16 +127,22 @@ namespace Menge {
 		static const std::string LABEL;
 
 	protected:
+    /** Definition of the amount of clearance required in connecting a vertex to the graph.  */
+    enum class Clearance {
+      Partial,  ///< Connection need only be traversible (see SpatialQuery::linkIsTraversible()).
+      Full      ///< Connection must be fully _visible_ (see SpatialQuery::queryVisibility()).
+    };
 
 		/*!
 		 *	@brief		Find the closest visible graph vertex to the given
 		 *				point.
 		 *
-		 *	@param		point		The point to connect to the graph.
-		 *	@param		radius		The radius of the agent testing.
-		 *	@returns	The index of the closest node.
+		 *	@param		point		   The point to connect to the graph.
+		 *	@param		radius		 The radius of the agent testing.
+     *  @param    clearance  The type of clearance required for connecting point to the graph.
+		 *	@returns	The index of the closest node (-1 if no node can be connected).
 		 */
-		size_t getClosestVertex( const Vector2 & point, float radius );
+		size_t getClosestVertex( const Vector2 & point, float radius, Clearance clearance);
 
 		/*!
 		 *	@brief		Computes the shortest path from start to end vertices.

src/Menge/MengeCore/resources/RoadMapPath.cpp

@@ -92,24 +92,30 @@ namespace Menge {
 		// TODO: Should I compute this blindly?  Although it is used in potentially three places
 		//		mostly, it won't be used.
 		Vector2 target = _goal->getTargetPoint( agent->_pos, agent->_radius );
+
+    // First confirm I can still see the point I'm headed toward.
 		if ( _targetID < _wayPointCount ) {
 			isVisible =
-				Menge::SPATIAL_QUERY->queryVisibility( agent->_pos, _wayPoints[ _targetID ],
+				Menge::SPATIAL_QUERY->linkIsTraversible( agent->_pos, _wayPoints[ _targetID ],
 				agent->_radius );
 		} else {
-			isVisible = Menge::SPATIAL_QUERY->queryVisibility( agent->_pos, target,
+			isVisible = Menge::SPATIAL_QUERY->linkIsTraversible( agent->_pos, target,
 															   agent->_radius );
 		}
+
+    // See if I can't advance my current waypoint to one further along my path.
 		size_t testID = _targetID + 1;
+    // TODO(curds01): Should I be advancing the counter if the current isn't visible? If so, justify
+    // this.
 		while ( testID < _wayPointCount &&
-				Menge::SPATIAL_QUERY->queryVisibility( agent->_pos, _wayPoints[ testID ],
+				Menge::SPATIAL_QUERY->linkIsTraversible( agent->_pos, _wayPoints[ testID ],
 				agent->_radius ) ) {
 			_targetID = testID;
 			isVisible = true;
 			++testID;
 		}
 		if ( _targetID == _wayPointCount - 1 ) {
-			if ( Menge::SPATIAL_QUERY->queryVisibility( agent->_pos, target, agent->_radius ) ) {
+			if ( Menge::SPATIAL_QUERY->linkIsTraversible( agent->_pos, target, agent->_radius ) ) {
 				++_targetID;
 				isVisible = true;
 			}
@@ -122,7 +128,7 @@ namespace Menge {
 			_validPos = agent->_pos;
 			pVel.setTarget( curr );
 		} else {
-			if (Menge::SPATIAL_QUERY->queryVisibility(agent->_pos, _validPos, agent->_radius)) {
+			if (Menge::SPATIAL_QUERY->linkIsTraversible(agent->_pos, _validPos, agent->_radius)) {
 				// This should never be the zero vector.
 				//	_validPos is set when the current waypoint is visible
 				//  this code is only achieved when it is NOT visible