aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/BreakableJoints/diff/step.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/step.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/step.cpp.diff')
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff130
1 files changed, 130 insertions, 0 deletions
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff
new file mode 100644
index 0000000..dfc8c2f
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff
@@ -0,0 +1,130 @@
1966,1066c966,989
2< /******************** breakable joint contribution ***********************/
3< // this saves us a few dereferences
4< dxJointBreakInfo *jBI = joint[i]->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< Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m);
15< dReal *cf1 = cforce + 8*b1->tag;
16< cf1[0] += (temp_fb.f1[0] = data1[0]);
17< cf1[1] += (temp_fb.f1[1] = data1[1]);
18< cf1[2] += (temp_fb.f1[2] = data1[2]);
19< cf1[4] += (temp_fb.t1[0] = data1[4]);
20< cf1[5] += (temp_fb.t1[1] = data1[5]);
21< cf1[6] += (temp_fb.t1[2] = data1[6]);
22< if (b2) {
23< Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
24< dReal *cf2 = cforce + 8*b2->tag;
25< cf2[0] += (temp_fb.f2[0] = data2[0]);
26< cf2[1] += (temp_fb.f2[1] = data2[1]);
27< cf2[2] += (temp_fb.f2[2] = data2[2]);
28< cf2[4] += (temp_fb.t2[0] = data2[4]);
29< cf2[5] += (temp_fb.t2[1] = data2[5]);
30< cf2[6] += (temp_fb.t2[2] = data2[6]);
31< }
32< // if the user requested so we must copy the feedback information to
33< // the feedback struct that the user suplied.
34< if (fb) {
35< // copy temp_fb to fb
36< fb->f1[0] = temp_fb.f1[0];
37< fb->f1[1] = temp_fb.f1[1];
38< fb->f1[2] = temp_fb.f1[2];
39< fb->t1[0] = temp_fb.t1[0];
40< fb->t1[1] = temp_fb.t1[1];
41< fb->t1[2] = temp_fb.t1[2];
42< if (b2) {
43< fb->f2[0] = temp_fb.f2[0];
44< fb->f2[1] = temp_fb.f2[1];
45< fb->f2[2] = temp_fb.f2[2];
46< fb->t2[0] = temp_fb.t2[0];
47< fb->t2[1] = temp_fb.t2[1];
48< fb->t2[2] = temp_fb.t2[2];
49< }
50< }
51< // if the joint is breakable we need to check the breaking conditions
52< if (jBI) {
53< dReal relCF1[3];
54< dReal relCT1[3];
55< // multiply the force and torque vectors by the rotation matrix of body 1
56< dMULTIPLY1_331 (&relCF1[0],b1->R,&temp_fb.f1[0]);
57< dMULTIPLY1_331 (&relCT1[0],b1->R,&temp_fb.t1[0]);
58< if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) {
59< // check if the force is to high
60< for (int i = 0; i < 3; i++) {
61< if (relCF1[i] > jBI->b1MaxF[i]) {
62< jBI->flags |= dJOINT_BROKEN;
63< goto doneCheckingBreaks;
64< }
65< }
66< }
67< if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) {
68< // check if the torque is to high
69< for (int i = 0; i < 3; i++) {
70< if (relCT1[i] > jBI->b1MaxT[i]) {
71< jBI->flags |= dJOINT_BROKEN;
72< goto doneCheckingBreaks;
73< }
74< }
75< }
76< if (b2) {
77< dReal relCF2[3];
78< dReal relCT2[3];
79< // multiply the force and torque vectors by the rotation matrix of body 2
80< dMULTIPLY1_331 (&relCF2[0],b2->R,&temp_fb.f2[0]);
81< dMULTIPLY1_331 (&relCT2[0],b2->R,&temp_fb.t2[0]);
82< if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) {
83< // check if the force is to high
84< for (int i = 0; i < 3; i++) {
85< if (relCF2[i] > jBI->b2MaxF[i]) {
86< jBI->flags |= dJOINT_BROKEN;
87< goto doneCheckingBreaks;
88< }
89< }
90< }
91< if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) {
92< // check if the torque is to high
93< for (int i = 0; i < 3; i++) {
94< if (relCT2[i] > jBI->b2MaxT[i]) {
95< jBI->flags |= dJOINT_BROKEN;
96< goto doneCheckingBreaks;
97< }
98< }
99< }
100< }
101< doneCheckingBreaks:
102< ;
103---
104> if (fb) {
105> // the user has requested feedback on the amount of force that this
106> // joint is applying to the bodies. we use a slightly slower
107> // computation that splits out the force components and puts them
108> // in the feedback structure.
109> dReal data1[8],data2[8];
110> Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m);
111> dReal *cf1 = cforce + 8*b1->tag;
112> cf1[0] += (fb->f1[0] = data1[0]);
113> cf1[1] += (fb->f1[1] = data1[1]);
114> cf1[2] += (fb->f1[2] = data1[2]);
115> cf1[4] += (fb->t1[0] = data1[4]);
116> cf1[5] += (fb->t1[1] = data1[5]);
117> cf1[6] += (fb->t1[2] = data1[6]);
118> if (b2){
119> Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
120> dReal *cf2 = cforce + 8*b2->tag;
121> cf2[0] += (fb->f2[0] = data2[0]);
122> cf2[1] += (fb->f2[1] = data2[1]);
123> cf2[2] += (fb->f2[2] = data2[2]);
124> cf2[4] += (fb->t2[0] = data2[4]);
125> cf2[5] += (fb->t2[1] = data2[5]);
126> cf2[6] += (fb->t2[2] = data2[6]);
127> }
1281068,1069d990
129< }
130< /*************************************************************************/
generated by cgit v1.1 at 2025-01-01 15:00:18 +0000