| 544 | } |
| 545 | |
| 546 | void |
| 547 | dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoint **joints, int nj, dReal stepsize, int maxiterations) |
| 548 | { |
| 549 | # ifdef TIMING |
| 550 | dTimerNow ("preprocessing"); |
| 551 | # endif |
| 552 | dxBody *bodyPair[2], *body; |
| 553 | dReal *GIPair[2], *GinvIPair[2]; |
| 554 | dxJoint *joint; |
| 555 | int iter, b, j, i; |
| 556 | dReal ministep = stepsize / maxiterations; |
| 557 | |
| 558 | // make a local copy of the joint array, because we might want to modify it. |
| 559 | // (the "dxJoint *const*" declaration says we're allowed to modify the joints |
| 560 | // but not the joint array, because the caller might need it unchanged). |
| 561 | //dxJoint **joints = (dxJoint **) ALLOCA (nj * sizeof (dxJoint *)); |
| 562 | //memcpy (joints, _joints, nj * sizeof (dxJoint *)); |
| 563 | |
| 564 | // get m = total constraint dimension, nub = number of unbounded variables. |
| 565 | // create constraint offset array and number-of-rows array for all joints. |
| 566 | // the constraints are re-ordered as follows: the purely unbounded |
| 567 | // constraints, the mixed unbounded + LCP constraints, and last the purely |
| 568 | // LCP constraints. this assists the LCP solver to put all unbounded |
| 569 | // variables at the start for a quick factorization. |
| 570 | // |
| 571 | // joints with m=0 are inactive and are removed from the joints array |
| 572 | // entirely, so that the code that follows does not consider them. |
| 573 | // also number all active joints in the joint list (set their tag values). |
| 574 | // inactive joints receive a tag value of -1. |
| 575 | |
| 576 | int m = 0; |
| 577 | dxJoint::Info1 * info = (dxJoint::Info1 *) ALLOCA (nj * sizeof (dxJoint::Info1)); |
| 578 | int *ofs = (int *) ALLOCA (nj * sizeof (int)); |
| 579 | for (i = 0, j = 0; j < nj; j++) |
| 580 | { // i=dest, j=src |
| 581 | joints[j]->vtable->getInfo1 (joints[j], info + i); |
| 582 | dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); |
| 583 | if (info[i].m > 0) |
| 584 | { |
| 585 | joints[i] = joints[j]; |
| 586 | joints[i]->tag = i; |
| 587 | i++; |
| 588 | } |
| 589 | else |
| 590 | { |
| 591 | joints[j]->tag = -1; |
| 592 | } |
| 593 | } |
| 594 | nj = i; |
| 595 | |
| 596 | // the purely unbounded constraints |
| 597 | for (i = 0; i < nj; i++) |
| 598 | { |
| 599 | ofs[i] = m; |
| 600 | m += info[i].m; |
| 601 | } |
| 602 | dReal *c = NULL; |
| 603 | dReal *cfm = NULL; |
no test coverage detected