BallJoint and LockJoint works. Fixed construction of shur complement and other math related stuff