Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp')
-rw-r--r--extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp387
1 files changed, 46 insertions, 341 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
index 012c321fd6d..d97096d9f26 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
@@ -22,6 +22,52 @@ subject to the following restrictions:
#include "LinearMath/btMinMax.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+
+
+btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
+:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
+ m_contactManifold(*contactManifold)
+{
+
+}
+
+btContactConstraint::~btContactConstraint()
+{
+
+}
+
+void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
+{
+ m_contactManifold = *contactManifold;
+}
+
+void btContactConstraint::getInfo1 (btConstraintInfo1* info)
+{
+
+}
+
+void btContactConstraint::getInfo2 (btConstraintInfo2* info)
+{
+
+}
+
+void btContactConstraint::buildJacobian()
+{
+
+}
+
+
+
+
+
+#include "btContactConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btContactSolverInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+
#define ASSERT2 btAssert
#define USE_INTERNAL_APPLY_IMPULSE 1
@@ -85,345 +131,4 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
-//response between two dynamic objects with friction
-btScalar resolveSingleCollision(
- btRigidBody& body1,
- btRigidBody& body2,
- btManifoldPoint& contactPoint,
- const btContactSolverInfo& solverInfo)
-{
-
- const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
- const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
- const btVector3& normal = contactPoint.m_normalWorldOnB;
-
- //constant over all iterations
- btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition();
- btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
-
- btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
- btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
- btVector3 vel = vel1 - vel2;
- btScalar rel_vel;
- rel_vel = normal.dot(vel);
-
- btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
-
- // btScalar damping = solverInfo.m_damping ;
- btScalar Kerp = solverInfo.m_erp;
- btScalar Kcor = Kerp *Kfps;
-
- btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
- btAssert(cpd);
- btScalar distance = cpd->m_penetration;
- btScalar positionalError = Kcor *-distance;
- btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
-
- btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
-
- btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;
-
- btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-
- // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
- btScalar oldNormalImpulse = cpd->m_appliedImpulse;
- btScalar sum = oldNormalImpulse + normalImpulse;
- cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
- normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
-
-#ifdef USE_INTERNAL_APPLY_IMPULSE
- if (body1.getInvMass())
- {
- body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
- }
- if (body2.getInvMass())
- {
- body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
- }
-#else //USE_INTERNAL_APPLY_IMPULSE
- body1.applyImpulse(normal*(normalImpulse), rel_pos1);
- body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
-
- return normalImpulse;
-}
-
-
-btScalar resolveSingleFriction(
- btRigidBody& body1,
- btRigidBody& body2,
- btManifoldPoint& contactPoint,
- const btContactSolverInfo& solverInfo)
-{
-
- (void)solverInfo;
-
- const btVector3& pos1 = contactPoint.getPositionWorldOnA();
- const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-
- btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
- btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-
- btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
- btAssert(cpd);
-
- btScalar combinedFriction = cpd->m_friction;
-
- btScalar limit = cpd->m_appliedImpulse * combinedFriction;
-
- if (cpd->m_appliedImpulse>btScalar(0.))
- //friction
- {
- //apply friction in the 2 tangential directions
-
- // 1st tangent
- btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
- btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
- btVector3 vel = vel1 - vel2;
-
- btScalar j1,j2;
-
- {
-
- btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
-
- // calculate j that moves us to zero relative velocity
- j1 = -vrel * cpd->m_jacDiagABInvTangent0;
- btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
- cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
- btSetMin(cpd->m_accumulatedTangentImpulse0, limit);
- btSetMax(cpd->m_accumulatedTangentImpulse0, -limit);
- j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
-
- }
- {
- // 2nd tangent
-
- btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
-
- // calculate j that moves us to zero relative velocity
- j2 = -vrel * cpd->m_jacDiagABInvTangent1;
- btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
- cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
- btSetMin(cpd->m_accumulatedTangentImpulse1, limit);
- btSetMax(cpd->m_accumulatedTangentImpulse1, -limit);
- j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
- }
-
-#ifdef USE_INTERNAL_APPLY_IMPULSE
- if (body1.getInvMass())
- {
- body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
- body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
- }
- if (body2.getInvMass())
- {
- body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
- body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);
- }
-#else //USE_INTERNAL_APPLY_IMPULSE
- body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
- body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
-
-
- }
- return cpd->m_appliedImpulse;
-}
-
-
-btScalar resolveSingleFrictionOriginal(
- btRigidBody& body1,
- btRigidBody& body2,
- btManifoldPoint& contactPoint,
- const btContactSolverInfo& solverInfo);
-
-btScalar resolveSingleFrictionOriginal(
- btRigidBody& body1,
- btRigidBody& body2,
- btManifoldPoint& contactPoint,
- const btContactSolverInfo& solverInfo)
-{
-
- (void)solverInfo;
-
- const btVector3& pos1 = contactPoint.getPositionWorldOnA();
- const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-
- btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
- btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-
- btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
- btAssert(cpd);
-
- btScalar combinedFriction = cpd->m_friction;
-
- btScalar limit = cpd->m_appliedImpulse * combinedFriction;
- //if (contactPoint.m_appliedImpulse>btScalar(0.))
- //friction
- {
- //apply friction in the 2 tangential directions
-
- {
- // 1st tangent
- btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
- btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
- btVector3 vel = vel1 - vel2;
-
- btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
-
- // calculate j that moves us to zero relative velocity
- btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
- btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
- btSetMin(total, limit);
- btSetMax(total, -limit);
- j = total - cpd->m_accumulatedTangentImpulse0;
- cpd->m_accumulatedTangentImpulse0 = total;
- body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
- body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
- }
-
-
- {
- // 2nd tangent
- btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
- btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
- btVector3 vel = vel1 - vel2;
-
- btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
-
- // calculate j that moves us to zero relative velocity
- btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
- btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
- btSetMin(total, limit);
- btSetMax(total, -limit);
- j = total - cpd->m_accumulatedTangentImpulse1;
- cpd->m_accumulatedTangentImpulse1 = total;
- body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
- body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
- }
- }
- return cpd->m_appliedImpulse;
-}
-
-
-//velocity + friction
-//response between two dynamic objects with friction
-btScalar resolveSingleCollisionCombined(
- btRigidBody& body1,
- btRigidBody& body2,
- btManifoldPoint& contactPoint,
- const btContactSolverInfo& solverInfo)
-{
-
- const btVector3& pos1 = contactPoint.getPositionWorldOnA();
- const btVector3& pos2 = contactPoint.getPositionWorldOnB();
- const btVector3& normal = contactPoint.m_normalWorldOnB;
-
- btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
- btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-
- btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
- btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
- btVector3 vel = vel1 - vel2;
- btScalar rel_vel;
- rel_vel = normal.dot(vel);
-
- btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
-
- //btScalar damping = solverInfo.m_damping ;
- btScalar Kerp = solverInfo.m_erp;
- btScalar Kcor = Kerp *Kfps;
-
- btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
- btAssert(cpd);
- btScalar distance = cpd->m_penetration;
- btScalar positionalError = Kcor *-distance;
- btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
-
- btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
-
- btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;
-
- btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-
- // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
- btScalar oldNormalImpulse = cpd->m_appliedImpulse;
- btScalar sum = oldNormalImpulse + normalImpulse;
- cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
- normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
-
-
-#ifdef USE_INTERNAL_APPLY_IMPULSE
- if (body1.getInvMass())
- {
- body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
- }
- if (body2.getInvMass())
- {
- body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
- }
-#else //USE_INTERNAL_APPLY_IMPULSE
- body1.applyImpulse(normal*(normalImpulse), rel_pos1);
- body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
-
- {
- //friction
- btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
- btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
- btVector3 vel = vel1 - vel2;
-
- rel_vel = normal.dot(vel);
-
-
- btVector3 lat_vel = vel - normal * rel_vel;
- btScalar lat_rel_vel = lat_vel.length();
-
- btScalar combinedFriction = cpd->m_friction;
-
- if (cpd->m_appliedImpulse > 0)
- if (lat_rel_vel > SIMD_EPSILON)
- {
- lat_vel /= lat_rel_vel;
- btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
- btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
- btScalar friction_impulse = lat_rel_vel /
- (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
- btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
-
- btSetMin(friction_impulse, normal_impulse);
- btSetMax(friction_impulse, -normal_impulse);
- body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
- body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
- }
- }
-
-
-
- return normalImpulse;
-}
-
-btScalar resolveSingleFrictionEmpty(
- btRigidBody& body1,
- btRigidBody& body2,
- btManifoldPoint& contactPoint,
- const btContactSolverInfo& solverInfo);
-
-btScalar resolveSingleFrictionEmpty(
- btRigidBody& body1,
- btRigidBody& body2,
- btManifoldPoint& contactPoint,
- const btContactSolverInfo& solverInfo)
-{
- (void)contactPoint;
- (void)body1;
- (void)body2;
- (void)solverInfo;
-
-
- return btScalar(0.);
-}
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-01 19:03:24 +0300