| 45 |
|
return; |
| 46 |
|
} |
| 47 |
|
|
| 48 |
– |
|
| 48 |
|
vect r_ab; /*the vector whose origin is a and end is b */ |
| 49 |
|
double force; /* the force scaling factor. */ |
| 50 |
|
double Fab_x; /*the x,y, and z components of the force */ |
| 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 |
|
} |
