I just found the Branch & Bound source code in the HLSDK: (I'm not quite sure though, wonder if this is Dynamic Plan or Branch & Bound)
PHP Code:
//=========================================================
// CGraph - FindShortestPath
//
// accepts a capability mask (afCapMask), and will only
// find a path usable by a monster with those capabilities
// returns the number of nodes copied into supplied array
//=========================================================
int CGraph :: FindShortestPath ( int *piPath, int iStart, int iDest, int iHull, int afCapMask)
{
int iVisitNode;
int iCurrentNode;
int iNumPathNodes;
int iHullMask;
if ( !m_fGraphPresent || !m_fGraphPointersSet )
{// protect us in the case that the node graph isn't available or built
ALERT ( at_aiconsole, "Graph not ready!\n" );
return FALSE;
}
if ( iStart < 0 || iStart > m_cNodes )
{// The start node is bad?
ALERT ( at_aiconsole, "Can't build a path, iStart is %d!\n", iStart );
return FALSE;
}
if (iStart == iDest)
{
piPath[0] = iStart;
piPath[1] = iDest;
return 2;
}
// Is routing information present.
//
if (m_fRoutingComplete)
{
int iCap = CapIndex( afCapMask );
iNumPathNodes = 0;
piPath[iNumPathNodes++] = iStart;
iCurrentNode = iStart;
int iNext;
//ALERT(at_aiconsole, "GOAL: %d to %d\n", iStart, iDest);
// Until we arrive at the destination
//
while (iCurrentNode != iDest)
{
iNext = NextNodeInRoute( iCurrentNode, iDest, iHull, iCap );
if (iCurrentNode == iNext)
{
//ALERT(at_aiconsole, "SVD: Can't get there from here..\n");
return 0;
break;
}
if (iNumPathNodes >= MAX_PATH_SIZE)
{
//ALERT(at_aiconsole, "SVD: Don't return the entire path.\n");
break;
}
piPath[iNumPathNodes++] = iNext;
iCurrentNode = iNext;
}
//ALERT( at_aiconsole, "SVD: Path with %d nodes.\n", iNumPathNodes);
}
else
{
CQueuePriority queue;
switch( iHull )
{
case NODE_SMALL_HULL:
iHullMask = bits_LINK_SMALL_HULL;
break;
case NODE_HUMAN_HULL:
iHullMask = bits_LINK_HUMAN_HULL;
break;
case NODE_LARGE_HULL:
iHullMask = bits_LINK_LARGE_HULL;
break;
case NODE_FLY_HULL:
iHullMask = bits_LINK_FLY_HULL;
break;
}
// Mark all the nodes as unvisited.
//
for ( int i = 0; i < m_cNodes; i++)
{
m_pNodes[ i ].m_flClosestSoFar = -1.0;
}
m_pNodes[ iStart ].m_flClosestSoFar = 0.0;
m_pNodes[ iStart ].m_iPreviousNode = iStart;// tag this as the origin node
queue.Insert( iStart, 0.0 );// insert start node
while ( !queue.Empty() )
{
// now pull a node out of the queue
float flCurrentDistance;
iCurrentNode = queue.Remove(flCurrentDistance);
// For straight-line weights, the following Shortcut works. For arbitrary weights,
// it doesn't.
//
if (iCurrentNode == iDest) break;
CNode *pCurrentNode = &m_pNodes[ iCurrentNode ];
for ( i = 0 ; i < pCurrentNode->m_cNumLinks ; i++ )
{// run through all of this node's neighbors
iVisitNode = INodeLink ( iCurrentNode, i );
if ( ( m_pLinkPool[ m_pNodes[ iCurrentNode ].m_iFirstLink + i ].m_afLinkInfo & iHullMask ) != iHullMask )
{// monster is too large to walk this connection
//ALERT ( at_aiconsole, "fat ass %d/%d\n",m_pLinkPool[ m_pNodes[ iCurrentNode ].m_iFirstLink + i ].m_afLinkInfo, iMonsterHull );
continue;
}
// check the connection from the current node to the node we're about to mark visited and push into the queue
if ( m_pLinkPool[ m_pNodes[ iCurrentNode ].m_iFirstLink + i ].m_pLinkEnt != NULL )
{// there's a brush ent in the way! Don't mark this node or put it into the queue unless the monster can negotiate it
if ( !HandleLinkEnt ( iCurrentNode, m_pLinkPool[ m_pNodes[ iCurrentNode ].m_iFirstLink + i ].m_pLinkEnt, afCapMask, NODEGRAPH_STATIC ) )
{// monster should not try to go this way.
continue;
}
}
float flOurDistance = flCurrentDistance + m_pLinkPool[ m_pNodes[ iCurrentNode ].m_iFirstLink + i].m_flWeight;
if ( m_pNodes[ iVisitNode ].m_flClosestSoFar < -0.5
|| flOurDistance < m_pNodes[ iVisitNode ].m_flClosestSoFar - 0.001 )
{
m_pNodes[iVisitNode].m_flClosestSoFar = flOurDistance;
m_pNodes[iVisitNode].m_iPreviousNode = iCurrentNode;
queue.Insert ( iVisitNode, flOurDistance );
}
}
}
if ( m_pNodes[iDest].m_flClosestSoFar < -0.5 )
{// Destination is unreachable, no path found.
return 0;
}
// the queue is not empty
// now we must walk backwards through the m_iPreviousNode field, and count how many connections there are in the path
iCurrentNode = iDest;
iNumPathNodes = 1;// count the dest
while ( iCurrentNode != iStart )
{
iNumPathNodes++;
iCurrentNode = m_pNodes[ iCurrentNode ].m_iPreviousNode;
}
iCurrentNode = iDest;
for ( i = iNumPathNodes - 1 ; i >= 0 ; i-- )
{
piPath[ i ] = iCurrentNode;
iCurrentNode = m_pNodes [ iCurrentNode ].m_iPreviousNode;
}
}
#if 0
if (m_fRoutingComplete)
{
// This will draw the entire path that was generated for the monster.
for ( int i = 0 ; i < iNumPathNodes - 1 ; i++ )
{
MESSAGE_BEGIN( MSG_BROADCAST, SVC_TEMPENTITY );
WRITE_BYTE( TE_SHOWLINE);
WRITE_COORD( m_pNodes[ piPath[ i ] ].m_vecOrigin.x );
WRITE_COORD( m_pNodes[ piPath[ i ] ].m_vecOrigin.y );
WRITE_COORD( m_pNodes[ piPath[ i ] ].m_vecOrigin.z + NODE_HEIGHT );
WRITE_COORD( m_pNodes[ piPath[ i + 1 ] ].m_vecOrigin.x );
WRITE_COORD( m_pNodes[ piPath[ i + 1 ] ].m_vecOrigin.y );
WRITE_COORD( m_pNodes[ piPath[ i + 1 ] ].m_vecOrigin.z + NODE_HEIGHT );
MESSAGE_END();
}
}
#endif
#if 0 // MAZE map
MESSAGE_BEGIN( MSG_BROADCAST, SVC_TEMPENTITY );
WRITE_BYTE( TE_SHOWLINE);
WRITE_COORD( m_pNodes[ 4 ].m_vecOrigin.x );
WRITE_COORD( m_pNodes[ 4 ].m_vecOrigin.y );
WRITE_COORD( m_pNodes[ 4 ].m_vecOrigin.z + NODE_HEIGHT );
WRITE_COORD( m_pNodes[ 9 ].m_vecOrigin.x );
WRITE_COORD( m_pNodes[ 9 ].m_vecOrigin.y );
WRITE_COORD( m_pNodes[ 9 ].m_vecOrigin.z + NODE_HEIGHT );
MESSAGE_END();
#endif
return iNumPathNodes;
}
(note that you can't rip this code to your program as it's owned by valve unless it's a HL bot or so, this is NOT "open source"; but you can learn ideas from it)
[edit]
well this isn't BB because of this one...
PHP Code:
float flOurDistance = flCurrentDistance + m_pLinkPool[ m_pNodes[ iCurrentNode ].m_iFirstLink + i].m_flWeight;
if ( m_pNodes[ iVisitNode ].m_flClosestSoFar < -0.5
|| flOurDistance < m_pNodes[ iVisitNode ].m_flClosestSoFar - 0.001 )
but remove these lines it will be BB:
PHP Code:
float flOurDistance = flCurrentDistance + m_pLinkPool[ m_pNodes[ iCurrentNode ].m_iFirstLink + i].m_flWeight;
if ( m_pNodes[ iVisitNode ].m_flClosestSoFar < -0.5
|| flOurDistance < m_pNodes[ iVisitNode ].m_flClosestSoFar - 0.001 )
{
m_pNodes[iVisitNode].m_flClosestSoFar = flOurDistance;
[/edit]