From what I can gather, your AI has a test to see if it will collide with something. The result of this test appears to be assigned to the variable alternativeWaypointer
allowing for the following if statement:
if(!alternativeWaypointer){
relativePosition = computePosition(trajectory);
}else{
relativePosition = computePosition(adjustTrajectory(trajectory));
}
It's possible that your adjust trajectory logic isn't working. Perhaps this can be tested? In lieu of this, a likely problem is that your adjusted trajectory isn't sufficiently adjusted to avoid the collision. We can solve this by creating a series of hypothetical trajectories and then picking the best candidate.
alternativeWaypointer = isCollisionLikely(trajectory);
if(!alternativeWaypointer) {
relativePosition = computePosition(trajectory);
} else {
/* generate alternative trajectories, adjustments range from minimal to extreme */
List<Trajectory> alternatives = adjustTrajectoryList(trajectory, MINIMAL, EXTREME);
/* Of these, pick one and run with it */
Trajectory preferred = leastLikelyCollisionTrajectory(alternatives);
relativePosition = computePosition(preferred);
}
You could use your existing logic to pick a preferred trajectory. It won't be the most sophisticated thing in the world, but it would be a starting point:
Trajectory leastLikelyCollistionTrajectory(List<Trajectory> options) {
/* iterate from minimal adjustment to extreme */
foreach(Trajectory t in options) {
if(!isCollisionLikely(t)) {
return t; /* return the first trajectory that seems ok */
}
}
/* otherwise resort to the most extreme and hope */
return options.last();
}
I've mixed pseudo code with a bit of java in an attempt to make it clearer. Does this look like it would solve your problem?