51 |
|
double Fab_y; |
52 |
|
double Fab_z; |
53 |
|
|
54 |
+ |
double aR[3], bR[3]; |
55 |
+ |
double aF[3], bF[3]; |
56 |
+ |
|
57 |
|
/* initialize the vector */ |
58 |
+ |
|
59 |
+ |
c_p_a->getPos(aR); |
60 |
+ |
c_p_b->getPos(bR); |
61 |
|
|
62 |
< |
r_ab.x = c_p_b->getX() - c_p_a->getX(); |
63 |
< |
r_ab.y = c_p_b->getY() - c_p_a->getY(); |
64 |
< |
r_ab.z = c_p_b->getZ() - c_p_a->getZ(); |
62 |
> |
r_ab.x = bR[0] - aR[0]; |
63 |
> |
r_ab.y = bR[1] - aR[1]; |
64 |
> |
r_ab.z = bR[2] - aR[2]; |
65 |
> |
|
66 |
|
r_ab.length = sqrt((r_ab.x * r_ab.x + r_ab.y * r_ab.y + r_ab.z * r_ab.z)); |
67 |
|
|
68 |
|
/* calculate the force here */ |
73 |
|
Fab_y = -force * r_ab.y / r_ab.length; |
74 |
|
Fab_z = -force * r_ab.z / r_ab.length; |
75 |
|
|
76 |
< |
c_p_a->addFx(Fab_x); |
77 |
< |
c_p_a->addFy(Fab_y); |
78 |
< |
c_p_a->addFz(Fab_z); |
76 |
> |
aF[0] = Fab_x; |
77 |
> |
aF[1] = Fab_y; |
78 |
> |
aF[2] = Fab_z; |
79 |
|
|
80 |
< |
c_p_b->addFx(-Fab_x); |
81 |
< |
c_p_b->addFy(-Fab_y); |
82 |
< |
c_p_b->addFz(-Fab_z); |
80 |
> |
bF[0] = -Fab_x; |
81 |
> |
bF[1] = -Fab_y; |
82 |
> |
bF[2] = -Fab_z; |
83 |
|
|
84 |
+ |
c_p_a->addFrc(aF); |
85 |
+ |
c_p_b->addFrc(bF); |
86 |
+ |
|
87 |
|
return; |
88 |
|
} |