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 |
|
} |