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; }