| 341 | // Integrate bodies |
| 342 | |
| 343 | void |
| 344 | dInternalStepFast (dxWorld * world, dxBody * body[2], dReal * GI[2], dReal * GinvI[2], dxJoint * joint, dxJoint::Info1 info, dxJoint::Info2 Jinfo, dReal stepsize) |
| 345 | { |
| 346 | int i, j, k; |
| 347 | # ifdef TIMING |
| 348 | dTimerNow ("constraint preprocessing"); |
| 349 | # endif |
| 350 | |
| 351 | dReal stepsize1 = dRecip (stepsize); |
| 352 | |
| 353 | int m = info.m; |
| 354 | // nothing to do if no constraints. |
| 355 | if (m <= 0) |
| 356 | return; |
| 357 | |
| 358 | int nub = 0; |
| 359 | if (info.nub == info.m) |
| 360 | nub = m; |
| 361 | |
| 362 | // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same |
| 363 | // format as J so we just go through the constraints in J multiplying by |
| 364 | // the appropriate scalars and matrices. |
| 365 | # ifdef TIMING |
| 366 | dTimerNow ("compute A"); |
| 367 | # endif |
| 368 | dReal JinvM[2 * 6 * 8]; |
| 369 | //dSetZero (JinvM, 2 * m * 8); |
| 370 | |
| 371 | dReal *Jsrc = Jinfo.J1l; |
| 372 | dReal *Jdst = JinvM; |
| 373 | if (body[0]) |
| 374 | { |
| 375 | for (j = m - 1; j >= 0; j--) |
| 376 | { |
| 377 | for (k = 0; k < 3; k++) |
| 378 | Jdst[k] = Jsrc[k] * body[0]->invMass; |
| 379 | dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[0]); |
| 380 | Jsrc += 8; |
| 381 | Jdst += 8; |
| 382 | } |
| 383 | } |
| 384 | if (body[1]) |
| 385 | { |
| 386 | Jsrc = Jinfo.J2l; |
| 387 | Jdst = JinvM + 8 * m; |
| 388 | for (j = m - 1; j >= 0; j--) |
| 389 | { |
| 390 | for (k = 0; k < 3; k++) |
| 391 | Jdst[k] = Jsrc[k] * body[1]->invMass; |
| 392 | dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[1]); |
| 393 | Jsrc += 8; |
| 394 | Jdst += 8; |
| 395 | } |
| 396 | } |
| 397 | |
| 398 | |
| 399 | // now compute A = JinvM * J'. |
| 400 | int mskip = dPAD (m); |
no test coverage detected