Added force to fixedPoint constraint.

Moved application of rotation matrices to function.

Implemented predictedPosition function.

Added maximum iteration check and under-relaxation variable.