aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff
diff options
context:
space:
mode:
authordan miller2007-10-19 05:20:48 +0000
committerdan miller2007-10-19 05:20:48 +0000
commitd48ea5bb797037069d641da41da0f195f0124491 (patch)
tree40ff433d94859d629aac933d5ec73b382f62ba1a /libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff
parentdont ask (diff)
downloadopensim-SC-d48ea5bb797037069d641da41da0f195f0124491.zip
opensim-SC-d48ea5bb797037069d641da41da0f195f0124491.tar.gz
opensim-SC-d48ea5bb797037069d641da41da0f195f0124491.tar.bz2
opensim-SC-d48ea5bb797037069d641da41da0f195f0124491.tar.xz
one more for the gipper
Diffstat (limited to 'libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff')
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff143
1 files changed, 143 insertions, 0 deletions
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff
new file mode 100644
index 0000000..ed64cba
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff
@@ -0,0 +1,143 @@
1587,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];
22603,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]);
36614,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]);
122694d616
123< /*************************************************************************/
1241178,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< /*************************************************************************/
generated by cgit v1.1 at 2025-01-29 09:27:58 +0000