Implement multibody joints and the new solver

src/dynamics/joint/multibody_joint/multibody_workspace.rs

@@ -0,0 +1,27 @@
+use crate::dynamics::RigidBodyVelocity;
+use crate::math::Real;
+use na::DVector;
+
+/// A temporary workspace for various updates of the multibody.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone)]
+pub(crate) struct MultibodyWorkspace {
+    pub accs: Vec<RigidBodyVelocity>,
+    pub ndofs_vec: DVector<Real>,
+}
+
+impl MultibodyWorkspace {
+    /// Create an empty workspace.
+    pub fn new() -> Self {
+        MultibodyWorkspace {
+            accs: Vec::new(),
+            ndofs_vec: DVector::zeros(0),
+        }
+    }
+
+    /// Resize the workspace so it is enough for `nlinks` links.
+    pub fn resize(&mut self, nlinks: usize, ndofs: usize) {
+        self.accs.resize(nlinks, RigidBodyVelocity::zero());
+        self.ndofs_vec = DVector::zeros(ndofs)
+    }
+}