dali-physics/third-party/bullet3/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btContactConstraint.h"
  17 #include "BulletDynamics/Dynamics/btRigidBody.h"
  18 #include "LinearMath/btVector3.h"
  19 #include "btJacobianEntry.h"
  20 #include "btContactSolverInfo.h"
  21 #include "LinearMath/btMinMax.h"
  22 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
  23
  24 btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold, btRigidBody& rbA, btRigidBody& rbB)
  25         : btTypedConstraint(CONTACT_CONSTRAINT_TYPE, rbA, rbB),
  26           m_contactManifold(*contactManifold)
  27 {
  28 }
  29
  30 btContactConstraint::~btContactConstraint()
  31 {
  32 }
  33
  34 void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
  35 {
  36         m_contactManifold = *contactManifold;
  37 }
  38
  39 void btContactConstraint::getInfo1(btConstraintInfo1* info)
  40 {
  41 }
  42
  43 void btContactConstraint::getInfo2(btConstraintInfo2* info)
  44 {
  45 }
  46
  47 void btContactConstraint::buildJacobian()
  48 {
  49 }
  50
  51 #include "btContactConstraint.h"
  52 #include "BulletDynamics/Dynamics/btRigidBody.h"
  53 #include "LinearMath/btVector3.h"
  54 #include "btJacobianEntry.h"
  55 #include "btContactSolverInfo.h"
  56 #include "LinearMath/btMinMax.h"
  57 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
  58
  59 //response  between two dynamic objects without friction and no restitution, assuming 0 penetration depth
  60 btScalar resolveSingleCollision(
  61         btRigidBody* body1,
  62         btCollisionObject* colObj2,
  63         const btVector3& contactPositionWorld,
  64         const btVector3& contactNormalOnB,
  65         const btContactSolverInfo& solverInfo,
  66         btScalar distance)
  67 {
  68         btRigidBody* body2 = btRigidBody::upcast(colObj2);
  69
  70         const btVector3& normal = contactNormalOnB;
  71
  72         btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin();
  73         btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
  74
  75         btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
  76         btVector3 vel2 = body2 ? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0, 0, 0);
  77         btVector3 vel = vel1 - vel2;
  78         btScalar rel_vel;
  79         rel_vel = normal.dot(vel);
  80
  81         btScalar combinedRestitution = 0.f;
  82         btScalar restitution = combinedRestitution * -rel_vel;
  83
  84         btScalar positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
  85         btScalar velocityError = -(1.0f + restitution) * rel_vel;  // * damping;
  86         btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld, normal);
  87         btScalar denom1 = body2 ? body2->computeImpulseDenominator(contactPositionWorld, normal) : 0.f;
  88         btScalar relaxation = 1.f;
  89         btScalar jacDiagABInv = relaxation / (denom0 + denom1);
  90
  91         btScalar penetrationImpulse = positionalError * jacDiagABInv;
  92         btScalar velocityImpulse = velocityError * jacDiagABInv;
  93
  94         btScalar normalImpulse = penetrationImpulse + velocityImpulse;
  95         normalImpulse = 0.f > normalImpulse ? 0.f : normalImpulse;
  96
  97         body1->applyImpulse(normal * (normalImpulse), rel_pos1);
  98         if (body2)
  99                 body2->applyImpulse(-normal * (normalImpulse), rel_pos2);
 100
 101         return normalImpulse;
 102 }
 103
 104 //bilateral constraint between two dynamic objects
 105 void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
 106                                                         btRigidBody& body2, const btVector3& pos2,
 107                                                         btScalar distance, const btVector3& normal, btScalar& impulse, btScalar timeStep)
 108 {
 109         (void)timeStep;
 110         (void)distance;
 111
 112         btScalar normalLenSqr = normal.length2();
 113         btAssert(btFabs(normalLenSqr) < btScalar(1.1));
 114         if (normalLenSqr > btScalar(1.1))
 115         {
 116                 impulse = btScalar(0.);
 117                 return;
 118         }
 119         btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
 120         btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
 121         //this jacobian entry could be re-used for all iterations
 122
 123         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
 124         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
 125         btVector3 vel = vel1 - vel2;
 126
 127         btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
 128                                                 body2.getCenterOfMassTransform().getBasis().transpose(),
 129                                                 rel_pos1, rel_pos2, normal, body1.getInvInertiaDiagLocal(), body1.getInvMass(),
 130                                                 body2.getInvInertiaDiagLocal(), body2.getInvMass());
 131
 132         btScalar jacDiagAB = jac.getDiagonal();
 133         btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
 134
 135         btScalar rel_vel = jac.getRelativeVelocity(
 136                 body1.getLinearVelocity(),
 137                 body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
 138                 body2.getLinearVelocity(),
 139                 body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
 140
 141         rel_vel = normal.dot(vel);
 142
 143         //todo: move this into proper structure
 144         btScalar contactDamping = btScalar(0.2);
 145
 146 #ifdef ONLY_USE_LINEAR_MASS
 147         btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
 148         impulse = -contactDamping * rel_vel * massTerm;
 149 #else
 150         btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
 151         impulse = velocityImpulse;
 152 #endif
 153 }