source: code/branches/ode/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff @ 216

587,598c587,593
< /******************** breakable joint contribution ***********************/
<       // this saves us a few dereferences
<     dxJointBreakInfo *jBI = joint->breakInfo;
<     // we need joint feedback if the joint is breakable or if the user
<     // requested feedback.
<       if (jBI||fb) {
<               // we need feedback on the amount of force that this joint is
<               // applying to the bodies. we use a slightly slower computation
<               // that splits out the force components and puts them in the
<               // feedback structure.
<               dJointFeedback temp_fb; // temporary storage for joint feedback
<               dReal data1[8],data2[8];
---
>       if (fb)
>       {
>               // the user has requested feedback on the amount of force that this
>               // joint is applying to the bodies. we use a slightly slower
>               // computation that splits out the force components and puts them
>               // in the feedback structure.
>               dReal data1[8], data2[8];
603,608c598,603
<                       cf1[0] = (temp_fb.f1[0] = data1[0]);
<                       cf1[1] = (temp_fb.f1[1] = data1[1]);
<                       cf1[2] = (temp_fb.f1[2] = data1[2]);
<                       cf1[4] = (temp_fb.t1[0] = data1[4]);
<                       cf1[5] = (temp_fb.t1[1] = data1[5]);
<                       cf1[6] = (temp_fb.t1[2] = data1[6]);
---
>                       cf1[0] = (fb->f1[0] = data1[0]);
>                       cf1[1] = (fb->f1[1] = data1[1]);
>                       cf1[2] = (fb->f1[2] = data1[2]);
>                       cf1[4] = (fb->t1[0] = data1[4]);
>                       cf1[5] = (fb->t1[1] = data1[5]);
>                       cf1[6] = (fb->t1[2] = data1[6]);
614,691c609,614
<                       cf2[0] = (temp_fb.f2[0] = data2[0]);
<                       cf2[1] = (temp_fb.f2[1] = data2[1]);
<                       cf2[2] = (temp_fb.f2[2] = data2[2]);
<                       cf2[4] = (temp_fb.t2[0] = data2[4]);
<                       cf2[5] = (temp_fb.t2[1] = data2[5]);
<                       cf2[6] = (temp_fb.t2[2] = data2[6]);
<               }
<               // if the user requested so we must copy the feedback information to
<               // the feedback struct that the user suplied.
<               if (fb) {
<                       // copy temp_fb to fb
<                       fb->f1[0] = temp_fb.f1[0];
<                       fb->f1[1] = temp_fb.f1[1];
<                       fb->f1[2] = temp_fb.f1[2];
<                       fb->t1[0] = temp_fb.t1[0];
<                       fb->t1[1] = temp_fb.t1[1];
<                       fb->t1[2] = temp_fb.t1[2];
<                       if (body[1]) {
<                               fb->f2[0] = temp_fb.f2[0];
<                               fb->f2[1] = temp_fb.f2[1];
<                               fb->f2[2] = temp_fb.f2[2];
<                               fb->t2[0] = temp_fb.t2[0];
<                               fb->t2[1] = temp_fb.t2[1];
<                               fb->t2[2] = temp_fb.t2[2];
<                       }
<               }
<               // if the joint is breakable we need to check the breaking conditions
<               if (jBI) {
<                       dReal relCF1[3];
<                       dReal relCT1[3];
<                       // multiply the force and torque vectors by the rotation matrix of body 1
<                       dMULTIPLY1_331 (&relCF1[0],body[0]->R,&temp_fb.f1[0]);
<                       dMULTIPLY1_331 (&relCT1[0],body[0]->R,&temp_fb.t1[0]);
<                       if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) {
<                               // check if the force is to high
<                               for (int i = 0; i < 3; i++) {
<                                       if (relCF1[i] > jBI->b1MaxF[i]) {
<                                               jBI->flags |= dJOINT_BROKEN;
<                                               goto doneCheckingBreaks;
<                                       }
<                               }
<                       }
<                       if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) {
<                               // check if the torque is to high
<                               for (int i = 0; i < 3; i++) {
<                                       if (relCT1[i] > jBI->b1MaxT[i]) {
<                                               jBI->flags |= dJOINT_BROKEN;
<                                               goto doneCheckingBreaks;
<                                       }
<                               }
<                       }
<                       if (body[1]) {
<                               dReal relCF2[3];
<                               dReal relCT2[3];
<                               // multiply the force and torque vectors by the rotation matrix of body 2
<                               dMULTIPLY1_331 (&relCF2[0],body[1]->R,&temp_fb.f2[0]);
<                               dMULTIPLY1_331 (&relCT2[0],body[1]->R,&temp_fb.t2[0]);
<                               if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) {
<                                       // check if the force is to high
<                                       for (int i = 0; i < 3; i++) {
<                                               if (relCF2[i] > jBI->b2MaxF[i]) {
<                                                       jBI->flags |= dJOINT_BROKEN;
<                                                       goto doneCheckingBreaks;
<                                               }
<                                       }
<                               }
<                               if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) {
<                                       // check if the torque is to high
<                                       for (int i = 0; i < 3; i++) {
<                                               if (relCT2[i] > jBI->b2MaxT[i]) {
<                                                       jBI->flags |= dJOINT_BROKEN;
<                                                       goto doneCheckingBreaks;
<                                               }
<                                       }
<                               }
<                       }
<                       doneCheckingBreaks:
<                       ;
---
>                       cf2[0] = (fb->f2[0] = data2[0]);
>                       cf2[1] = (fb->f2[1] = data2[1]);
>                       cf2[2] = (fb->f2[2] = data2[2]);
>                       cf2[4] = (fb->t2[0] = data2[4]);
>                       cf2[5] = (fb->t2[1] = data2[5]);
>                       cf2[6] = (fb->t2[2] = data2[6]);
694d616
< /*************************************************************************/
1178,1196d1099
<   /******************** breakable joint contribution ***********************/
<   dxJoint* nextJ;
<   if (!world->firstjoint)
<     nextJ = 0;
<   else
<     nextJ = (dxJoint*)world->firstjoint->next;
<   for (j=world->firstjoint; j; j=nextJ) {
<       nextJ = (dxJoint*)j->next;
<       // check if joint is breakable and broken
<     if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) {
<               // detach (break) the joint
<         dJointAttach (j, 0, 0);
<               // call the callback function if it is set
<               if (j->breakInfo->callback) j->breakInfo->callback (j);
<               // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set
<               if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j);
<       }
<   }
<   /*************************************************************************/