Formatted rbdlsim.cc

src/rbdlsim.cc

@@ -299,7 +299,6 @@ void CalcImpulseVariables(
   const VectorNd& q = body->q;
   const VectorNd& qdot = body->qdot;
 
-
   // Calculate local coordinates of the contact point
   UpdateKinematicsCustom(*model, &q, nullptr, nullptr);
   Vector3d point_local_b =
@@ -311,21 +310,32 @@ void CalcImpulseVariables(
   *MInv = M.inverse();
 
   MatrixNd G_constr(MatrixNd::Zero(3, ndof));
-  CalcPointJacobian(
-      *model,
-      q,
-      body_index,
-      point_local_b,
-      G_constr,
-      false);
+  CalcPointJacobian(*model, q, body_index, point_local_b, G_constr, false);
   (*jac) = dir.transpose() * G_constr;
 
   *G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose();
 }
 
-void PrepareConstraintImpulse (SimBody* body_a, SimBody* body_b, CollisionInfo& cinfo) {
-  CalcImpulseVariables (body_a, cinfo.mBodyAIndex, cinfo.posA, cinfo.dir, &cinfo.MInvA, &cinfo.jacA, &cinfo.GMInvGTA);
-  CalcImpulseVariables (body_b, cinfo.mBodyBIndex, cinfo.posB, -cinfo.dir, &cinfo.MInvB, &cinfo.jacB, &cinfo.GMInvGTB);
+void PrepareConstraintImpulse(
+    SimBody* body_a,
+    SimBody* body_b,
+    CollisionInfo& cinfo) {
+  CalcImpulseVariables(
+      body_a,
+      cinfo.mBodyAIndex,
+      cinfo.posA,
+      cinfo.dir,
+      &cinfo.MInvA,
+      &cinfo.jacA,
+      &cinfo.GMInvGTA);
+  CalcImpulseVariables(
+      body_b,
+      cinfo.mBodyBIndex,
+      cinfo.posB,
+      -cinfo.dir,
+      &cinfo.MInvB,
+      &cinfo.jacB,
+      &cinfo.GMInvGTB);
 }
 
 void CalcConstraintImpulse(
@@ -358,21 +368,20 @@ void ApplyConstraintImpulse(
     SimBody* body,
     const MatrixNd& MInv,
     const VectorNd& jac,
-    double impulse
-) {
-
-}
+    double impulse) {}
 
 void ApplyConstraintImpulse(
     SimBody* body_a,
     SimBody* body_b,
     CollisionInfo& cinfo) {
   if (!body_a->mIsStatic) {
-    body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose() * (-cinfo.dir.transpose() * cinfo.accumImpulseB);
+    body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose()
+                    * (-cinfo.dir.transpose() * cinfo.accumImpulseB);
   }
 
   if (!body_b->mIsStatic) {
-    body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() * (-cinfo.dir.transpose() * cinfo.accumImpulseB);
+    body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose()
+                    * (-cinfo.dir.transpose() * cinfo.accumImpulseB);
   }
 }