[216] | 1 | 587,598c587,593 |
---|
| 2 | < /******************** breakable joint contribution ***********************/ |
---|
| 3 | < // this saves us a few dereferences |
---|
| 4 | < dxJointBreakInfo *jBI = joint->breakInfo; |
---|
| 5 | < // we need joint feedback if the joint is breakable or if the user |
---|
| 6 | < // requested feedback. |
---|
| 7 | < if (jBI||fb) { |
---|
| 8 | < // we need feedback on the amount of force that this joint is |
---|
| 9 | < // applying to the bodies. we use a slightly slower computation |
---|
| 10 | < // that splits out the force components and puts them in the |
---|
| 11 | < // feedback structure. |
---|
| 12 | < dJointFeedback temp_fb; // temporary storage for joint feedback |
---|
| 13 | < dReal data1[8],data2[8]; |
---|
| 14 | --- |
---|
| 15 | > if (fb) |
---|
| 16 | > { |
---|
| 17 | > // the user has requested feedback on the amount of force that this |
---|
| 18 | > // joint is applying to the bodies. we use a slightly slower |
---|
| 19 | > // computation that splits out the force components and puts them |
---|
| 20 | > // in the feedback structure. |
---|
| 21 | > dReal data1[8], data2[8]; |
---|
| 22 | 603,608c598,603 |
---|
| 23 | < cf1[0] = (temp_fb.f1[0] = data1[0]); |
---|
| 24 | < cf1[1] = (temp_fb.f1[1] = data1[1]); |
---|
| 25 | < cf1[2] = (temp_fb.f1[2] = data1[2]); |
---|
| 26 | < cf1[4] = (temp_fb.t1[0] = data1[4]); |
---|
| 27 | < cf1[5] = (temp_fb.t1[1] = data1[5]); |
---|
| 28 | < cf1[6] = (temp_fb.t1[2] = data1[6]); |
---|
| 29 | --- |
---|
| 30 | > cf1[0] = (fb->f1[0] = data1[0]); |
---|
| 31 | > cf1[1] = (fb->f1[1] = data1[1]); |
---|
| 32 | > cf1[2] = (fb->f1[2] = data1[2]); |
---|
| 33 | > cf1[4] = (fb->t1[0] = data1[4]); |
---|
| 34 | > cf1[5] = (fb->t1[1] = data1[5]); |
---|
| 35 | > cf1[6] = (fb->t1[2] = data1[6]); |
---|
| 36 | 614,691c609,614 |
---|
| 37 | < cf2[0] = (temp_fb.f2[0] = data2[0]); |
---|
| 38 | < cf2[1] = (temp_fb.f2[1] = data2[1]); |
---|
| 39 | < cf2[2] = (temp_fb.f2[2] = data2[2]); |
---|
| 40 | < cf2[4] = (temp_fb.t2[0] = data2[4]); |
---|
| 41 | < cf2[5] = (temp_fb.t2[1] = data2[5]); |
---|
| 42 | < cf2[6] = (temp_fb.t2[2] = data2[6]); |
---|
| 43 | < } |
---|
| 44 | < // if the user requested so we must copy the feedback information to |
---|
| 45 | < // the feedback struct that the user suplied. |
---|
| 46 | < if (fb) { |
---|
| 47 | < // copy temp_fb to fb |
---|
| 48 | < fb->f1[0] = temp_fb.f1[0]; |
---|
| 49 | < fb->f1[1] = temp_fb.f1[1]; |
---|
| 50 | < fb->f1[2] = temp_fb.f1[2]; |
---|
| 51 | < fb->t1[0] = temp_fb.t1[0]; |
---|
| 52 | < fb->t1[1] = temp_fb.t1[1]; |
---|
| 53 | < fb->t1[2] = temp_fb.t1[2]; |
---|
| 54 | < if (body[1]) { |
---|
| 55 | < fb->f2[0] = temp_fb.f2[0]; |
---|
| 56 | < fb->f2[1] = temp_fb.f2[1]; |
---|
| 57 | < fb->f2[2] = temp_fb.f2[2]; |
---|
| 58 | < fb->t2[0] = temp_fb.t2[0]; |
---|
| 59 | < fb->t2[1] = temp_fb.t2[1]; |
---|
| 60 | < fb->t2[2] = temp_fb.t2[2]; |
---|
| 61 | < } |
---|
| 62 | < } |
---|
| 63 | < // if the joint is breakable we need to check the breaking conditions |
---|
| 64 | < if (jBI) { |
---|
| 65 | < dReal relCF1[3]; |
---|
| 66 | < dReal relCT1[3]; |
---|
| 67 | < // multiply the force and torque vectors by the rotation matrix of body 1 |
---|
| 68 | < dMULTIPLY1_331 (&relCF1[0],body[0]->R,&temp_fb.f1[0]); |
---|
| 69 | < dMULTIPLY1_331 (&relCT1[0],body[0]->R,&temp_fb.t1[0]); |
---|
| 70 | < if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) { |
---|
| 71 | < // check if the force is to high |
---|
| 72 | < for (int i = 0; i < 3; i++) { |
---|
| 73 | < if (relCF1[i] > jBI->b1MaxF[i]) { |
---|
| 74 | < jBI->flags |= dJOINT_BROKEN; |
---|
| 75 | < goto doneCheckingBreaks; |
---|
| 76 | < } |
---|
| 77 | < } |
---|
| 78 | < } |
---|
| 79 | < if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) { |
---|
| 80 | < // check if the torque is to high |
---|
| 81 | < for (int i = 0; i < 3; i++) { |
---|
| 82 | < if (relCT1[i] > jBI->b1MaxT[i]) { |
---|
| 83 | < jBI->flags |= dJOINT_BROKEN; |
---|
| 84 | < goto doneCheckingBreaks; |
---|
| 85 | < } |
---|
| 86 | < } |
---|
| 87 | < } |
---|
| 88 | < if (body[1]) { |
---|
| 89 | < dReal relCF2[3]; |
---|
| 90 | < dReal relCT2[3]; |
---|
| 91 | < // multiply the force and torque vectors by the rotation matrix of body 2 |
---|
| 92 | < dMULTIPLY1_331 (&relCF2[0],body[1]->R,&temp_fb.f2[0]); |
---|
| 93 | < dMULTIPLY1_331 (&relCT2[0],body[1]->R,&temp_fb.t2[0]); |
---|
| 94 | < if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) { |
---|
| 95 | < // check if the force is to high |
---|
| 96 | < for (int i = 0; i < 3; i++) { |
---|
| 97 | < if (relCF2[i] > jBI->b2MaxF[i]) { |
---|
| 98 | < jBI->flags |= dJOINT_BROKEN; |
---|
| 99 | < goto doneCheckingBreaks; |
---|
| 100 | < } |
---|
| 101 | < } |
---|
| 102 | < } |
---|
| 103 | < if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) { |
---|
| 104 | < // check if the torque is to high |
---|
| 105 | < for (int i = 0; i < 3; i++) { |
---|
| 106 | < if (relCT2[i] > jBI->b2MaxT[i]) { |
---|
| 107 | < jBI->flags |= dJOINT_BROKEN; |
---|
| 108 | < goto doneCheckingBreaks; |
---|
| 109 | < } |
---|
| 110 | < } |
---|
| 111 | < } |
---|
| 112 | < } |
---|
| 113 | < doneCheckingBreaks: |
---|
| 114 | < ; |
---|
| 115 | --- |
---|
| 116 | > cf2[0] = (fb->f2[0] = data2[0]); |
---|
| 117 | > cf2[1] = (fb->f2[1] = data2[1]); |
---|
| 118 | > cf2[2] = (fb->f2[2] = data2[2]); |
---|
| 119 | > cf2[4] = (fb->t2[0] = data2[4]); |
---|
| 120 | > cf2[5] = (fb->t2[1] = data2[5]); |
---|
| 121 | > cf2[6] = (fb->t2[2] = data2[6]); |
---|
| 122 | 694d616 |
---|
| 123 | < /*************************************************************************/ |
---|
| 124 | 1178,1196d1099 |
---|
| 125 | < /******************** breakable joint contribution ***********************/ |
---|
| 126 | < dxJoint* nextJ; |
---|
| 127 | < if (!world->firstjoint) |
---|
| 128 | < nextJ = 0; |
---|
| 129 | < else |
---|
| 130 | < nextJ = (dxJoint*)world->firstjoint->next; |
---|
| 131 | < for (j=world->firstjoint; j; j=nextJ) { |
---|
| 132 | < nextJ = (dxJoint*)j->next; |
---|
| 133 | < // check if joint is breakable and broken |
---|
| 134 | < if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) { |
---|
| 135 | < // detach (break) the joint |
---|
| 136 | < dJointAttach (j, 0, 0); |
---|
| 137 | < // call the callback function if it is set |
---|
| 138 | < if (j->breakInfo->callback) j->breakInfo->callback (j); |
---|
| 139 | < // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set |
---|
| 140 | < if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j); |
---|
| 141 | < } |
---|
| 142 | < } |
---|
| 143 | < /*************************************************************************/ |
---|