Implementing constraint solver
Fix bug in BodyNode::updateVelocity() and BodyNode::updateAcceleration()
Implementing constraint solver