| 245 | // interval h, thereby adjusting its position and orientation. |
| 246 | |
| 247 | static inline void |
| 248 | moveAndRotateBody (dxBody * b, dReal h) |
| 249 | { |
| 250 | int j; |
| 251 | |
| 252 | // handle linear velocity |
| 253 | for (j = 0; j < 3; j++) |
| 254 | b->pos[j] += h * b->lvel[j]; |
| 255 | |
| 256 | if (b->flags & dxBodyFlagFiniteRotation) |
| 257 | { |
| 258 | dVector3 irv; // infitesimal rotation vector |
| 259 | dQuaternion q; // quaternion for finite rotation |
| 260 | |
| 261 | if (b->flags & dxBodyFlagFiniteRotationAxis) |
| 262 | { |
| 263 | // split the angular velocity vector into a component along the finite |
| 264 | // rotation axis, and a component orthogonal to it. |
| 265 | dVector3 frv, irv; // finite rotation vector |
| 266 | dReal k = dDOT (b->finite_rot_axis, b->avel); |
| 267 | frv[0] = b->finite_rot_axis[0] * k; |
| 268 | frv[1] = b->finite_rot_axis[1] * k; |
| 269 | frv[2] = b->finite_rot_axis[2] * k; |
| 270 | irv[0] = b->avel[0] - frv[0]; |
| 271 | irv[1] = b->avel[1] - frv[1]; |
| 272 | irv[2] = b->avel[2] - frv[2]; |
| 273 | |
| 274 | // make a rotation quaternion q that corresponds to frv * h. |
| 275 | // compare this with the full-finite-rotation case below. |
| 276 | h *= REAL (0.5); |
| 277 | dReal theta = k * h; |
| 278 | q[0] = dCos (theta); |
| 279 | dReal s = sinc (theta) * h; |
| 280 | q[1] = frv[0] * s; |
| 281 | q[2] = frv[1] * s; |
| 282 | q[3] = frv[2] * s; |
| 283 | } |
| 284 | else |
| 285 | { |
| 286 | // make a rotation quaternion q that corresponds to w * h |
| 287 | dReal wlen = dSqrt (b->avel[0] * b->avel[0] + b->avel[1] * b->avel[1] + b->avel[2] * b->avel[2]); |
| 288 | h *= REAL (0.5); |
| 289 | dReal theta = wlen * h; |
| 290 | q[0] = dCos (theta); |
| 291 | dReal s = sinc (theta) * h; |
| 292 | q[1] = b->avel[0] * s; |
| 293 | q[2] = b->avel[1] * s; |
| 294 | q[3] = b->avel[2] * s; |
| 295 | } |
| 296 | |
| 297 | // do the finite rotation |
| 298 | dQuaternion q2; |
| 299 | dQMultiply0 (q2, q, b->q); |
| 300 | for (j = 0; j < 4; j++) |
| 301 | b->q[j] = q2[j]; |
| 302 | |
| 303 | // do the infitesimal rotation if required |
| 304 | if (b->flags & dxBodyFlagFiniteRotationAxis) |
no test coverage detected