diff options
author | Erwin Coumans <blender@erwincoumans.com> | 2006-10-23 06:54:30 +0400 |
---|---|---|
committer | Erwin Coumans <blender@erwincoumans.com> | 2006-10-23 06:54:30 +0400 |
commit | 44d16f056215e6068f0b186a0ab766165cf3966e (patch) | |
tree | f0ad85e29c32563d1d4c1c46db4e2cd22f7f78dc /extern/bullet2/src/BulletDynamics/ConstraintSolver | |
parent | e459764b4b056959e354edca3868a91ff9bc272f (diff) |
Added refactored Bullet 2.x library. Important: these files are not part of the Blender build yet. First, the integration will be updated to make use of the new Bullet version. Then all build systems needs to be updated.
The refactoring didn't leave a single file the same, all filenames and classes have bt prefix, methodnames start with lowercase, a single headerfile can be included, and also a single include path.
Plan is to make use of this Bullet 2.x version in extern/bullet2 within the coming weeks, then extern/bullet can be discarded/ignored/content removed.
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver')
17 files changed, 2402 insertions, 0 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h new file mode 100644 index 00000000000..e073797f989 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -0,0 +1,41 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef CONSTRAINT_SOLVER_H +#define CONSTRAINT_SOLVER_H + +class btPersistentManifold; +class btRigidBody; + +struct btContactSolverInfo; +struct btBroadphaseProxy; +class btIDebugDraw; + +/// btConstraintSolver provides solver interface +class btConstraintSolver +{ + +public: + + virtual ~btConstraintSolver() {} + + virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0; + +}; + + + + +#endif //CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp new file mode 100644 index 00000000000..9019035f586 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -0,0 +1,234 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#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 assert + +//some values to find stable tresholds + +float useGlobalSettingContacts = false;//true; +btScalar contactDamping = 0.2f; +btScalar contactTau = .02f;//0.02f;//*0.02f; + + + + + + + +//bilateral constraint between two dynamic objects +void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, + btRigidBody& body2, const btVector3& pos2, + btScalar distance, const btVector3& normal,btScalar& impulse ,float timeStep) +{ + float normalLenSqr = normal.length2(); + ASSERT2(fabs(normalLenSqr) < 1.1f); + if (normalLenSqr > 1.1f) + { + impulse = 0.f; + return; + } + btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); + //this jacobian entry could be re-used for all iterations + + btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + + btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), + body2.getCenterOfMassTransform().getBasis().transpose(), + rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), + body2.getInvInertiaDiagLocal(),body2.getInvMass()); + + btScalar jacDiagAB = jac.getDiagonal(); + btScalar jacDiagABInv = 1.f / jacDiagAB; + + btScalar 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); + + +#ifdef ONLY_USE_LINEAR_MASS + btScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); + impulse = - contactDamping * rel_vel * massTerm; +#else + btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; + impulse = velocityImpulse; +#endif +} + + + + +//velocity + friction +//response between two dynamic objects with friction +float resolveSingleCollision( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo + + ) +{ + + const btVector3& pos1 = contactPoint.getPositionWorldOnA(); + const btVector3& pos2 = contactPoint.getPositionWorldOnB(); + + +// printf("distance=%f\n",distance); + + 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 = 1.f / solverInfo.m_timeStep ; + + float damping = solverInfo.m_damping ; + float Kerp = solverInfo.m_erp; + + if (useGlobalSettingContacts) + { + damping = contactDamping; + Kerp = contactTau; + } + + float Kcor = Kerp *Kfps; + + //printf("dist=%f\n",distance); + + btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; + assert(cpd); + + btScalar distance = cpd->m_penetration;//contactPoint.getDistance(); + + + //distance = 0.f; + btScalar positionalError = Kcor *-distance; + //jacDiagABInv; + 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 + float oldNormalImpulse = cpd->m_appliedImpulse; + float sum = oldNormalImpulse + normalImpulse; + cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum; + + normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse; + + body1.applyImpulse(normal*(normalImpulse), rel_pos1); + body2.applyImpulse(-normal*(normalImpulse), rel_pos2); + + return normalImpulse; +} + + +float resolveSingleFriction( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& 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; + assert(cpd); + + float combinedFriction = cpd->m_friction; + + btScalar limit = cpd->m_appliedImpulse * combinedFriction; + //if (contactPoint.m_appliedImpulse>0.f) + //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; + float total = cpd->m_accumulatedTangentImpulse0 + j; + GEN_set_min(total, limit); + GEN_set_max(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; + float total = cpd->m_accumulatedTangentImpulse1 + j; + GEN_set_min(total, limit); + GEN_set_max(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; +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h new file mode 100644 index 00000000000..42ded30ae04 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -0,0 +1,104 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef CONTACT_CONSTRAINT_H +#define CONTACT_CONSTRAINT_H + +//todo: make into a proper class working with the iterative constraint solver + +class btRigidBody; +#include "LinearMath/btVector3.h" +#include "LinearMath/btScalar.h" +struct btContactSolverInfo; +class btManifoldPoint; + +enum { + DEFAULT_CONTACT_SOLVER_TYPE=0, + CONTACT_SOLVER_TYPE1, + CONTACT_SOLVER_TYPE2, + USER_CONTACT_SOLVER_TYPE1, + MAX_CONTACT_SOLVER_TYPES +}; + + +typedef float (*ContactSolverFunc)(btRigidBody& body1, + btRigidBody& body2, + class btManifoldPoint& contactPoint, + const btContactSolverInfo& info); + +///stores some extra information to each contact point. It is not in the contact point, because that want to keep the collision detection independent from the constraint solver. +struct btConstraintPersistentData +{ + inline btConstraintPersistentData() + :m_appliedImpulse(0.f), + m_prevAppliedImpulse(0.f), + m_accumulatedTangentImpulse0(0.f), + m_accumulatedTangentImpulse1(0.f), + m_jacDiagABInv(0.f), + m_persistentLifeTime(0), + m_restitution(0.f), + m_friction(0.f), + m_penetration(0.f), + m_contactSolverFunc(0), + m_frictionSolverFunc(0) + { + } + + + /// total applied impulse during most recent frame + float m_appliedImpulse; + float m_prevAppliedImpulse; + float m_accumulatedTangentImpulse0; + float m_accumulatedTangentImpulse1; + + float m_jacDiagABInv; + float m_jacDiagABInvTangent0; + float m_jacDiagABInvTangent1; + int m_persistentLifeTime; + float m_restitution; + float m_friction; + float m_penetration; + btVector3 m_frictionWorldTangential0; + btVector3 m_frictionWorldTangential1; + + ContactSolverFunc m_contactSolverFunc; + ContactSolverFunc m_frictionSolverFunc; + +}; + +///bilateral constraint between two dynamic objects +///positive distance = separation, negative distance = penetration +void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, + btRigidBody& body2, const btVector3& pos2, + btScalar distance, const btVector3& normal,btScalar& impulse ,float timeStep); + + +///contact constraint resolution: +///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint +///positive distance = separation, negative distance = penetration +float resolveSingleCollision( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& info); + +float resolveSingleFriction( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo + ); + +#endif //CONTACT_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h new file mode 100644 index 00000000000..ed1ba6ac1ba --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -0,0 +1,47 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef CONTACT_SOLVER_INFO +#define CONTACT_SOLVER_INFO + + +struct btContactSolverInfo +{ + + inline btContactSolverInfo() + { + m_tau = 0.6f; + m_damping = 1.0f; + m_friction = 0.3f; + m_restitution = 0.f; + m_maxErrorReduction = 20.f; + m_numIterations = 10; + m_erp = 0.4f; + m_sor = 1.3f; + } + + float m_tau; + float m_damping; + float m_friction; + float m_timeStep; + float m_restitution; + int m_numIterations; + float m_maxErrorReduction; + float m_sor; + float m_erp; + +}; + +#endif //CONTACT_SOLVER_INFO diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp new file mode 100644 index 00000000000..a8ab1bbad3a --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -0,0 +1,250 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btGeneric6DofConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" + +static const btScalar kSign[] = { 1.0f, -1.0f, 1.0f }; +static const int kAxisA[] = { 1, 0, 0 }; +static const int kAxisB[] = { 2, 2, 1 }; + +btGeneric6DofConstraint::btGeneric6DofConstraint() +{ +} + +btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) +: btTypedConstraint(rbA, rbB) +, m_frameInA(frameInA) +, m_frameInB(frameInB) +{ + //free means upper < lower, + //locked means upper == lower + //limited means upper > lower + //so start all locked + for (int i=0; i<6;++i) + { + m_lowerLimit[i] = 0.0f; + m_upperLimit[i] = 0.0f; + m_accumulatedImpulse[i] = 0.0f; + } + +} + + +void btGeneric6DofConstraint::buildJacobian() +{ + btVector3 normal(0,0,0); + + const btVector3& pivotInA = m_frameInA.getOrigin(); + const btVector3& pivotInB = m_frameInB.getOrigin(); + + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); + + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + int i; + //linear part + for (i=0;i<3;i++) + { + if (isLimited(i)) + { + normal[i] = 1; + + // Create linear atom + new (&m_jacLinear[i]) btJacobianEntry( + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(), + m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(), + normal, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + + // Apply accumulated impulse + btVector3 impulse_vector = m_accumulatedImpulse[i] * normal; + + m_rbA.applyImpulse( impulse_vector, rel_pos1); + m_rbB.applyImpulse(-impulse_vector, rel_pos2); + + normal[i] = 0; + } + } + + // angular part + for (i=0;i<3;i++) + { + if (isLimited(i+3)) + { + btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); + btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); + + // Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe + btVector3 axis = kSign[i] * axisA.cross(axisB); + + // Create angular atom + new (&m_jacAng[i]) btJacobianEntry(axis, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + // Apply accumulated impulse + btVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis; + + m_rbA.applyTorqueImpulse( impulse_vector); + m_rbB.applyTorqueImpulse(-impulse_vector); + } + } +} + +void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) +{ + btScalar tau = 0.1f; + btScalar damping = 1.0f; + + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); + + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 normal(0,0,0); + int i; + + // linear + for (i=0;i<3;i++) + { + if (isLimited(i)) + { + btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + + normal[i] = 1; + btScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal(); + + //velocity error (first order error) + btScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); + + btScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv; + m_accumulatedImpulse[i] += impulse; + + btVector3 impulse_vector = normal * impulse; + m_rbA.applyImpulse( impulse_vector, rel_pos1); + m_rbB.applyImpulse(-impulse_vector, rel_pos2); + + normal[i] = 0; + } + } + + // angular + for (i=0;i<3;i++) + { + if (isLimited(i+3)) + { + btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + btScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal(); + + //velocity error (first order error) + btScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + + //positional error (zeroth order error) + btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); + btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); + + btScalar rel_pos = kSign[i] * axisA.dot(axisB); + + //impulse + btScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv; + m_accumulatedImpulse[i + 3] += impulse; + + // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above) + btVector3 axis = kSign[i] * axisA.cross(axisB); + btVector3 impulse_vector = axis * impulse; + + m_rbA.applyTorqueImpulse( impulse_vector); + m_rbB.applyTorqueImpulse(-impulse_vector); + } + } +} + +void btGeneric6DofConstraint::updateRHS(btScalar timeStep) +{ + +} + +btScalar btGeneric6DofConstraint::computeAngle(int axis) const + { + btScalar angle; + + switch (axis) + { + case 0: + { + btVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1); + btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1); + btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2); + + btScalar s = v1.dot(w2); + btScalar c = v1.dot(v2); + + angle = btAtan2( s, c ); + } + break; + + case 1: + { + btVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2); + btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2); + btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0); + + btScalar s = w1.dot(u2); + btScalar c = w1.dot(w2); + + angle = btAtan2( s, c ); + } + break; + + case 2: + { + btVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0); + btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0); + btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1); + + btScalar s = u1.dot(v2); + btScalar c = u1.dot(u2); + + angle = btAtan2( s, c ); + } + break; + default: assert ( 0 ) ; break ; + } + + return angle; + } + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h new file mode 100644 index 00000000000..329048b5737 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -0,0 +1,114 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef GENERIC_6DOF_CONSTRAINT_H +#define GENERIC_6DOF_CONSTRAINT_H + +#include "LinearMath/btVector3.h" + +#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "btTypedConstraint.h" + +class btRigidBody; + + + +/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space +/// btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked' +/// Work in progress (is still a Hinge actually) +class btGeneric6DofConstraint : public btTypedConstraint +{ + btJacobianEntry m_jacLinear[3]; // 3 orthogonal linear constraints + btJacobianEntry m_jacAng[3]; // 3 orthogonal angular constraints + + btTransform m_frameInA; // the constraint space w.r.t body A + btTransform m_frameInB; // the constraint space w.r.t body B + + btScalar m_lowerLimit[6]; // the constraint lower limits + btScalar m_upperLimit[6]; // the constraint upper limits + + btScalar m_accumulatedImpulse[6]; + + +public: + btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ); + + btGeneric6DofConstraint(); + + virtual void buildJacobian(); + + virtual void solveConstraint(btScalar timeStep); + + void updateRHS(btScalar timeStep); + + btScalar computeAngle(int axis) const; + + void setLinearLowerLimit(const btVector3& linearLower) + { + m_lowerLimit[0] = linearLower.getX(); + m_lowerLimit[1] = linearLower.getY(); + m_lowerLimit[2] = linearLower.getZ(); + } + + void setLinearUpperLimit(const btVector3& linearUpper) + { + m_upperLimit[0] = linearUpper.getX(); + m_upperLimit[1] = linearUpper.getY(); + m_upperLimit[2] = linearUpper.getZ(); + } + + void setAngularLowerLimit(const btVector3& angularLower) + { + m_lowerLimit[3] = angularLower.getX(); + m_lowerLimit[4] = angularLower.getY(); + m_lowerLimit[5] = angularLower.getZ(); + } + + void setAngularUpperLimit(const btVector3& angularUpper) + { + m_upperLimit[3] = angularUpper.getX(); + m_upperLimit[4] = angularUpper.getY(); + m_upperLimit[5] = angularUpper.getZ(); + } + + //first 3 are linear, next 3 are angular + void SetLimit(int axis, btScalar lo, btScalar hi) + { + m_lowerLimit[axis] = lo; + m_upperLimit[axis] = hi; + } + + //free means upper < lower, + //locked means upper == lower + //limited means upper > lower + //limitIndex: first 3 are linear, next 3 are angular + bool isLimited(int limitIndex) + { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + + const btRigidBody& getRigidBodyA() const + { + return m_rbA; + } + const btRigidBody& getRigidBodyB() const + { + return m_rbB; + } + + +}; + +#endif //GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp new file mode 100644 index 00000000000..b507e4c7bb8 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -0,0 +1,275 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btHingeConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" + + +btHingeConstraint::btHingeConstraint() +{ +} + +btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, + btVector3& axisInA,btVector3& axisInB) +:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), +m_axisInA(axisInA), +m_axisInB(-axisInB), +m_angularOnly(false) +{ + +} + + +btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) +:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), +m_axisInA(axisInA), +//fixed axis in worldspace +m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA), +m_angularOnly(false) +{ + +} + +void btHingeConstraint::buildJacobian() +{ + m_appliedImpulse = 0.f; + + btVector3 normal(0,0,0); + + if (!m_angularOnly) + { + for (int i=0;i<3;i++) + { + normal[i] = 1; + new (&m_jac[i]) btJacobianEntry( + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), + m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), + normal, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + normal[i] = 0; + } + } + + //calculate two perpendicular jointAxis, orthogonal to hingeAxis + //these two jointAxis require equal angular velocities for both bodies + + //this is unused for now, it's a todo + btVector3 axisWorldA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + btVector3 jointAxis0; + btVector3 jointAxis1; + btPlaneSpace1(axisWorldA,jointAxis0,jointAxis1); + + new (&m_jacAng[0]) btJacobianEntry(jointAxis0, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + new (&m_jacAng[1]) btJacobianEntry(jointAxis1, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + +} + +void btHingeConstraint::solveConstraint(btScalar timeStep) +{ +//#define NEW_IMPLEMENTATION + +#ifdef NEW_IMPLEMENTATION + btScalar tau = 0.3f; + btScalar damping = 1.f; + + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + + // Dirk: Don't we need to update this after each applied impulse + btVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + btVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + + if (!m_angularOnly) + { + btVector3 normal(0,0,0); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + // Dirk: Get new angular velocity since it changed after applying an impulse + angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + //velocity error (first order error) + btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); + + btScalar impulse = tau*depth/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv; + + btVector3 impulse_vector = normal * impulse; + m_rbA.applyImpulse( impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); + m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); + + normal[i] = 0; + } + } + + ///solve angular part + + // get axes in world space + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB; + + // constraint axes in world space + btVector3 jointAxis0; + btVector3 jointAxis1; + btPlaneSpace1(axisA,jointAxis0,jointAxis1); + + + // Dirk: Get new angular velocity since it changed after applying an impulse + angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + btScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal(); + btScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + float tau1 = tau;//0.f; + + btScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0; + btVector3 angular_impulse0 = jointAxis0 * impulse0; + + m_rbA.applyTorqueImpulse( angular_impulse0); + m_rbB.applyTorqueImpulse(-angular_impulse0); + + + + // Dirk: Get new angular velocity since it changed after applying an impulse + angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + btScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal(); + btScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB);; + + btScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1; + btVector3 angular_impulse1 = jointAxis1 * impulse1; + + m_rbA.applyTorqueImpulse( angular_impulse1); + m_rbB.applyTorqueImpulse(-angular_impulse1); + +#else + + + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + + btVector3 normal(0,0,0); + btScalar tau = 0.3f; + btScalar damping = 1.f; + +//linear part + { + for (int i=0;i<3;i++) + { + normal[i] = 1; + btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + btScalar impulse = depth*tau/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; + m_appliedImpulse += impulse; + btVector3 impulse_vector = normal * impulse; + m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); + m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); + + normal[i] = 0; + } + } + + ///solve angular part + + // get axes in world space + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB; + + const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); + const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); + btVector3 angA = angVelA - axisA * axisA.dot(angVelA); + btVector3 angB = angVelB - axisB * axisB.dot(angVelB); + btVector3 velrel = angA-angB; + + //solve angular velocity correction + float relaxation = 1.f; + float len = velrel.length(); + if (len > 0.00001f) + { + btVector3 normal = velrel.normalized(); + float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + + getRigidBodyB().computeAngularImpulseDenominator(normal); + // scale for mass and relaxation + velrel *= (1.f/denom) * 0.9; + } + + //solve angular positional correction + btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep); + float len2 = angularError.length(); + if (len2>0.00001f) + { + btVector3 normal2 = angularError.normalized(); + float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + + getRigidBodyB().computeAngularImpulseDenominator(normal2); + angularError *= (1.f/denom2) * relaxation; + } + + m_rbA.applyTorqueImpulse(-velrel+angularError); + m_rbB.applyTorqueImpulse(velrel-angularError); + +#endif + +} + +void btHingeConstraint::updateRHS(btScalar timeStep) +{ + +} + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h new file mode 100644 index 00000000000..3bade2b091d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -0,0 +1,73 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef HINGECONSTRAINT_H +#define HINGECONSTRAINT_H + +#include "LinearMath/btVector3.h" + +#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "btTypedConstraint.h" + +class btRigidBody; + + +/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space +/// axis defines the orientation of the hinge axis +class btHingeConstraint : public btTypedConstraint +{ + btJacobianEntry m_jac[3]; //3 orthogonal linear constraints + btJacobianEntry m_jacAng[2]; //2 orthogonal angular constraints + + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btVector3 m_axisInA; + btVector3 m_axisInB; + + bool m_angularOnly; + +public: + + btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,btVector3& axisInA,btVector3& axisInB); + + btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA); + + btHingeConstraint(); + + virtual void buildJacobian(); + + virtual void solveConstraint(btScalar timeStep); + + void updateRHS(btScalar timeStep); + + const btRigidBody& getRigidBodyA() const + { + return m_rbA; + } + const btRigidBody& getRigidBodyB() const + { + return m_rbB; + } + + void setAngularOnly(bool angularOnly) + { + m_angularOnly = angularOnly; + } + + + +}; + +#endif //HINGECONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h new file mode 100644 index 00000000000..bb2aecfb6d0 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -0,0 +1,156 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef JACOBIAN_ENTRY_H +#define JACOBIAN_ENTRY_H + +#include "LinearMath/btVector3.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + + +//notes: +// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components +// which makes the btJacobianEntry memory layout 16 bytes +// if you only are interested in angular part, just feed massInvA and massInvB zero + +/// Jacobian entry is an abstraction that allows to describe constraints +/// it can be used in combination with a constraint solver +/// Can be used to relate the effect of an impulse to the constraint error +class btJacobianEntry +{ +public: + btJacobianEntry() {}; + //constraint between two different rigidbodies + btJacobianEntry( + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + const btVector3& rel_pos1,const btVector3& rel_pos2, + const btVector3& jointAxis, + const btVector3& inertiaInvA, + const btScalar massInvA, + const btVector3& inertiaInvB, + const btScalar massInvB) + :m_linearJointAxis(jointAxis) + { + m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis)); + m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); + + ASSERT(m_Adiag > 0.0f); + } + + //angular constraint between two different rigidbodies + btJacobianEntry(const btVector3& jointAxis, + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + const btVector3& inertiaInvA, + const btVector3& inertiaInvB) + :m_linearJointAxis(btVector3(0.f,0.f,0.f)) + { + m_aJ= world2A*jointAxis; + m_bJ = world2B*-jointAxis; + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ASSERT(m_Adiag > 0.0f); + } + + //angular constraint between two different rigidbodies + btJacobianEntry(const btVector3& axisInA, + const btVector3& axisInB, + const btVector3& inertiaInvA, + const btVector3& inertiaInvB) + : m_linearJointAxis(btVector3(0.f,0.f,0.f)) + , m_aJ(axisInA) + , m_bJ(-axisInB) + { + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ASSERT(m_Adiag > 0.0f); + } + + //constraint on one rigidbody + btJacobianEntry( + const btMatrix3x3& world2A, + const btVector3& rel_pos1,const btVector3& rel_pos2, + const btVector3& jointAxis, + const btVector3& inertiaInvA, + const btScalar massInvA) + :m_linearJointAxis(jointAxis) + { + m_aJ= world2A*(rel_pos1.cross(jointAxis)); + m_bJ = world2A*(rel_pos2.cross(-jointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = btVector3(0.f,0.f,0.f); + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); + + ASSERT(m_Adiag > 0.0f); + } + + btScalar getDiagonal() const { return m_Adiag; } + + // for two constraints on the same rigidbody (for example vehicle friction) + btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const + { + const btJacobianEntry& jacA = *this; + btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); + btScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ); + return lin + ang; + } + + + + // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) + btScalar getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const + { + const btJacobianEntry& jacA = *this; + btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; + btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; + btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; + btVector3 lin0 = massInvA * lin ; + btVector3 lin1 = massInvB * lin; + btVector3 sum = ang0+ang1+lin0+lin1; + return sum[0]+sum[1]+sum[2]; + } + + btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB) + { + btVector3 linrel = linvelA - linvelB; + btVector3 angvela = angvelA * m_aJ; + btVector3 angvelb = angvelB * m_bJ; + linrel *= m_linearJointAxis; + angvela += angvelb; + angvela += linrel; + btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2]; + return rel_vel2 + SIMD_EPSILON; + } +//private: + + btVector3 m_linearJointAxis; + btVector3 m_aJ; + btVector3 m_bJ; + btVector3 m_0MinvJt; + btVector3 m_1MinvJt; + //Optimization: can be stored in the w/last component of one of the vectors + btScalar m_Adiag; + +}; + +#endif //JACOBIAN_ENTRY_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp new file mode 100644 index 00000000000..d15bdaad790 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -0,0 +1,115 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btPoint2PointConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + + + + +btPoint2PointConstraint::btPoint2PointConstraint() +{ +} + +btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB) +:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB) +{ + +} + + +btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) +:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)) +{ + +} + +void btPoint2PointConstraint::buildJacobian() +{ + m_appliedImpulse = 0.f; + + btVector3 normal(0,0,0); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + new (&m_jac[i]) btJacobianEntry( + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), + m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), + normal, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + normal[i] = 0; + } + +} + +void btPoint2PointConstraint::solveConstraint(btScalar timeStep) +{ + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + + + btVector3 normal(0,0,0); + + +// btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); +// btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + //this jacobian entry could be re-used for all iterations + + btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + btScalar rel_vel; + rel_vel = normal.dot(vel); + + /* + //velocity error (first order error) + btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + */ + + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + + btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; + m_appliedImpulse+=impulse; + btVector3 impulse_vector = normal * impulse; + m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); + m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); + + normal[i] = 0; + } +} + +void btPoint2PointConstraint::updateRHS(btScalar timeStep) +{ + +} + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h new file mode 100644 index 00000000000..8aae8d74ce7 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -0,0 +1,78 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef POINT2POINTCONSTRAINT_H +#define POINT2POINTCONSTRAINT_H + +#include "LinearMath/btVector3.h" + +#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "btTypedConstraint.h" + +class btRigidBody; + +struct btConstraintSetting +{ + btConstraintSetting() : + m_tau(0.3f), + m_damping(1.f) + { + } + float m_tau; + float m_damping; +}; + +/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space +class btPoint2PointConstraint : public btTypedConstraint +{ + btJacobianEntry m_jac[3]; //3 orthogonal linear constraints + + btVector3 m_pivotInA; + btVector3 m_pivotInB; + + + +public: + + btConstraintSetting m_setting; + + btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB); + + btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA); + + btPoint2PointConstraint(); + + virtual void buildJacobian(); + + + virtual void solveConstraint(btScalar timeStep); + + void updateRHS(btScalar timeStep); + + void setPivotA(const btVector3& pivotA) + { + m_pivotInA = pivotA; + } + + void setPivotB(const btVector3& pivotB) + { + m_pivotInB = pivotB; + } + + + +}; + +#endif //POINT2POINTCONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp new file mode 100644 index 00000000000..e747177a516 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -0,0 +1,361 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btSequentialImpulseConstraintSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "btContactConstraint.h" +#include "btSolve2LinearConstraint.h" +#include "btContactSolverInfo.h" +#include "LinearMath/btIDebugDraw.h" +#include "btJacobianEntry.h" +#include "LinearMath/btMinMax.h" + +#ifdef USE_PROFILE +#include "LinearMath/btQuickprof.h" +#endif //USE_PROFILE + +int totalCpd = 0; + +int gTotalContactPoints = 0; + +bool MyContactDestroyedCallback(void* userPersistentData) +{ + assert (userPersistentData); + btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData; + delete cpd; + totalCpd--; + //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData); + return true; +} + + +btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() +{ + gContactDestroyedCallback = &MyContactDestroyedCallback; + + //initialize default friction/contact funcs + int i,j; + for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++) + for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++) + { + + m_contactDispatch[i][j] = resolveSingleCollision; + m_frictionDispatch[i][j] = resolveSingleFriction; + } +} + +/// btSequentialImpulseConstraintSolver Sequentially applies impulses +float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + + btContactSolverInfo info = infoGlobal; + + int numiter = infoGlobal.m_numIterations; +#ifdef USE_PROFILE + btProfiler::beginBlock("solve"); +#endif //USE_PROFILE + + { + int j; + for (j=0;j<numManifolds;j++) + { + int k=j; + //interleaving the preparation with solving impacts the behaviour a lot, todo: find out why + + prepareConstraints(manifoldPtr[k],info,debugDrawer); + solve(manifoldPtr[k],info,0,debugDrawer); + } + } + + + //should traverse the contacts random order... + int i; + for ( i = 0;i<numiter-1;i++) + { + int j; + for (j=0;j<numManifolds;j++) + { + int k=j; + if (i&1) + k=numManifolds-j-1; + + solve(manifoldPtr[k],info,i,debugDrawer); + } + + } +#ifdef USE_PROFILE + btProfiler::endBlock("solve"); + + btProfiler::beginBlock("solveFriction"); +#endif //USE_PROFILE + + //now solve the friction + for (i = 0;i<numiter-1;i++) + { + int j; + for (j=0;j<numManifolds;j++) + { + int k = j; + if (i&1) + k=numManifolds-j-1; + solveFriction(manifoldPtr[k],info,i,debugDrawer); + } + } +#ifdef USE_PROFILE + btProfiler::endBlock("solveFriction"); +#endif //USE_PROFILE + + return 0.f; +} + + +float penetrationResolveFactor = 0.9f; +btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) +{ + btScalar rest = restitution * -rel_vel; + return rest; +} + + +void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer) +{ + + btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); + btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); + + + //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop + { + manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform()); + + int numpoints = manifoldPtr->getNumContacts(); + + gTotalContactPoints += numpoints; + + btVector3 color(0,1,0); + for (int i=0;i<numpoints ;i++) + { + btManifoldPoint& cp = manifoldPtr->getContactPoint(i); + if (cp.getDistance() <= 0.f) + { + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition(); + + + //this jacobian entry is re-used for all iterations + btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(), + body1->getCenterOfMassTransform().getBasis().transpose(), + rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(), + body1->getInvInertiaDiagLocal(),body1->getInvMass()); + + + btScalar jacDiagAB = jac.getDiagonal(); + + btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; + if (cpd) + { + //might be invalid + cpd->m_persistentLifeTime++; + if (cpd->m_persistentLifeTime != cp.getLifeTime()) + { + //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); + new (cpd) btConstraintPersistentData; + cpd->m_persistentLifeTime = cp.getLifeTime(); + + } else + { + //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); + + } + } else + { + + cpd = new btConstraintPersistentData(); + totalCpd ++; + //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); + cp.m_userPersistentData = cpd; + cpd->m_persistentLifeTime = cp.getLifeTime(); + //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); + + } + assert(cpd); + + cpd->m_jacDiagABInv = 1.f / jacDiagAB; + + //Dependent on Rigidbody A and B types, fetch the contact/friction response func + //perhaps do a similar thing for friction/restutution combiner funcs... + + cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType]; + cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType]; + + btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = cp.m_normalWorldOnB.dot(vel); + + float combinedRestitution = cp.m_combinedRestitution; + + cpd->m_penetration = cp.getDistance(); + cpd->m_friction = cp.m_combinedFriction; + cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution); + if (cpd->m_restitution <= 0.) //0.f) + { + cpd->m_restitution = 0.0f; + + }; + + //restitution and penetration work in same direction so + //rel_vel + + btScalar penVel = -cpd->m_penetration/info.m_timeStep; + + if (cpd->m_restitution >= penVel) + { + cpd->m_penetration = 0.f; + } + + + float relaxation = info.m_damping; + cpd->m_appliedImpulse *= relaxation; + //for friction + cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse; + + //re-calculate friction direction every frame, todo: check if this is really needed + btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1); + + +#define NO_FRICTION_WARMSTART 1 + + #ifdef NO_FRICTION_WARMSTART + cpd->m_accumulatedTangentImpulse0 = 0.f; + cpd->m_accumulatedTangentImpulse1 = 0.f; + #endif //NO_FRICTION_WARMSTART + float denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0); + float denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0); + float denom = relaxation/(denom0+denom1); + cpd->m_jacDiagABInvTangent0 = denom; + + + denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1); + denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1); + denom = relaxation/(denom0+denom1); + cpd->m_jacDiagABInvTangent1 = denom; + + btVector3 totalImpulse = + #ifndef NO_FRICTION_WARMSTART + cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+ + cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+ + #endif //NO_FRICTION_WARMSTART + cp.m_normalWorldOnB*cpd->m_appliedImpulse; + + //apply previous frames impulse on both bodies + body0->applyImpulse(totalImpulse, rel_pos1); + body1->applyImpulse(-totalImpulse, rel_pos2); + } + + } + } +} + +float btSequentialImpulseConstraintSolver::solve(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) +{ + + btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); + btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); + + float maxImpulse = 0.f; + + { + const int numpoints = manifoldPtr->getNumContacts(); + + btVector3 color(0,1,0); + for (int i=0;i<numpoints ;i++) + { + + int j=i; + if (iter % 2) + j = numpoints-1-i; + else + j=i; + + btManifoldPoint& cp = manifoldPtr->getContactPoint(j); + if (cp.getDistance() <= 0.f) + { + + if (iter == 0) + { + if (debugDrawer) + debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); + } + + { + + btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; + float impulse = cpd->m_contactSolverFunc( + *body0,*body1, + cp, + info); + + if (maxImpulse < impulse) + maxImpulse = impulse; + + } + } + } + } + return maxImpulse; +} + +float btSequentialImpulseConstraintSolver::solveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) +{ + btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); + btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); + + + { + const int numpoints = manifoldPtr->getNumContacts(); + + btVector3 color(0,1,0); + for (int i=0;i<numpoints ;i++) + { + + int j=i; + //if (iter % 2) + // j = numpoints-1-i; + + btManifoldPoint& cp = manifoldPtr->getContactPoint(j); + if (cp.getDistance() <= 0.f) + { + + btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; + cpd->m_frictionSolverFunc( + *body0,*body1, + cp, + info); + + + } + } + + + } + return 0.f; +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h new file mode 100644 index 00000000000..e399a5cd734 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -0,0 +1,64 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H +#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H + +#include "btConstraintSolver.h" +class btIDebugDraw; + +#include "btContactConstraint.h" + + + +/// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses +/// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com +/// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) +/// Applies impulses for combined restitution and penetration recovery and to simulate friction +class btSequentialImpulseConstraintSolver : public btConstraintSolver +{ + float solve(btPersistentManifold* manifold, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + float solveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer); + + ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; + ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; + +public: + + btSequentialImpulseConstraintSolver(); + + ///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody + ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType + void setContactSolverFunc(ContactSolverFunc func,int type0,int type1) + { + m_contactDispatch[type0][type1] = func; + } + + ///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody + ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType + void SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1) + { + m_frictionDispatch[type0][type1] = func; + } + + virtual ~btSequentialImpulseConstraintSolver() {} + + virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0); + +}; + +#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp new file mode 100644 index 00000000000..bf92434b69b --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp @@ -0,0 +1,241 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "btSolve2LinearConstraint.h" + +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btVector3.h" +#include "btJacobianEntry.h" + + +void btSolve2LinearConstraint::resolveUnilateralPairConstraint( + btRigidBody* body1, + btRigidBody* body2, + + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA,const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB,const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1,const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0,btScalar& imp1) +{ + + imp0 = 0.f; + imp1 = 0.f; + + btScalar len = fabs(normalA.length())-1.f; + if (fabs(len) >= SIMD_EPSILON) + return; + + ASSERT(len < SIMD_EPSILON); + + + //this jacobian entry could be re-used for all iterations + btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, + invInertiaBDiag,invMassB); + btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, + invInertiaBDiag,invMassB); + + //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); + //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); + + const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); + const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); + +// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv + btScalar massTerm = 1.f / (invMassA + invMassB); + + + // calculate rhs (or error) terms + const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping; + const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping; + + + // dC/dv * dv = -C + + // jacobian * impulse = -error + // + + //impulse = jacobianInverse * -error + + // inverting 2x2 symmetric system (offdiagonal are equal!) + // + + + btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); + btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); + + //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; + //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; + + imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; + imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; + + //[a b] [d -c] + //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) + + //[jA nD] * [imp0] = [dv0] + //[nD jB] [imp1] [dv1] + +} + + + +void btSolve2LinearConstraint::resolveBilateralPairConstraint( + btRigidBody* body1, + btRigidBody* body2, + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA,const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB,const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1,const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0,btScalar& imp1) +{ + + imp0 = 0.f; + imp1 = 0.f; + + btScalar len = fabs(normalA.length())-1.f; + if (fabs(len) >= SIMD_EPSILON) + return; + + ASSERT(len < SIMD_EPSILON); + + + //this jacobian entry could be re-used for all iterations + btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, + invInertiaBDiag,invMassB); + btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, + invInertiaBDiag,invMassB); + + //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); + //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); + + const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); + const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); + + // calculate rhs (or error) terms + const btScalar dv0 = depthA * m_tau - vel0 * m_damping; + const btScalar dv1 = depthB * m_tau - vel1 * m_damping; + + // dC/dv * dv = -C + + // jacobian * impulse = -error + // + + //impulse = jacobianInverse * -error + + // inverting 2x2 symmetric system (offdiagonal are equal!) + // + + + btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); + btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); + + //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; + //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; + + imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; + imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; + + //[a b] [d -c] + //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) + + //[jA nD] * [imp0] = [dv0] + //[nD jB] [imp1] [dv1] + + if ( imp0 > 0.0f) + { + if ( imp1 > 0.0f ) + { + //both positive + } + else + { + imp1 = 0.f; + + // now imp0>0 imp1<0 + imp0 = dv0 / jacA.getDiagonal(); + if ( imp0 > 0.0f ) + { + } else + { + imp0 = 0.f; + } + } + } + else + { + imp0 = 0.f; + + imp1 = dv1 / jacB.getDiagonal(); + if ( imp1 <= 0.0f ) + { + imp1 = 0.f; + // now imp0>0 imp1<0 + imp0 = dv0 / jacA.getDiagonal(); + if ( imp0 > 0.0f ) + { + } else + { + imp0 = 0.f; + } + } else + { + } + } +} + + + +void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, + const btScalar invMassA, + const btVector3& linvelA,const btVector3& angvelA, + const btVector3& rel_posA1, + const btMatrix3x3& invInertiaBWS, + const btScalar invMassB, + const btVector3& linvelB,const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1,const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0,btScalar& imp1) +{ + +} + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h new file mode 100644 index 00000000000..639e4c9433f --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h @@ -0,0 +1,106 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SOLVE_2LINEAR_CONSTRAINT_H +#define SOLVE_2LINEAR_CONSTRAINT_H + +#include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btVector3.h" + + +class btRigidBody; + + + +/// constraint class used for lateral tyre friction. +class btSolve2LinearConstraint +{ + btScalar m_tau; + btScalar m_damping; + +public: + + btSolve2LinearConstraint(btScalar tau,btScalar damping) + { + m_tau = tau; + m_damping = damping; + } + // + // solve unilateral constraint (equality, direct method) + // + void resolveUnilateralPairConstraint( + btRigidBody* body0, + btRigidBody* body1, + + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA,const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB,const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1,const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0,btScalar& imp1); + + + // + // solving 2x2 lcp problem (inequality, direct solution ) + // + void resolveBilateralPairConstraint( + btRigidBody* body0, + btRigidBody* body1, + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA,const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB,const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1,const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0,btScalar& imp1); + + + void resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, + const btScalar invMassA, + const btVector3& linvelA,const btVector3& angvelA, + const btVector3& rel_posA1, + const btMatrix3x3& invInertiaBWS, + const btScalar invMassB, + const btVector3& linvelB,const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1,const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0,btScalar& imp1); + + +}; + +#endif //SOLVE_2LINEAR_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp new file mode 100644 index 00000000000..ea7d5c8b903 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -0,0 +1,53 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btTypedConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + +static btRigidBody s_fixed(0, btTransform::getIdentity(),0); + +btTypedConstraint::btTypedConstraint() +: m_userConstraintType(-1), +m_userConstraintId(-1), +m_rbA(s_fixed), +m_rbB(s_fixed), +m_appliedImpulse(0.f) +{ + s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); +} +btTypedConstraint::btTypedConstraint(btRigidBody& rbA) +: m_userConstraintType(-1), +m_userConstraintId(-1), +m_rbA(rbA), +m_rbB(s_fixed), +m_appliedImpulse(0.f) +{ + s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); + +} + + +btTypedConstraint::btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB) +: m_userConstraintType(-1), +m_userConstraintId(-1), +m_rbA(rbA), +m_rbB(rbB), +m_appliedImpulse(0.f) +{ + s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); + +} + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h new file mode 100644 index 00000000000..bc25eaa759c --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -0,0 +1,90 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef TYPED_CONSTRAINT_H +#define TYPED_CONSTRAINT_H + +class btRigidBody; +#include "LinearMath/btScalar.h" + +///TypedConstraint is the baseclass for Bullet constraints and vehicles +class btTypedConstraint +{ + int m_userConstraintType; + int m_userConstraintId; + + +protected: + btRigidBody& m_rbA; + btRigidBody& m_rbB; + float m_appliedImpulse; + + +public: + + btTypedConstraint(); + virtual ~btTypedConstraint() {}; + btTypedConstraint(btRigidBody& rbA); + + btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB); + + virtual void buildJacobian() = 0; + + virtual void solveConstraint(btScalar timeStep) = 0; + + const btRigidBody& getRigidBodyA() const + { + return m_rbA; + } + const btRigidBody& getRigidBodyB() const + { + return m_rbB; + } + + btRigidBody& getRigidBodyA() + { + return m_rbA; + } + btRigidBody& getRigidBodyB() + { + return m_rbB; + } + + int getUserConstraintType() const + { + return m_userConstraintType ; + } + + void setUserConstraintType(int userConstraintType) + { + m_userConstraintType = userConstraintType; + }; + + void setUserConstraintId(int uid) + { + m_userConstraintId = uid; + } + + int getUserConstraintId() + { + return m_userConstraintId; + } + float getAppliedImpulse() + { + return m_appliedImpulse; + } +}; + +#endif //TYPED_CONSTRAINT_H |