| 180 | } |
| 181 | |
| 182 | void integrateVelocity(double deltaTime) |
| 183 | { |
| 184 | LWPose newPose; |
| 185 | |
| 186 | newPose.m_position = m_worldPose.m_position + m_linearVelocity * deltaTime; |
| 187 | |
| 188 | if (m_flags & LWFLAG_USE_QUATERNION_DERIVATIVE) |
| 189 | { |
| 190 | newPose.m_orientation = m_worldPose.m_orientation; |
| 191 | newPose.m_orientation += (m_angularVelocity * newPose.m_orientation) * (deltaTime * btScalar(0.5)); |
| 192 | newPose.m_orientation.normalize(); |
| 193 | m_worldPose = newPose; |
| 194 | } |
| 195 | else |
| 196 | { |
| 197 | //Exponential map |
| 198 | //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia |
| 199 | |
| 200 | //btQuaternion q_w = [ sin(|w|*dt/2) * w/|w| , cos(|w|*dt/2)] |
| 201 | //btQuaternion q_new = q_w * q_old; |
| 202 | |
| 203 | b3Vector3 axis; |
| 204 | b3Scalar fAngle = m_angularVelocity.length(); |
| 205 | //limit the angular motion |
| 206 | const btScalar angularMotionThreshold = btScalar(0.5) * SIMD_HALF_PI; |
| 207 | |
| 208 | if (fAngle * deltaTime > angularMotionThreshold) |
| 209 | { |
| 210 | fAngle = angularMotionThreshold / deltaTime; |
| 211 | } |
| 212 | |
| 213 | if (fAngle < btScalar(0.001)) |
| 214 | { |
| 215 | // use Taylor's expansions of sync function |
| 216 | axis = m_angularVelocity * (btScalar(0.5) * deltaTime - (deltaTime * deltaTime * deltaTime) * (btScalar(0.020833333333)) * fAngle * fAngle); |
| 217 | } |
| 218 | else |
| 219 | { |
| 220 | // sync(fAngle) = sin(c*fAngle)/t |
| 221 | axis = m_angularVelocity * (btSin(btScalar(0.5) * fAngle * deltaTime) / fAngle); |
| 222 | } |
| 223 | b3Quaternion dorn(axis.x, axis.y, axis.z, btCos(fAngle * deltaTime * b3Scalar(0.5))); |
| 224 | b3Quaternion orn0 = m_worldPose.m_orientation; |
| 225 | |
| 226 | b3Quaternion predictedOrn = dorn * orn0; |
| 227 | predictedOrn.normalize(); |
| 228 | m_worldPose.m_orientation = predictedOrn; |
| 229 | } |
| 230 | } |
| 231 | |
| 232 | void stepSimulation(double deltaTime) |
| 233 | { |
no test coverage detected