Waypoint *psPathNetwork::FindNearestWaypoint(int group, csVector3& v,iSector *sector, float range, float * found_range)
{
csList<Waypoint*>::Iterator iter(waypointGroups[group]);
Waypoint *wp;
float min_range = range;
Waypoint *min_wp = NULL;
while (iter.HasNext())
{
wp = iter.Next();
float dist2 = world->Distance(v,sector,wp->loc.pos,wp->GetSector(engine));
if (min_range < 0 || dist2 < min_range)
{
min_range = dist2;
min_wp = wp;
}
}
if (min_wp && found_range) *found_range = min_range;
return min_wp;
}
Waypoint *psPathNetwork::FindRandomWaypoint(int group, csVector3& v,iSector *sector, float range, float * found_range)
{
csList<Waypoint*>::Iterator iter(waypointGroups[group]);
Waypoint *wp;
csArray<Waypoint*> nearby;
csArray<float> dist;
while (iter.HasNext())
{
wp = iter.Next();
float dist2 = world->Distance(v,sector,wp->loc.pos,wp->GetSector(engine));
if (range < 0 || dist2 < range)
{
nearby.Push(wp);
dist.Push(dist2);
}
}
if (nearby.GetSize()>0) // found one or more closer than range
{
size_t pick = psGetRandom((uint32)nearby.GetSize());
if (found_range) *found_range = sqrt(dist[pick]);
return nearby[pick];
}
return NULL;
}
Waypoint *psPathNetwork::FindWaypoint(csVector3& v, iSector *sector)
{
csPDelArray<Waypoint>::Iterator iter(waypoints.GetIterator());
while (iter.HasNext())
{
Waypoint *wp = iter.Next();
float dist = world->Distance(v,sector,wp->loc.pos,wp->GetSector(engine));
if (dist < wp->loc.radius)
{
return wp;
}
}
return NULL;
}
size_t psPathNetwork::FindWaypointsInSector(iSector *sector, csList<Waypoint*>& list)
{
size_t count = 0;
csPDelArray<Waypoint>::Iterator iter(waypoints.GetIterator());
Waypoint *wp;
// Initialize
while (iter.HasNext())
{
wp = iter.Next();
if ( wp->GetSector(engine) == sector )
{
list.PushBack(wp);
count++;
}
}
return count;
}