| 92 | // interval h, thereby adjusting its position and orientation. |
| 93 | |
| 94 | void dxStepBody (dxBody *b, dReal h) |
| 95 | { |
| 96 | int j; |
| 97 | #ifdef DEBUG_VALID |
| 98 | dIASSERT(dValid(b->avel[0])&&dValid(b->avel[1])&&dValid(b->avel[2])); |
| 99 | #endif |
| 100 | // handle linear velocity |
| 101 | for (j=0; j<3; j++) b->pos[j] += h * b->lvel[j]; |
| 102 | |
| 103 | if (b->flags & dxBodyFlagFiniteRotation) { |
| 104 | dVector3 irv; // infitesimal rotation vector |
| 105 | dQuaternion q; // quaternion for finite rotation |
| 106 | |
| 107 | if (b->flags & dxBodyFlagFiniteRotationAxis) { |
| 108 | // split the angular velocity vector into a component along the finite |
| 109 | // rotation axis, and a component orthogonal to it. |
| 110 | dVector3 frv; // finite rotation vector |
| 111 | dReal k = dDOT (b->finite_rot_axis,b->avel); |
| 112 | frv[0] = b->finite_rot_axis[0] * k; |
| 113 | frv[1] = b->finite_rot_axis[1] * k; |
| 114 | frv[2] = b->finite_rot_axis[2] * k; |
| 115 | irv[0] = b->avel[0] - frv[0]; |
| 116 | irv[1] = b->avel[1] - frv[1]; |
| 117 | irv[2] = b->avel[2] - frv[2]; |
| 118 | |
| 119 | // make a rotation quaternion q that corresponds to frv * h. |
| 120 | // compare this with the full-finite-rotation case below. |
| 121 | h *= REAL(0.5); |
| 122 | dReal theta = k * h; |
| 123 | q[0] = dCos(theta); |
| 124 | dReal s = sinc(theta) * h; |
| 125 | q[1] = frv[0] * s; |
| 126 | q[2] = frv[1] * s; |
| 127 | q[3] = frv[2] * s; |
| 128 | } |
| 129 | else { |
| 130 | // make a rotation quaternion q that corresponds to w * h |
| 131 | dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] + |
| 132 | b->avel[2]*b->avel[2]); |
| 133 | h *= REAL(0.5); |
| 134 | dReal theta = wlen * h; |
| 135 | q[0] = dCos(theta); |
| 136 | dReal s = sinc(theta) * h; |
| 137 | q[1] = b->avel[0] * s; |
| 138 | q[2] = b->avel[1] * s; |
| 139 | q[3] = b->avel[2] * s; |
| 140 | } |
| 141 | |
| 142 | // do the finite rotation |
| 143 | dQuaternion q2; |
| 144 | dQMultiply0 (q2,q,b->q); |
| 145 | for (j=0; j<4; j++) b->q[j] = q2[j]; |
| 146 | |
| 147 | // do the infitesimal rotation if required |
| 148 | if (b->flags & dxBodyFlagFiniteRotationAxis) { |
| 149 | dReal dq[4]; |
| 150 | dWtoDQ (irv,b->q,dq); |
| 151 | for (j=0; j<4; j++) b->q[j] += h * dq[j]; |
no test coverage detected