diff options
Diffstat (limited to 'extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp')
| -rw-r--r-- | extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp | 162 |
1 files changed, 15 insertions, 147 deletions
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp index 10bb4af0a7d..8345e23517f 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp @@ -17,23 +17,20 @@ #define ASSERT2 assert - +//some values to find stable penalty method tresholds +float MAX_FRICTION = 100.f; static SimdScalar ContactThreshold = -10.0f; - float useGlobalSettingContacts = false;//true; - SimdScalar contactDamping = 0.2f; SimdScalar contactTau = .02f;//0.02f;//*0.02f; - SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution) { return 0.f; // return restitution * GEN_min(1.0f, rel_vel / ContactThreshold); } -float MAX_FRICTION = 100.f; SimdScalar calculateCombinedFriction(RigidBody& body0,RigidBody& body1) { @@ -46,54 +43,11 @@ SimdScalar calculateCombinedFriction(RigidBody& body0,RigidBody& body1) } -void applyFrictionInContactPointOld(RigidBody& body1, const SimdVector3& pos1, - RigidBody& body2, const SimdVector3& pos2, - const SimdVector3& normal,float normalImpulse, - const ContactSolverInfo& solverInfo) -{ - - - if (normalImpulse>0.f) - { - SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); - SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); - - SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - SimdVector3 vel = vel1 - vel2; - - SimdScalar rel_vel = normal.dot(vel); - - float combinedFriction = calculateCombinedFriction(body1,body2); - -#define PER_CONTACT_FRICTION -#ifdef PER_CONTACT_FRICTION - SimdVector3 lat_vel = vel - normal * rel_vel; - SimdScalar lat_rel_vel = lat_vel.length(); - - if (lat_rel_vel > SIMD_EPSILON) - { - lat_vel /= lat_rel_vel; - SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel); - SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel); - SimdScalar frictionMaxImpulse = lat_rel_vel / - (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); - SimdScalar frictionImpulse = (normalImpulse) * combinedFriction; - GEN_set_min(frictionImpulse,frictionMaxImpulse ); - - body1.applyImpulse(lat_vel * -frictionImpulse, rel_pos1); - body2.applyImpulse(lat_vel * frictionImpulse, rel_pos2); - - } -#endif - } -} - //bilateral constraint between two dynamic objects void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1, RigidBody& body2, const SimdVector3& pos2, - SimdScalar depth, const SimdVector3& normal,SimdScalar& impulse ,float timeStep) + SimdScalar distance, const SimdVector3& normal,SimdScalar& impulse ,float timeStep) { float normalLenSqr = normal.length2(); ASSERT2(fabs(normalLenSqr) < 1.1f); @@ -110,10 +64,7 @@ void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1, SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); SimdVector3 vel = vel1 - vel2; - SimdScalar rel_vel; - /* - JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), body2.getCenterOfMassTransform().getBasis().transpose(), rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), @@ -130,100 +81,22 @@ void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1, float a; a=jacDiagABInv; - */ - rel_vel = normal.dot(vel); - - - - - /*int color = 255+255*256; - - DrawRasterizerLine(pos1,pos1+normal,color); -*/ + rel_vel = normal.dot(vel); - SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); - - impulse = - contactDamping * rel_vel * massTerm;//jacDiagABInv; - - //SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; - //impulse = velocityImpulse; +#ifdef ONLY_USE_LINEAR_MASS + SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); + impulse = - contactDamping * rel_vel * massTerm; +#else + SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; + impulse = velocityImpulse; +#endif } - -//velocity + friction -//response between two dynamic objects with friction -float resolveSingleCollision(RigidBody& body1, const SimdVector3& pos1, - RigidBody& body2, const SimdVector3& pos2, - SimdScalar depth, const SimdVector3& normal, - const ContactSolverInfo& solverInfo - ) -{ - - float normalLenSqr = normal.length2(); - ASSERT2(fabs(normalLenSqr) < 1.1f); - if (normalLenSqr > 1.1f) - return 0.f; - - SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); - SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); - //this jacobian entry could be re-used for all iterations - - SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - SimdVector3 vel = vel1 - vel2; - SimdScalar rel_vel; - rel_vel = normal.dot(vel); - -// if (rel_vel< 0.f)//-SIMD_EPSILON) -// { - float combinedRestitution = body1.getRestitution() * body2.getRestitution(); - - SimdScalar rest = restitutionCurve(rel_vel, combinedRestitution); - - SimdScalar timeCorrection = 360.f*solverInfo.m_timeStep; - float damping = solverInfo.m_damping ; - float tau = solverInfo.m_tau; - - if (useGlobalSettingContacts) - { - damping = contactDamping; - tau = contactTau; - } - - if (depth < 0.f) - return 0.f;//bdepth = 0.f; - - SimdScalar penetrationImpulse = (depth*tau*timeCorrection);// * massTerm;//jacDiagABInv - - SimdScalar velocityImpulse = -(1.0f + rest) * damping * rel_vel; - - SimdScalar impulse = penetrationImpulse + velocityImpulse; - SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(normal); - SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(normal); - impulse /= - (body1.getInvMass() + body2.getInvMass() + normal.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); - - if (impulse > 0.f) - { - - body1.applyImpulse(normal*(impulse), rel_pos1); - body2.applyImpulse(-normal*(impulse), rel_pos2); - } else - { - impulse = 0.f; - } - - return impulse;//velocityImpulse;//impulse; - -} - - - //velocity + friction //response between two dynamic objects with friction float resolveSingleCollisionWithFriction( @@ -232,7 +105,7 @@ float resolveSingleCollisionWithFriction( const SimdVector3& pos1, RigidBody& body2, const SimdVector3& pos2, - SimdScalar depth, + SimdScalar distance, const SimdVector3& normal, const ContactSolverInfo& solverInfo @@ -283,22 +156,17 @@ SimdScalar rel_vel; damping = contactDamping; tau = contactTau; } - SimdScalar penetrationImpulse = (depth* tau *timeCorrection) * jacDiagABInv; + SimdScalar penetrationImpulse = (-distance* tau *timeCorrection) * jacDiagABInv; - if (penetrationImpulse < 0.f) - penetrationImpulse = 0.f; - SimdScalar velocityImpulse = -(1.0f + rest) * damping * rel_vel * jacDiagABInv; SimdScalar friction_impulse = 0.f; - if (velocityImpulse <= 0.f) - velocityImpulse = 0.f; + SimdScalar totalimpulse = penetrationImpulse+velocityImpulse; -// SimdScalar impulse = penetrationImpulse + velocityImpulse; - //if (impulse > 0.f) + if (totalimpulse > 0.f) { // SimdVector3 impulse_vector = normal * impulse; body1.applyImpulse(normal*(velocityImpulse+penetrationImpulse), rel_pos1); |