void Resample(PointVector& points, size_t n)
{
double I = PathLength(points) / (n - 1); // interval length
double D = 0.0;
PointVector newpoints;
newpoints.push_back(points[0]);
for (size_t i = 1; i < points.size(); ++i)
{
double d = Distance(points[i - 1], points[i]);
if ((D + d) >= I)
{
double qx = points[i - 1]->X + ((I - D) / d) * (points[i]->X - points[i - 1]->X);
double qy = points[i - 1]->Y + ((I - D) / d) * (points[i]->Y - points[i - 1]->Y);
PointPtr q(new Point(qx, qy));
newpoints.push_back(q);
points.insert(points.begin()+i,q);
D = 0.0;
}
else D += d;
}
// somtimes we fall a rounding-error short of adding the last point, so add it if so
if (newpoints.size() == n - 1)
{
newpoints.push_back(points[points.size() - 1]);
}
points.clear();
points.insert(points.begin(), newpoints.begin(), newpoints.end());
}
void SetAVL::PathLength(int item, int &steps) {
// used to calculate the number of steps it takes to find item
// since steps is a reference, its value will be accumulated each
// call of the recursive PathLength method. This method assumes that
// steps has been initialized
PathLength(tree->getRoot(),item,steps);
}
void SetAVL::PathLength(TreeNode* node, int item, int &steps) {
// used to calculate the number of steps it takes to find item
// steps is increamented each recursive call this method
// traverses the tree similarly to the Find method of AVL.
// steps does not get modified if the node is NULL or
// if the nodes data is item
if (node == 0) {
return;
}
// otherwise traverse the tree
if (item < node->getData()) {
steps++;
PathLength(node->Left(), item, steps);
}
else if (item > node->getData()) {
steps++;
PathLength(node->Right(), item, steps);
}
else {
return;
}
}
void ArchiveReader::Write( DataRef data )
{
const uint8_t* start = data.Start();
const uint8_t* end = data.End();
while ( start != end )
{
switch ( m_state )
{
case STATE_PATH_ENCODING:
start = PathEncoding( start, end );
break;
case STATE_FILE_LENGTH_LENGTH:
start = FileLengthLength( start, end );
break;
case STATE_PATH_LENGTH:
start = PathLength( start, end );
break;
case STATE_PATH:
start = Path( start, end );
break;
case STATE_DATE_LENGTH:
start = DateLength( start, end );
break;
case STATE_DATE:
start = Date( start, end );
break;
case STATE_FILE_LENGTH:
start = FileLength( start, end );
break;
case STATE_FILE:
start = File( start, end );
break;
case STATE_DONE:
throw Error( "More data received after the end of the archive" );
}
}
}
Array* Resample(Array* points, int num)
{
Array* workingPoints = Array::createWithArray(points);
Array* newPoints = Array::create(points->getObjectAtIndex(0), NULL);
float I = PathLength(points) / (num -1);
float D = 0.0;
int i;
PointObject* p1;
PointObject* p2;
PointObject* newPoint;
for ( i=1; i<workingPoints->count(); i++ ) {
p1 = (PointObject*)workingPoints->getObjectAtIndex(i-1);
p2 = (PointObject*)workingPoints->getObjectAtIndex(i);
float d = Distance(p1, p2);
if ((D + d) >= I) {
float x = p1->x + ((I-D) / d) * (p2->x - p1->x);
float y = p1->y + ((I-D) / d) * (p2->y - p1->y);
newPoint = PointObject::create(x, y);
newPoints->addObject(newPoint);
workingPoints = Splice(workingPoints, newPoint, i);
D = 0.0;
} else {
D += d;
}
}
// rounding error handling
if ( newPoints->count() < num ) {
PointObject* finalValue = (PointObject*)points->getObjectAtIndex(points->count()-1);
for (int j = 0; j < (num-newPoints->count()); j++) {
newPoints->addObject(finalValue);
}
}
return newPoints;
}
/*
============
FindOptimalPath
Returns true if there is a path all the way to the goal.
============
*/
bool FindOptimalPath( const pathNode_t *root, const obstacle_t *obstacles, int numObstacles, const float height, const idVec3 &curDir, idVec3 &seekPos ) {
int i, numPathPoints, bestNumPathPoints;
const pathNode_t *node, *lastNode, *bestNode;
idVec2 optimizedPath[MAX_OBSTACLE_PATH];
float pathLength, bestPathLength;
bool pathToGoalExists, optimizedPathCalculated;
seekPos.Zero();
seekPos.z = height;
pathToGoalExists = false;
optimizedPathCalculated = false;
bestNode = root;
bestNumPathPoints = 0;
bestPathLength = idMath::INFINITY;
node = root;
while( node ) {
pathToGoalExists |= ( node->dist < 0.1f );
if ( node->dist <= bestNode->dist ) {
if ( idMath::Fabs( node->dist - bestNode->dist ) < 0.1f ) {
if ( !optimizedPathCalculated ) {
bestNumPathPoints = OptimizePath( root, bestNode, obstacles, numObstacles, optimizedPath );
bestPathLength = PathLength( optimizedPath, bestNumPathPoints, curDir.ToVec2() );
seekPos.ToVec2() = optimizedPath[1];
}
numPathPoints = OptimizePath( root, node, obstacles, numObstacles, optimizedPath );
pathLength = PathLength( optimizedPath, numPathPoints, curDir.ToVec2() );
if ( pathLength < bestPathLength ) {
bestNode = node;
bestNumPathPoints = numPathPoints;
bestPathLength = pathLength;
seekPos.ToVec2() = optimizedPath[1];
}
optimizedPathCalculated = true;
} else {
bestNode = node;
optimizedPathCalculated = false;
}
}
if ( node->children[0] ) {
node = node->children[0];
} else if ( node->children[1] ) {
node = node->children[1];
} else {
for ( lastNode = node, node = node->parent; node; lastNode = node, node = node->parent ) {
if ( node->children[1] && node->children[1] != lastNode ) {
node = node->children[1];
break;
}
}
}
}
if ( !pathToGoalExists ) {
seekPos.ToVec2() = root->children[0]->pos;
} else if ( !optimizedPathCalculated ) {
OptimizePath( root, bestNode, obstacles, numObstacles, optimizedPath );
seekPos.ToVec2() = optimizedPath[1];
}
if ( ai_showObstacleAvoidance.GetBool() ) {
idVec3 start, end;
start.z = end.z = height + 4.0f;
numPathPoints = OptimizePath( root, bestNode, obstacles, numObstacles, optimizedPath );
for ( i = 0; i < numPathPoints-1; i++ ) {
start.ToVec2() = optimizedPath[i];
end.ToVec2() = optimizedPath[i+1];
gameRenderWorld->DebugArrow( colorCyan, start, end, 1 );
}
}
return pathToGoalExists;
}
