working revamp of RigidBody and RigidBodySolver with some concepts off of jsdyn