vertex.setIncludingFrame(projectionArea.getReferenceFrame(), projectionArea.getVertex(i));
finalICPToICPVector.setToZero(projectionArea.getReferenceFrame());
finalICPToICPVector.sub(capturePoint, finalCapturePoint);
icpToCandidateVector.setToZero(projectionArea.getReferenceFrame());
icpToCandidateVector.sub(vertex, capturePoint);
<BUG>double newAngle = icpToCandidateVector.angle(finalICPToICPVector);
</BUG>
if (newAngle < angle)
{
angle = newAngle;

vertex.setIncludingFrame(projectionArea.getReferenceFrame(), projectionArea.getVertex(i));
finalICPToICPVector.setToZero(projectionArea.getReferenceFrame());
finalICPToICPVector.sub(capturePoint, finalCapturePoint);
icpToCandidateVector.setToZero(projectionArea.getReferenceFrame());
icpToCandidateVector.sub(vertex, capturePoint);
double newAngle = Math.abs(icpToCandidateVector.angle(finalICPToICPVector));
if (newAngle < angle)
{
angle = newAngle;
