diff options
Diffstat (limited to 'extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp')
| -rw-r--r-- | extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp | 324 |
1 files changed, 324 insertions, 0 deletions
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp new file mode 100644 index 00000000000..e3603ccec67 --- /dev/null +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp @@ -0,0 +1,324 @@ +/* + * Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com + * + * Permission to use, copy, modify, distribute and sell this software + * and its documentation for any purpose is hereby granted without fee, + * provided that the above copyright notice appear in all copies. + * Erwin Coumans makes no representations about the suitability + * of this software for any purpose. + * It is provided "as is" without express or implied warranty. +*/ +#include "ContactConstraint.h" +#include "Dynamics/RigidBody.h" +#include "SimdVector3.h" +#include "JacobianEntry.h" +#include "ContactSolverInfo.h" +#include "GEN_minmax.h" + +#define ASSERT2 assert + + +static SimdScalar ContactThreshold = -10.0f; + +float useGlobalSettingContacts = false;//true; + +SimdScalar contactDamping = 0.9f; +SimdScalar contactTau = .2f;//0.02f;//*0.02f; + + + +SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution) +{ + return restitution; +// return restitution * GEN_min(1.0f, rel_vel / ContactThreshold); +} + +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); + +#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) * solverInfo.m_friction; + 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) +{ + float normalLenSqr = normal.length2(); + ASSERT2(fabs(normalLenSqr) < 1.1f); + if (normalLenSqr > 1.1f) + { + impulse = 0.f; + return; + } + 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; + + /* + + JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), + body2.getCenterOfMassTransform().getBasis().transpose(), + rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), + body2.getInvInertiaDiagLocal(),body2.getInvMass()); + + SimdScalar jacDiagAB = jac.getDiagonal(); + SimdScalar jacDiagABInv = 1.f / jacDiagAB; + + SimdScalar rel_vel = jac.getRelativeVelocity( + body1.getLinearVelocity(), + body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(), + body2.getLinearVelocity(), + body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); + float a; + a=jacDiagABInv; + + */ + rel_vel = normal.dot(vel); + + + + + /*int color = 255+255*256; + + DrawRasterizerLine(pos1,pos1+normal,color); +*/ + + + SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); + + float contactDamping = 0.9f; + impulse = - contactDamping * rel_vel * massTerm;//jacDiagABInv; + + //SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; + //impulse = velocityImpulse; + +} + + + + + +//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) +// { + SimdScalar rest = restitutionCurve(rel_vel, solverInfo.m_restitution); + + SimdScalar timeCorrection = 1.f;///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( + + 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 + + JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), + body2.getCenterOfMassTransform().getBasis().transpose(), + rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), + body2.getInvInertiaDiagLocal(),body2.getInvMass()); + + SimdScalar jacDiagAB = jac.getDiagonal(); + SimdScalar jacDiagABInv = 1.f / jacDiagAB; + SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + SimdVector3 vel = vel1 - vel2; +SimdScalar rel_vel; + /* rel_vel = jac.getRelativeVelocity( + body1.getLinearVelocity(), + body1.getTransform().getBasis().transpose() * body1.getAngularVelocity(), + body2.getLinearVelocity(), + body2.getTransform().getBasis().transpose() * body2.getAngularVelocity()); +*/ + rel_vel = normal.dot(vel); + +// if (rel_vel< 0.f)//-SIMD_EPSILON) +// { + SimdScalar rest = restitutionCurve(rel_vel, solverInfo.m_restitution); +// SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); + + SimdScalar timeCorrection = 0.5f / solverInfo.m_timeStep ; + + float damping = solverInfo.m_damping ; + float tau = solverInfo.m_tau; + + if (useGlobalSettingContacts) + { + damping = contactDamping; + tau = contactTau; + } + SimdScalar penetrationImpulse = (depth* 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 impulse = penetrationImpulse + velocityImpulse; + //if (impulse > 0.f) + { +// SimdVector3 impulse_vector = normal * impulse; + body1.applyImpulse(normal*(velocityImpulse+penetrationImpulse), rel_pos1); + + body2.applyImpulse(-normal*(velocityImpulse+penetrationImpulse), rel_pos2); + + //friction + + { + + SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + SimdVector3 vel = vel1 - vel2; + + rel_vel = normal.dot(vel); + +#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); + friction_impulse = lat_rel_vel / + (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); + SimdScalar normal_impulse = (penetrationImpulse+ + velocityImpulse) * solverInfo.m_friction; + GEN_set_min(friction_impulse, normal_impulse); + + body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1); + body2.applyImpulse(lat_vel * friction_impulse, rel_pos2); + + } +#endif + } + } + return velocityImpulse + friction_impulse; +} |