void IterateDynamicsFunctional3D::processGenericBlocks (
Box3D domain, std::vector<AtomicBlock3D*> blocks )
{
PLB_PRECONDITION( blocks.size()==1 );
AtomicContainerBlock3D& container = *dynamic_cast<AtomicContainerBlock3D*>(blocks[0]);
StoreDynamicsID* storeId = dynamic_cast<StoreDynamicsID*>(container.getData());
std::vector<int> nextIDs(previousMaximum.size());
for (pluint i=0; i<nextIDs.size(); ++i) {
nextIDs[i] = -1;
}
if (!storeId->empty()) {
nextIDs = storeId->getCurrent();
util::extendVectorSize(nextIDs, previousMaximum.size());
if (vectorEquals(nextIDs,previousMaximum)) {
nextIDs = storeId->iterate();
util::extendVectorSize(nextIDs, previousMaximum.size());
}
}
for (pluint i=0; i<nextIDs.size(); ++i) {
this->getStatistics().gatherMax(maxIds[i], (double)nextIDs[i]);
}
}
void pathfinder( const BNavmesh *navmesh, const BVector *start, const BVector *end, BPath *outPath )
{
assert( outPath->numVertices == 0 );
const BTriangle *const startTriangle = findTriangleContainingPoint( navmesh, start );
const BTriangle *const endTriangle = findTriangleContainingPoint( navmesh, end );
const BTriangle *adjustedStartTriangle, *adjustedEndTriangle;
BVector adjustedStart, adjustedEnd;
if ( startTriangle == NULL )
{
projectPointOntoNavmesh( navmesh, start, &adjustedStartTriangle, &adjustedStart );
}
else
{
adjustedStart = *start;
adjustedStartTriangle = startTriangle;
}
assert( adjustedStartTriangle );
const int isDestinationReachable = endTriangle != NULL && endTriangle->connectedComponent == adjustedStartTriangle->connectedComponent;
if ( !isDestinationReachable )
{
projectPointOntoConnectedComponent( navmesh, end, adjustedStartTriangle->connectedComponent, &adjustedEndTriangle, &adjustedEnd );
}
else
{
adjustedEnd = *end;
adjustedEndTriangle = endTriangle;
}
assert( adjustedEndTriangle );
assert( isPointInsideNavmeshTriangle( navmesh, &adjustedStart, adjustedStartTriangle ) );
assert( isPointInsideNavmeshTriangle( navmesh, &adjustedEnd, adjustedEndTriangle ) );
BAStarOutput aStarOutput;
aStar( navmesh, adjustedStartTriangle, adjustedEndTriangle, &adjustedEnd, &aStarOutput );
funnelPath( navmesh, &adjustedStart, &adjustedEnd, &aStarOutput, outPath );
freeAStarOutput( &aStarOutput );
assert( outPath->numVertices > 0 );
assert( outPath->vertices != NULL );
if ( !vectorEquals( start, &adjustedStart ) )
{
BVector *newVertices = malloc( ( 1 + outPath->numVertices ) * sizeof( BVector ) );
newVertices[0] = *start;
memcpy( &newVertices[1], outPath->vertices, ( outPath->numVertices ) * sizeof( BVector ) );
free( outPath->vertices );
outPath->vertices = newVertices;
outPath->numVertices++;
}
if ( isDestinationReachable && !vectorEquals( end, &adjustedEnd ) )
{
BVector *newVertices = malloc( ( 1 + outPath->numVertices ) * sizeof( BVector ) );
newVertices[outPath->numVertices] = *end;
memcpy( newVertices, outPath->vertices, ( outPath->numVertices ) * sizeof( BVector ) );
free( outPath->vertices );
outPath->vertices = newVertices;
outPath->numVertices++;
}
}
// Based on: http://digestingduck.blogspot.com/2010/03/simple-stupid-funnel-algorithm.html
static void funnelPath( const BNavmesh *navmesh, const BVector *start, const BVector *end, const BAStarOutput *aStarOutput, BPath *outPath )
{
const int numTriangles = aStarOutput->numTriangles;
const BTriangle **triangles = aStarOutput->triangles;
if ( numTriangles < 1 )
{
outPath->numVertices = 0;
outPath->vertices = NULL;
return;
}
if ( numTriangles == 1 )
{
outPath->numVertices = 2;
outPath->vertices = malloc( 2 * sizeof( BVector ) );
outPath->vertices[0] = *start;
outPath->vertices[1] = *end;
return;
}
assert( isPointInsideNavmeshTriangle( navmesh, start, triangles[0] ) );
assert( isPointInsideNavmeshTriangle( navmesh, end, triangles[numTriangles - 1] ) );
const int numPortals = numTriangles;
BEdge *portals = malloc( numPortals * sizeof( BEdge ) );
const int maxVertices = numTriangles + 1;
outPath->vertices = malloc( sizeof( BVector ) * maxVertices );
outPath->vertices[0] = *start;
outPath->numVertices = 1;
for ( int triangleIndex = 0; triangleIndex < numTriangles - 1; triangleIndex++ )
{
assert( triangleIndex < numTriangles );
assert( triangleIndex < numPortals - 1 );
assert( triangleIndex + 1 < numTriangles );
getPortal( navmesh, triangles[triangleIndex], triangles[triangleIndex + 1], &portals[triangleIndex] );
}
portals[numPortals - 1].start = *end;
portals[numPortals - 1].end = *end;
BVector portalApex, portalLeft, portalRight;
portalApex = *start;
portalLeft = *start;
portalRight = *start;
int apexIndex, leftIndex, rightIndex;
apexIndex = 0;
leftIndex = 0;
rightIndex = 0;
for ( int portalIndex = 0; portalIndex < numPortals; portalIndex++ )
{
const BVector *right = &portals[portalIndex].start;
const BVector *left = &portals[portalIndex].end;
if ( triangleArea2( &portalApex, &portalRight, right ) <= 0 )
{
if ( vectorEquals( &portalApex, &portalRight ) || triangleArea2( &portalApex, &portalLeft, right ) > 0 )
{
// Tighten
portalRight = *right;
rightIndex = portalIndex;
}
else
{
// Right over left, insert left to path and restart scan from portal left point
assert( outPath->numVertices < maxVertices );
if ( !vectorEquals( &portalLeft, &outPath->vertices[outPath->numVertices - 1] ) )
{
outPath->vertices[outPath->numVertices] = portalLeft;
outPath->numVertices++;
}
// Make current left the new apex
portalApex = portalLeft;
apexIndex = leftIndex;
// Reset portal
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
// Restart scan
portalIndex = apexIndex;
continue;
}
}
if ( triangleArea2( &portalApex, &portalLeft, left ) >= 0 )
{
if ( vectorEquals( &portalApex, &portalLeft ) || triangleArea2( &portalApex, &portalRight, left ) < 0 )
{
// Tighten
portalLeft = *left;
leftIndex = portalIndex;
}
else
{
// Left over right, insert right to path and restart scan from portal right point
assert( outPath->numVertices < maxVertices );
if ( !vectorEquals( &portalRight, &outPath->vertices[outPath->numVertices - 1] ) )
{
outPath->vertices[outPath->numVertices] = portalRight;
outPath->numVertices++;
}
// Make current right the new apex
portalApex = portalRight;
apexIndex = rightIndex;
// Reset portal
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
// Restart scan
portalIndex = apexIndex;
continue;
}
}
}
assert( vectorEquals( &outPath->vertices[0], start ) );
if ( !vectorEquals( &outPath->vertices[outPath->numVertices - 1], end ) )
{
assert( outPath->numVertices < maxVertices );
outPath->vertices[outPath->numVertices - 1] = *end;
outPath->numVertices++;
}
free( portals );
}
