| 236 | }; |
| 237 | |
| 238 | b3Scalar resolveCollision(LWRigidBody& bodyA, |
| 239 | LWRigidBody& bodyB, |
| 240 | LWContactPoint& contactPoint) |
| 241 | { |
| 242 | b3Assert(contactPoint.m_distance <= 0); |
| 243 | |
| 244 | btScalar appliedImpulse = 0.f; |
| 245 | |
| 246 | b3Vector3 rel_pos1 = contactPoint.m_ptOnAWorld - bodyA.m_worldPose.m_position; |
| 247 | b3Vector3 rel_pos2 = contactPoint.m_ptOnBWorld - bodyB.getPosition(); |
| 248 | |
| 249 | btScalar rel_vel = contactPoint.m_normalOnB.dot(bodyA.getVelocity(rel_pos1) - bodyB.getVelocity(rel_pos2)); |
| 250 | if (rel_vel < -B3_EPSILON) |
| 251 | { |
| 252 | b3Vector3 temp1 = bodyA.m_invInertiaTensorWorld * rel_pos1.cross(contactPoint.m_normalOnB); |
| 253 | b3Vector3 temp2 = bodyB.m_invInertiaTensorWorld * rel_pos2.cross(contactPoint.m_normalOnB); |
| 254 | |
| 255 | btScalar impulse = -(1.0f + gRestitution) * rel_vel / |
| 256 | (bodyA.m_invMass + bodyB.m_invMass + contactPoint.m_normalOnB.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); |
| 257 | |
| 258 | b3Vector3 impulse_vector = contactPoint.m_normalOnB * impulse; |
| 259 | b3Printf("impulse = %f\n", impulse); |
| 260 | appliedImpulse = impulse; |
| 261 | bodyA.applyImpulse(impulse_vector, rel_pos1); |
| 262 | bodyB.applyImpulse(-impulse_vector, rel_pos2); |
| 263 | } |
| 264 | return appliedImpulse; |
| 265 | } |
| 266 | |
| 267 | class Tutorial : public CommonExampleInterface |
| 268 | { |
no test coverage detected