/**
* Add a spline to the path
*
* @param wFrame
* @param path
* @param pfi
*/
public void addAHermiteSpline(
final SimpleWorldFrame wFrame, final Path path, final PathFinderInput pfi) {
TrackedTigerBot bot = wFrame.getBot(pfi.getBotId());
float dstOrient = pfi.getDstOrient();
List<IVector2> mPath = new ArrayList<IVector2>();
mPath.add(DistanceUnit.MILLIMETERS.toMeters(bot.getPos()));
for (IVector2 p : path.getPath()) {
mPath.add(DistanceUnit.MILLIMETERS.toMeters(p));
}
if (pfi.getMoveCon().getSpeed() > 0) {
trajGeneratorMove.setPositionTrajParams(
pfi.getMoveCon().getSpeed(), Sisyphus.maxLinearAcceleration);
} else {
trajGeneratorMove.setPositionTrajParams(
Sisyphus.maxLinearVelocity, Sisyphus.maxLinearAcceleration);
}
trajGeneratorMove.setRotationTrajParams(maxRotateVelocity, Sisyphus.maxRotateAcceleration);
trajGeneratorMove.setSplineParams(normalAngleToSpeed);
trajGeneratorMove.setReducePathScore(pathReductionScore);
SplinePair3D trajectories =
trajGeneratorMove.create(
mPath,
bot.getVel(),
pfi.getMoveCon().getVelAtDestination(),
bot.getAngle(),
dstOrient,
bot.getaVel(),
0);
path.setHermiteSpline(trajectories);
trajectories.setStartTime(System.nanoTime());
}
@Override
public void onNewPath(final Path path) {
currentPaths.put(path.getBotID(), path);
Integer counter = newPathCounter.get(path.getBotID());
if (counter == null) {
counter = 0;
}
newPathCounter.put(path.getBotID(), counter + 1);
notifyNewPath(path);
}
/**
* calculates a spline for a bot according a given movement condition
*
* @param botId
* @param worldFrame
* @param moveCon
* @param pathPlanningParams
* @return
*/
public ISpline calculateSpline(
final BotID botId,
final SimpleWorldFrame worldFrame,
final MovementCon moveCon,
final TuneableParameter pathPlanningParams) {
Map<BotID, Path> paths = new HashMap<BotID, Path>();
PathFinderInput pfi = new PathFinderInput(botId, paths, 0f, moveCon);
pfi.update(worldFrame);
IPathFinder pathFinder = new ERRTPlanner_WPC();
if (pathPlanningParams != null) {
((ERRTPlanner_WPC) pathFinder).setAdjustableParams(pathPlanningParams);
}
Path path = pathFinder.calcPath(pfi);
addAHermiteSpline(worldFrame, path, pfi);
return path.getSpline();
}
private void notifyPotentialNewPath(final Path path) {
synchronized (observers) {
IPathConsumer pathConsumer = observers.get(path.getBotID());
if (pathConsumer != null) {
pathConsumer.onPotentialNewPath(path);
}
}
}
@Override
public void onPotentialNewPath(final Path path) {
latestPaths.put(path.getBotID(), path);
notifyPotentialNewPath(path);
}