| 21 | } |
| 22 | |
| 23 | void CPHContactBodyEffector::Apply() |
| 24 | { |
| 25 | const dReal* linear_velocity = dBodyGetLinearVel(m_body); |
| 26 | dReal linear_velocity_smag = dDOT(linear_velocity, linear_velocity); |
| 27 | dReal linear_velocity_mag = _sqrt(linear_velocity_smag); |
| 28 | dReal effect = 10000.f * m_recip_flotation * m_recip_flotation; |
| 29 | dMass mass; |
| 30 | dBodyGetMass(m_body, &mass); |
| 31 | dReal l_air = linear_velocity_mag * effect; // force/velocity !!! |
| 32 | if (l_air > mass.mass / fixed_step) |
| 33 | l_air = mass.mass / fixed_step; // validate |
| 34 | |
| 35 | if (!fis_zero(l_air)) |
| 36 | { |
| 37 | dVector3 force = {-linear_velocity[0] * l_air, -linear_velocity[1] * l_air, -linear_velocity[2] * l_air, 0.f}; |
| 38 | |
| 39 | if (!m_material->Flags.is(SGameMtl::flPassable)) |
| 40 | { |
| 41 | dVector3& norm = m_contact.geom.normal; |
| 42 | accurate_normalize(norm); |
| 43 | dMass m; |
| 44 | dBodyGetMass(m_body, &m); |
| 45 | dReal prg = 1.f * dDOT(force, norm); //+dDOT(linear_velocity,norm)*m.mass/fixed_step |
| 46 | force[0] -= prg * norm[0]; |
| 47 | force[1] -= prg * norm[1]; |
| 48 | force[2] -= prg * norm[2]; |
| 49 | } |
| 50 | dBodyAddForce(m_body, force[0], force[1], force[2]); |
| 51 | } |
| 52 | dBodySetData(m_body, NULL); |
| 53 | } |
no test coverage detected