Switched pin constraints resolve function to work with body positions and angles manually instead of using vector operations, to prevent jitter and bodies not coming to rest