Some further performance improvements to the PointInPlaneWithStiction constraint. I added new utility methods to ConstraintImpl that perform combined location, velocity, and acceleration operations to avoid duplicate work. Then I used these in the methods of PointInPlaneWithStiction that affect PGS.
Fix some debug message problems.
Tighten up inner loops of PGS solver by eliminating element access overhead.