29 |
|
double dadxk, dadyk, dadzk;//, dadxj, dadyj, dadzj; |
30 |
|
double daxi, dayi, dazi, daxk, dayk, dazk, daxj, dayj, dazj; |
31 |
|
|
32 |
+ |
double aR[3], bR[3], cR[3]; |
33 |
+ |
double aF[3], bF[3], cF[3]; |
34 |
|
|
35 |
+ |
c_p_a->getPos( aR ); |
36 |
+ |
c_p_b->getPos( bR ); |
37 |
+ |
c_p_c->getPos( cR ); |
38 |
+ |
|
39 |
|
|
40 |
< |
dx = c_p_a->getX() - c_p_b->getX(); |
41 |
< |
dy = c_p_a->getY() - c_p_b->getY(); |
42 |
< |
dz = c_p_a->getZ() - c_p_b->getZ(); |
40 |
> |
dx = aR[0] - bR[0]; |
41 |
> |
dy = aR[1] - bR[1]; |
42 |
> |
dz = aR[2] - bR[2]; |
43 |
|
|
44 |
< |
gx = c_p_c->getX() - c_p_b->getX(); |
45 |
< |
gy = c_p_c->getY() - c_p_b->getY(); |
46 |
< |
gz = c_p_c->getZ() - c_p_b->getZ(); |
44 |
> |
gx = cR[0] - bR[0]; |
45 |
> |
gy = cR[1] - bR[1]; |
46 |
> |
gz = cR[2] - bR[2]; |
47 |
|
|
48 |
|
dx2 = dx * dx; |
49 |
|
dy2 = dy * dy; |
113 |
|
daxj = -daxi - daxk; |
114 |
|
dayj = -dayi - dayk; |
115 |
|
dazj = -dazi - dazk; |
116 |
+ |
|
117 |
+ |
aF[0] = daxi; |
118 |
+ |
aF[1] = dayi; |
119 |
+ |
aF[2] = dazi; |
120 |
|
|
121 |
< |
c_p_a->addFx(daxi); |
122 |
< |
c_p_a->addFy(dayi); |
123 |
< |
c_p_a->addFz(dazi); |
121 |
> |
bF[0] = daxj; |
122 |
> |
bF[1] = dayj; |
123 |
> |
bF[2] = dazj; |
124 |
|
|
125 |
< |
c_p_b->addFx(daxj); |
126 |
< |
c_p_b->addFy(dayj); |
127 |
< |
c_p_b->addFz(dazj); |
125 |
> |
cF[0] = daxk; |
126 |
> |
cF[1] = dayk; |
127 |
> |
cF[2] = dazk; |
128 |
|
|
129 |
< |
c_p_c->addFx(daxk); |
130 |
< |
c_p_c->addFy(dayk); |
131 |
< |
c_p_c->addFz(dazk); |
129 |
> |
c_p_a->addFrc(aF); |
130 |
> |
c_p_b->addFrc(bF); |
131 |
> |
c_p_c->addFrc(cF); |
132 |
|
|
133 |
|
return; |
134 |
|
} |