Add private function mj_solveM_island for mass matrix solve with sub indices corresponding to one island.

PiperOrigin-RevId: 561622962 Change-Id: Ib43f03b3dd0c7456c57d4ba45a68e0708bf1c91c
2023-08-31 05:10:14 -07:00
parent ba5cf4d54d
commit 129abc6bca
3 changed files with 123 additions and 1 deletions
@@ -59,6 +59,9 @@ MJAPI void mj_solveLD(const mjModel* m, mjtNum* x, int n,
 // sparse backsubstitution:  x = inv(L'*D*L)*y, use factorization in d
 MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n);

+// sparse backsubstitution for one island:  x = inv(L'*D*L)*x, use factorization in d
+MJAPI void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* x, int island);
+
 // half of sparse backsubstitution:  x = sqrt(inv(D))*inv(L')*y
 MJAPI void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n);