chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Refactor a loop in engine_solver.c

Browse Source 
PiperOrigin-RevId: 560997065
Change-Id: I26540e4f750eb18d9497bf35396adbb15cc3a122
This commit is contained in:
Yuval Tassa
2023-08-29 05:15:58 -07:00
committed by Copybara-Service
parent 55173e58f9
commit 5f3e22a45f
1 changed files with 32 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_solver.c
+7 -3
View File
@@ -929,13 +929,17 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) {
mjtNum quadTotal[3]; mjtNum quadTotal[3];
mju_copy3(quadTotal, ctx->quadGauss); mju_copy3(quadTotal, ctx->quadGauss);
// process constraints
for (int i=0; i < nefc; i++) {
// equality // equality
for (int i=0; i < ne; i++) { if (i < ne) {
mju_addTo3(quadTotal, ctx->quad+3*i); mju_addTo3(quadTotal, ctx->quad+3*i);
continue;
} }
// friction // friction
for (int i=ne; i < ne+nf; i++) { if (i < ne + nf) {
// search point, friction loss, bound (Rf) // search point, friction loss, bound (Rf)
mjtNum start = ctx->Jaref[i], dir = ctx->Jv[i]; mjtNum start = ctx->Jaref[i], dir = ctx->Jv[i];
mjtNum x = start + alpha*dir; mjtNum x = start + alpha*dir;
@@ -958,10 +962,10 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) {
mjtNum qf[3] = {f*(-0.5*Rf+start), f*dir, 0}; mjtNum qf[3] = {f*(-0.5*Rf+start), f*dir, 0};
mju_addTo3(quadTotal, qf); mju_addTo3(quadTotal, qf);
} }
continue;
} }
// limit and contact // limit and contact
for (int i=ne+nf; i < nefc; i++) {
if (d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) { // elliptic cone if (d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) { // elliptic cone
// extract contact info // extract contact info
mjContact* con = d->contact + d->efc_id[i]; mjContact* con = d->contact + d->efc_id[i];