1 | 966,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 | > } |
---|
128 | 1068,1069d990 |
---|
129 | < } |
---|
130 | < /*************************************************************************/ |
---|