| 39 |
|
|
| 40 |
|
double r_cr1_r_cr2; /* the length of r_cr1 * length of r_cr2 */ |
| 41 |
|
|
| 42 |
< |
r_ab.x = c_p_b->getX() - c_p_a->getX(); |
| 43 |
< |
r_ab.y = c_p_b->getY() - c_p_a->getY(); |
| 44 |
< |
r_ab.z = c_p_b->getZ() - c_p_a->getZ(); |
| 42 |
> |
double aR[3], bR[3], cR[3], dR[3]; |
| 43 |
> |
double aF[3], bF[3], cF[3], dF[3]; |
| 44 |
> |
|
| 45 |
> |
c_p_a->getPos( aR ); |
| 46 |
> |
c_p_b->getPos( bR ); |
| 47 |
> |
c_p_c->getPos( cR ); |
| 48 |
> |
c_p_d->getPos( dR ); |
| 49 |
> |
|
| 50 |
> |
r_ab.x = bR[0] - aR[0]; |
| 51 |
> |
r_ab.y = bR[1] - aR[1]; |
| 52 |
> |
r_ab.z = bR[2] - aR[2]; |
| 53 |
|
r_ab.length = sqrt((r_ab.x * r_ab.x + r_ab.y * r_ab.y + r_ab.z * r_ab.z)); |
| 54 |
|
|
| 55 |
< |
r_cb.x = c_p_b->getX() - c_p_c->getX(); |
| 56 |
< |
r_cb.y = c_p_b->getY() - c_p_c->getY(); |
| 57 |
< |
r_cb.z = c_p_b->getZ() - c_p_c->getZ(); |
| 55 |
> |
r_cb.x = bR[0] - cR[0]; |
| 56 |
> |
r_cb.y = bR[1] - cR[1]; |
| 57 |
> |
r_cb.z = bR[2] - cR[2]; |
| 58 |
|
r_cb.length = sqrt((r_cb.x * r_cb.x + r_cb.y * r_cb.y + r_cb.z * r_cb.z)); |
| 59 |
|
|
| 60 |
< |
r_cd.x = c_p_d->getX() - c_p_c->getX(); |
| 61 |
< |
r_cd.y = c_p_d->getY() - c_p_c->getY(); |
| 62 |
< |
r_cd.z = c_p_d->getZ() - c_p_c->getZ(); |
| 60 |
> |
r_cd.x = dR[0] - cR[0]; |
| 61 |
> |
r_cd.y = dR[1] - cR[1]; |
| 62 |
> |
r_cd.z = dR[2] - cR[2]; |
| 63 |
|
r_cd.length = sqrt((r_cd.x * r_cd.x + r_cd.y * r_cd.y + r_cd.z * r_cd.z)); |
| 64 |
|
|
| 65 |
|
r_cr1.x = r_ab.y * r_cb.z - r_cb.y * r_ab.z; |
| 136 |
|
|
| 137 |
|
force = torsion_force(cos_phi); |
| 138 |
|
|
| 139 |
< |
c_p_a->addFx(force * (d_cos_dy_cr1 * r_cb.z - d_cos_dz_cr1 * r_cb.y)); |
| 140 |
< |
c_p_a->addFy(force * (d_cos_dz_cr1 * r_cb.x - d_cos_dx_cr1 * r_cb.z)); |
| 141 |
< |
c_p_a->addFz(force * (d_cos_dx_cr1 * r_cb.y - d_cos_dy_cr1 * r_cb.x)); |
| 139 |
> |
aF[0] = force * (d_cos_dy_cr1 * r_cb.z - d_cos_dz_cr1 * r_cb.y); |
| 140 |
> |
aF[1] = force * (d_cos_dz_cr1 * r_cb.x - d_cos_dx_cr1 * r_cb.z); |
| 141 |
> |
aF[2] = force * (d_cos_dx_cr1 * r_cb.y - d_cos_dy_cr1 * r_cb.x); |
| 142 |
|
|
| 143 |
< |
c_p_b->addFx(force * ( d_cos_dy_cr1 * (r_ab.z - r_cb.z) |
| 144 |
< |
- d_cos_dy_cr2 * r_cd.z |
| 145 |
< |
+ d_cos_dz_cr1 * (r_cb.y - r_ab.y) |
| 146 |
< |
+ d_cos_dz_cr2 * r_cd.y)); |
| 147 |
< |
c_p_b->addFy(force * ( d_cos_dx_cr1 * (r_cb.z - r_ab.z) |
| 148 |
< |
+ d_cos_dx_cr2 * r_cd.z |
| 149 |
< |
+ d_cos_dz_cr1 * (r_ab.x - r_cb.x) |
| 150 |
< |
- d_cos_dz_cr2 * r_cd.x)); |
| 151 |
< |
c_p_b->addFz(force * ( d_cos_dx_cr1 * (r_ab.y - r_cb.y) |
| 152 |
< |
- d_cos_dx_cr2 * r_cd.y |
| 153 |
< |
+ d_cos_dy_cr1 * (r_cb.x - r_ab.x) |
| 154 |
< |
+ d_cos_dy_cr2 * r_cd.x)); |
| 143 |
> |
bF[0] = force * ( d_cos_dy_cr1 * (r_ab.z - r_cb.z) |
| 144 |
> |
- d_cos_dy_cr2 * r_cd.z |
| 145 |
> |
+ d_cos_dz_cr1 * (r_cb.y - r_ab.y) |
| 146 |
> |
+ d_cos_dz_cr2 * r_cd.y); |
| 147 |
> |
bF[1] = force * ( d_cos_dx_cr1 * (r_cb.z - r_ab.z) |
| 148 |
> |
+ d_cos_dx_cr2 * r_cd.z |
| 149 |
> |
+ d_cos_dz_cr1 * (r_ab.x - r_cb.x) |
| 150 |
> |
- d_cos_dz_cr2 * r_cd.x); |
| 151 |
> |
bF[2] = force * ( d_cos_dx_cr1 * (r_ab.y - r_cb.y) |
| 152 |
> |
- d_cos_dx_cr2 * r_cd.y |
| 153 |
> |
+ d_cos_dy_cr1 * (r_cb.x - r_ab.x) |
| 154 |
> |
+ d_cos_dy_cr2 * r_cd.x); |
| 155 |
|
|
| 156 |
< |
c_p_c->addFx(force * (- d_cos_dy_cr1 * r_ab.z |
| 157 |
< |
- d_cos_dy_cr2 * (r_cb.z - r_cd.z) |
| 158 |
< |
+ d_cos_dz_cr1 * r_ab.y |
| 159 |
< |
- d_cos_dz_cr2 * (r_cd.y - r_cb.y))); |
| 160 |
< |
c_p_c->addFy(force * ( d_cos_dx_cr1 * r_ab.z |
| 161 |
< |
- d_cos_dx_cr2 * (r_cd.z - r_cb.z) |
| 162 |
< |
- d_cos_dz_cr1 * r_ab.x |
| 163 |
< |
- d_cos_dz_cr2 * (r_cb.x - r_cd.x))); |
| 164 |
< |
c_p_c->addFz(force * (- d_cos_dx_cr1 * r_ab.y |
| 165 |
< |
- d_cos_dx_cr2 * (r_cb.y - r_cd.y) |
| 166 |
< |
+ d_cos_dy_cr1 * r_ab.x |
| 167 |
< |
- d_cos_dy_cr2 * (r_cd.x - r_cb.x))); |
| 156 |
> |
cF[0] = force * (- d_cos_dy_cr1 * r_ab.z |
| 157 |
> |
- d_cos_dy_cr2 * (r_cb.z - r_cd.z) |
| 158 |
> |
+ d_cos_dz_cr1 * r_ab.y |
| 159 |
> |
- d_cos_dz_cr2 * (r_cd.y - r_cb.y)); |
| 160 |
> |
cF[1] = force * ( d_cos_dx_cr1 * r_ab.z |
| 161 |
> |
- d_cos_dx_cr2 * (r_cd.z - r_cb.z) |
| 162 |
> |
- d_cos_dz_cr1 * r_ab.x |
| 163 |
> |
- d_cos_dz_cr2 * (r_cb.x - r_cd.x)); |
| 164 |
> |
cF[2] = force * (- d_cos_dx_cr1 * r_ab.y |
| 165 |
> |
- d_cos_dx_cr2 * (r_cb.y - r_cd.y) |
| 166 |
> |
+ d_cos_dy_cr1 * r_ab.x |
| 167 |
> |
- d_cos_dy_cr2 * (r_cd.x - r_cb.x)); |
| 168 |
|
|
| 169 |
< |
c_p_d->addFx(force * (d_cos_dy_cr2 * r_cb.z - d_cos_dz_cr2 * r_cb.y)); |
| 170 |
< |
c_p_d->addFy(force * (d_cos_dz_cr2 * r_cb.x - d_cos_dx_cr2 * r_cb.z)); |
| 171 |
< |
c_p_d->addFz(force * (d_cos_dx_cr2 * r_cb.y - d_cos_dy_cr2 * r_cb.x)); |
| 169 |
> |
dF[0] = force * (d_cos_dy_cr2 * r_cb.z - d_cos_dz_cr2 * r_cb.y); |
| 170 |
> |
dF[1] = force * (d_cos_dz_cr2 * r_cb.x - d_cos_dx_cr2 * r_cb.z); |
| 171 |
> |
dF[2] = force * (d_cos_dx_cr2 * r_cb.y - d_cos_dy_cr2 * r_cb.x); |
| 172 |
> |
|
| 173 |
> |
|
| 174 |
> |
c_p_a->addFrc(aF); |
| 175 |
> |
c_p_b->addFrc(bF); |
| 176 |
> |
c_p_c->addFrc(cF); |
| 177 |
> |
c_p_d->addFrc(dF); |
| 178 |
|
} |