From c44471c38cf193fca680d297f31a7d3fe351fd85 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 26 Feb 2019 20:24:15 -0800 Subject: [PATCH 1/6] preparation for block solver btRigidBody. --- examples/BlockSolver/BlockSolverExample.cpp | 380 ++++++++ examples/BlockSolver/BlockSolverExample.h | 19 + examples/BlockSolver/btBlockSolver.cpp | 161 ++++ examples/BlockSolver/btBlockSolver.h | 29 + examples/Evolution/NN3DWalkersTimeWarpBase.h | 17 +- examples/ExampleBrowser/CMakeLists.txt | 4 + examples/ExampleBrowser/ExampleEntries.cpp | 8 + examples/ExampleBrowser/premake4.lua | 1 + .../ConstraintSolver/btConstraintSolver.h | 1 + .../btSequentialImpulseConstraintSolver.cpp | 815 ++++++++++++++---- .../btSequentialImpulseConstraintSolver.h | 124 ++- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 4 +- .../btMultiBodyMLCPConstraintSolver.h | 2 +- 13 files changed, 1377 insertions(+), 188 deletions(-) create mode 100644 examples/BlockSolver/BlockSolverExample.cpp create mode 100644 examples/BlockSolver/BlockSolverExample.h create mode 100644 examples/BlockSolver/btBlockSolver.cpp create mode 100644 examples/BlockSolver/btBlockSolver.h diff --git a/examples/BlockSolver/BlockSolverExample.cpp b/examples/BlockSolver/BlockSolverExample.cpp new file mode 100644 index 000000000..e0bff80bb --- /dev/null +++ b/examples/BlockSolver/BlockSolverExample.cpp @@ -0,0 +1,380 @@ +#include "BlockSolverExample.h" +#include "../OpenGLWindow/SimpleOpenGL3App.h" +#include "btBulletDynamicsCommon.h" + +#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" +#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" + +#include "BulletDynamics/Featherstone/btMultiBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyLink.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" +#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" +#include "btBlockSolver.h" + +#include "../OpenGLWindow/GLInstancingRenderer.h" +#include "BulletCollision/CollisionShapes/btShapeHull.h" + +#include "../CommonInterfaces/CommonMultiBodyBase.h" + +class BlockSolverExample : public CommonMultiBodyBase +{ + int m_option; +public: + BlockSolverExample(GUIHelperInterface* helper, int option); + virtual ~BlockSolverExample(); + + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + + virtual void resetCamera() + { + float dist = 1; + float pitch = -35; + float yaw = 50; + float targetPos[3] = {-3, 2.8, -2.5}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool fixedBase = false); + void createGround(const btVector3& halfExtents = btVector3(50, 50, 50), btScalar zOffSet = btScalar(-1.55)); + void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents); +}; + +static bool g_fixedBase = true; +static bool g_firstInit = true; +static float scaling = 0.4f; +static float friction = 1.; + + +BlockSolverExample::BlockSolverExample(GUIHelperInterface* helper, int option) + : CommonMultiBodyBase(helper), + m_option(option) +{ + m_guiHelper->setUpAxis(1); +} + +BlockSolverExample::~BlockSolverExample() +{ + // Do nothing +} + +void BlockSolverExample::stepSimulation(float deltaTime) +{ + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep); +} + +void BlockSolverExample::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + if (g_firstInit) + { + m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10. * scaling)); + m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50); + g_firstInit = false; + } + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + + + btMLCPSolverInterface* mlcp; + if (m_option&BLOCK_SOLVER_SI) + { + btAssert(!m_solver); + m_solver = new btMultiBodyConstraintSolver; + b3Printf("Constraint Solver: Sequential Impulse"); + } + if (m_option&BLOCK_SOLVER_MLCP_PGS) + { + btAssert(!m_solver); + mlcp = new btSolveProjectedGaussSeidel(); + m_solver = new btMultiBodyMLCPConstraintSolver(mlcp); + b3Printf("Constraint Solver: MLCP + PGS"); + } + if (m_option&BLOCK_SOLVER_MLCP_DANTZIG) + { + btAssert(!m_solver); + mlcp = new btDantzigSolver(); + m_solver = new btMultiBodyMLCPConstraintSolver(mlcp); + b3Printf("Constraint Solver: MLCP + Dantzig"); + } + if (m_option&BLOCK_SOLVER_BLOCK) + { + //m_solver = new btBlockSolver(); + } + + btAssert(m_solver); + + btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); + m_dynamicsWorld = world; + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); + m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good? + + + + ///////////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////// + + bool damping = true; + bool gyro = true; + int numLinks = 5; + bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + bool multibodyOnly = true; //false + bool canSleep = true; + bool selfCollide = true; + btVector3 linkHalfExtents(0.05, 0.37, 0.1); + btVector3 baseHalfExtents(0.05, 0.37, 0.1); + + btMultiBody* mbC1 = createFeatherstoneMultiBody(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase); + btMultiBody* mbC2 = createFeatherstoneMultiBody(world, numLinks, btVector3(-0.4f, 3.0f, 0.5f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase); + + mbC1->setCanSleep(canSleep); + mbC1->setHasSelfCollision(selfCollide); + mbC1->setUseGyroTerm(gyro); + + if (!damping) + { + mbC1->setLinearDamping(0.f); + mbC1->setAngularDamping(0.f); + } + else + { + mbC1->setLinearDamping(0.1f); + mbC1->setAngularDamping(0.9f); + } + // + m_dynamicsWorld->setGravity(btVector3(0, -9.81, 0)); + ////////////////////////////////////////////// + if (numLinks > 0) + { + btScalar q0 = 45.f * SIMD_PI / 180.f; + if (!spherical) + { + mbC1->setJointPosMultiDof(0, &q0); + } + else + { + btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); + quat0.normalize(); + mbC1->setJointPosMultiDof(0, quat0); + } + } + /// + addColliders(mbC1, world, baseHalfExtents, linkHalfExtents); + + mbC2->setCanSleep(canSleep); + mbC2->setHasSelfCollision(selfCollide); + mbC2->setUseGyroTerm(gyro); + // + if (!damping) + { + mbC2->setLinearDamping(0.f); + mbC2->setAngularDamping(0.f); + } + else + { + mbC2->setLinearDamping(0.1f); + mbC2->setAngularDamping(0.9f); + } + // + m_dynamicsWorld->setGravity(btVector3(0, -9.81, 0)); + ////////////////////////////////////////////// + if (numLinks > 0) + { + btScalar q0 = -45.f * SIMD_PI / 180.f; + if (!spherical) + { + mbC2->setJointPosMultiDof(0, &q0); + } + else + { + btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); + quat0.normalize(); + mbC2->setJointPosMultiDof(0, quat0); + } + } + /// + addColliders(mbC2, world, baseHalfExtents, linkHalfExtents); + + ///////////////////////////////////////////////////////////////// + btScalar groundHeight = -51.55; + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + + + createGround(); + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + + ///////////////////////////////////////////////////////////////// +} + +btMultiBody* BlockSolverExample::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool fixedBase) +{ + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = 1.f; + + if (baseMass) + { + btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); + delete pTempBox; + } + + bool canSleep = false; + + btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, fixedBase, canSleep); + + btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); + pMultiBody->setBasePos(basePosition); + pMultiBody->setWorldToBaseRot(baseOriQuat); + btVector3 vel(0, 0, 0); + + //init the links + btVector3 hingeJointAxis(1, 0, 0); + float linkMass = 1.f; + btVector3 linkInertiaDiag(0.f, 0.f, 0.f); + + btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); + pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); + delete pTempBox; + + //y-axis assumed up + btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + ////// + btScalar q0 = 0.f * SIMD_PI / 180.f; + btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); + quat0.normalize(); + ///// + + for (int i = 0; i < numLinks; ++i) + { + if (!spherical) + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true); + else + //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true); + } + + pMultiBody->finalizeMultiDof(); + + /// + pWorld->addMultiBody(pMultiBody); + /// + return pMultiBody; +} + +void BlockSolverExample::createGround(const btVector3& halfExtents, btScalar zOffSet) +{ + btCollisionShape* groundShape = new btBoxShape(halfExtents); + m_collisionShapes.push_back(groundShape); + + // rigidbody is dynamic if and only if mass is non zero, otherwise static + btScalar mass(0.); + const bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + // using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -halfExtents.z() + zOffSet, 0)); + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + // add the body to the dynamics world + m_dynamicsWorld->addRigidBody(body, 1, 1 + 2); +} + +void BlockSolverExample::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) +{ + btAlignedObjectArray world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray local_origin; + local_origin.resize(pMultiBody->getNumLinks() + 1); + world_to_local[0] = pMultiBody->getWorldToBaseRot(); + local_origin[0] = pMultiBody->getBasePos(); + + { + btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; + + if (1) + { + btCollisionShape* box = new btBoxShape(baseHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + + pWorld->addCollisionObject(col, 2, 1 + 2); + + col->setFriction(friction); + pMultiBody->setBaseCollider(col); + } + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + const int parent = pMultiBody->getParent(i); + world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1]; + local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i))); + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + btVector3 posr = local_origin[i + 1]; + + btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()}; + + btCollisionShape* box = new btBoxShape(linkHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); + + col->setCollisionShape(box); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + col->setFriction(friction); + pWorld->addCollisionObject(col, 2, 1 + 2); + + pMultiBody->getLink(i).m_collider = col; + } +} + +CommonExampleInterface* BlockSolverExampleCreateFunc(CommonExampleOptions& options) +{ + return new BlockSolverExample(options.m_guiHelper, options.m_option); +} diff --git a/examples/BlockSolver/BlockSolverExample.h b/examples/BlockSolver/BlockSolverExample.h new file mode 100644 index 000000000..0b49e8f71 --- /dev/null +++ b/examples/BlockSolver/BlockSolverExample.h @@ -0,0 +1,19 @@ + +#ifndef BLOCK_SOLVER_EXAMPLE_H +#define BLOCK_SOLVER_EXAMPLE_H + +enum BlockSolverOptions +{ + BLOCK_SOLVER_SI=1<<0, + BLOCK_SOLVER_MLCP_PGS = 1 << 1, + BLOCK_SOLVER_MLCP_DANTZIG = 1 << 2, + BLOCK_SOLVER_BLOCK = 1 << 3, + + BLOCK_SOLVER_SCENE_STACK= 1 << 5, + BLOCK_SOLVER_SCENE_CHAIN = 1<< 6, + +}; + +class CommonExampleInterface* BlockSolverExampleCreateFunc(struct CommonExampleOptions& options); + +#endif //BLOCK_SOLVER_EXAMPLE_H diff --git a/examples/BlockSolver/btBlockSolver.cpp b/examples/BlockSolver/btBlockSolver.cpp new file mode 100644 index 000000000..14f5f45a8 --- /dev/null +++ b/examples/BlockSolver/btBlockSolver.cpp @@ -0,0 +1,161 @@ +#include "btBlockSolver.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "LinearMath/btQuickprof.h" + +struct btBlockSolverInternalData +{ + btAlignedObjectArray m_tmpSolverBodyPool; + btConstraintArray m_tmpSolverContactConstraintPool; + btConstraintArray m_tmpSolverNonContactConstraintPool; + btConstraintArray m_tmpSolverContactFrictionConstraintPool; + btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; + + btAlignedObjectArray m_orderTmpConstraintPool; + btAlignedObjectArray m_orderNonContactConstraintPool; + btAlignedObjectArray m_orderFrictionConstraintPool; + btAlignedObjectArray m_tmpConstraintSizesPool; + + + unsigned long m_btSeed2; + int m_fixedBodyId; + int m_maxOverrideNumSolverIterations; + btAlignedObjectArray m_kinematicBodyUniqueIdToSolverBodyTable; // only used for multithreading + + btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric; + btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit; + btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse; + + btBlockSolverInternalData() + :m_btSeed2(0), + m_fixedBodyId(-1), + m_maxOverrideNumSolverIterations(0), + m_resolveSingleConstraintRowGeneric(btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()), + m_resolveSingleConstraintRowLowerLimit(btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()), + m_resolveSplitPenetrationImpulse(btSequentialImpulseConstraintSolver::getScalarSplitPenetrationImpulseGeneric()) + { + } +}; + + + + +btBlockSolver::btBlockSolver() +{ + m_data = new btBlockSolverInternalData; +} + +btBlockSolver::~btBlockSolver() +{ + delete m_data; +} + + +btScalar btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) +{ + + btSISolverSingleIterationData siData(m_data->m_tmpSolverBodyPool, + m_data->m_tmpSolverContactConstraintPool, + m_data->m_tmpSolverNonContactConstraintPool, + m_data->m_tmpSolverContactFrictionConstraintPool, + m_data->m_tmpSolverContactRollingFrictionConstraintPool, + m_data->m_orderTmpConstraintPool, + m_data->m_orderNonContactConstraintPool, + m_data->m_orderFrictionConstraintPool, + m_data->m_tmpConstraintSizesPool, + m_data->m_resolveSingleConstraintRowGeneric, + m_data->m_resolveSingleConstraintRowLowerLimit, + m_data->m_resolveSplitPenetrationImpulse, + m_data->m_kinematicBodyUniqueIdToSolverBodyTable, + m_data->m_btSeed2, + m_data->m_fixedBodyId, + m_data->m_maxOverrideNumSolverIterations); + + m_data->m_fixedBodyId = -1; + //todo: setup sse2/4 constraint row methods + + btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies, numBodies, info); + btSequentialImpulseConstraintSolver::convertJointsInternal(siData, constraints, numConstraints, info); + + int i; + btPersistentManifold* manifold = 0; + // btCollisionObject* colObj0=0,*colObj1=0; + + for (i = 0; i < numManifolds; i++) + { + manifold = manifoldPtr[i]; + btSequentialImpulseConstraintSolver::convertContactInternal(siData, manifold, info); + } + + + + int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size(); + int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size(); + int numFrictionPool = siData.m_tmpSolverContactFrictionConstraintPool.size(); + + ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints + siData.m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool); + if ((info.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2); + else + siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); + + siData.m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool); + { + int i; + for (i = 0; i < numNonContactPool; i++) + { + siData.m_orderNonContactConstraintPool[i] = i; + } + for (i = 0; i < numConstraintPool; i++) + { + siData.m_orderTmpConstraintPool[i] = i; + } + for (i = 0; i < numFrictionPool; i++) + { + siData.m_orderFrictionConstraintPool[i] = i; + } + } + + btScalar leastSquaresResidual = 0; + + + + { + BT_PROFILE("solveGroupCacheFriendlyIterations"); + ///this is a special step to resolve penetrations (just for contacts) + btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterationsInternal(siData, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, info, debugDrawer); + + int maxIterations = siData.m_maxOverrideNumSolverIterations > info.m_numIterations ? siData.m_maxOverrideNumSolverIterations : info.m_numIterations; + + for (int iteration = 0; iteration < maxIterations; iteration++) + //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) + { + leastSquaresResidual = btSequentialImpulseConstraintSolver::solveSingleIterationInternal(siData, iteration, constraints, numConstraints, info); + + if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) + { +#ifdef VERBOSE_RESIDUAL_PRINTF + printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); +#endif + break; + } + } + } + + btScalar res = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, info); + return res; +} + + + +void btBlockSolver::solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) +{ + //btMultiBodyConstraintSolver::solveMultiBodyGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, multiBodyConstraints, numMultiBodyConstraints, info, debugDrawer, dispatcher); +} + +void btBlockSolver::reset() +{ + //or just set m_data->m_btSeed2=0? + delete m_data; + m_data = new btBlockSolverInternalData; +} \ No newline at end of file diff --git a/examples/BlockSolver/btBlockSolver.h b/examples/BlockSolver/btBlockSolver.h new file mode 100644 index 000000000..8ea3e9742 --- /dev/null +++ b/examples/BlockSolver/btBlockSolver.h @@ -0,0 +1,29 @@ +#ifndef BT_BLOCK_SOLVER_H +#define BT_BLOCK_SOLVER_H + +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" + +class btBlockSolver : public btConstraintSolver +{ + struct btBlockSolverInternalData* m_data; + +public: + btBlockSolver(); + virtual ~btBlockSolver(); + + //btRigidBody + virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, class btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + + //btMultibody + virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + + ///clear internal cached data and reset random seed + virtual void reset(); + + virtual btConstraintSolverType getSolverType() const + { + return BT_BLOCK_SOLVER; + } +}; + +#endif //BT_BLOCK_SOLVER_H \ No newline at end of file diff --git a/examples/Evolution/NN3DWalkersTimeWarpBase.h b/examples/Evolution/NN3DWalkersTimeWarpBase.h index 0a237ee98..deef11476 100644 --- a/examples/Evolution/NN3DWalkersTimeWarpBase.h +++ b/examples/Evolution/NN3DWalkersTimeWarpBase.h @@ -91,7 +91,7 @@ enum SolverEnumType NNCGSOLVER = 2, DANZIGSOLVER = 3, LEMKESOLVER = 4, - FSSOLVER = 5, + NUM_SOLVERS = 6 }; @@ -103,7 +103,7 @@ static char GAUSSSEIDELSOLVER[] = "Gauss-Seidel Solver"; static char NNCGSOLVER[] = "NNCG Solver"; static char DANZIGSOLVER[] = "Danzig Solver"; static char LEMKESOLVER[] = "Lemke Solver"; -static char FSSOLVER[] = "FeatherStone Solver"; + }; // namespace SolverType static const char* solverTypes[NUM_SOLVERS]; @@ -324,7 +324,7 @@ struct NN3DWalkersTimeWarpBase : public CommonRigidBodyBase solverTypes[2] = SolverType::NNCGSOLVER; solverTypes[3] = SolverType::DANZIGSOLVER; solverTypes[4] = SolverType::LEMKESOLVER; - solverTypes[5] = SolverType::FSSOLVER; + { ComboBoxParams comboParams; @@ -499,19 +499,12 @@ struct NN3DWalkersTimeWarpBase : public CommonRigidBodyBase m_solver = new btMLCPSolver(mlcp); break; } - case FSSOLVER: - { - // b3Printf("=%s=",SolverType::FSSOLVER); - //Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support - m_solver = new btMultiBodyConstraintSolver; - - break; - } + default: break; } - if (SOLVER_TYPE != FSSOLVER) + if (1) { //TODO: Set parameters for other solvers diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index ef0789e42..ba6a3fbd7 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -202,6 +202,10 @@ SET(BulletExampleBrowser_SRCS ../MultiThreadedDemo/MultiThreadedDemo.h ../MultiThreadedDemo/CommonRigidBodyMTBase.cpp ../MultiThreadedDemo/CommonRigidBodyMTBase.h + ../BlockSolver/btBlockSolver.cpp + ../BlockSolver/btBlockSolver.h + ../BlockSolver/BlockSolverExample.cpp + ../BlockSolver/BlockSolverExample.h ../Tutorial/Tutorial.cpp ../Tutorial/Tutorial.h ../Tutorial/Dof6ConstraintTutorial.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 8f1f7d37a..cf8ad2f6b 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -1,5 +1,6 @@ #include "ExampleEntries.h" +#include "../BlockSolver/BlockSolverExample.h" #include "LinearMath/btAlignedObjectArray.h" #include "EmptyExample.h" #include "../RenderingExamples/RenderInstancingDemo.h" @@ -150,6 +151,13 @@ static ExampleEntry gDefaultExamples[] = // // ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT), + + ExampleEntry(0, "BlockSolver"), + ExampleEntry(1, "Stack MultiBody SI", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_STACK+ BLOCK_SOLVER_SI), + ExampleEntry(1, "Stack MultiBody MLCP PGS", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_STACK + BLOCK_SOLVER_MLCP_PGS), + ExampleEntry(1, "Stack MultiBody MLCP Dantzig", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_STACK + BLOCK_SOLVER_MLCP_DANTZIG), + ExampleEntry(1, "Stack MultiBody Block", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_STACK + BLOCK_SOLVER_BLOCK), + ExampleEntry(0, "Inverse Dynamics"), ExampleEntry(1, "Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc, BT_ID_LOAD_URDF), ExampleEntry(1, "Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc, BT_ID_PROGRAMMATICALLY), diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 94cd3f7ee..222ad911e 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -162,6 +162,7 @@ project "App_BulletExampleBrowser" "../Evolution/NN3DWalkers.h", "../Collision/*", "../RoboticsLearning/*", + "../BlockSolver/*", "../Collision/Internal/*", "../Benchmarks/*", "../MultiThreadedDemo/*", diff --git a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index 808433477..68a4a07a1 100644 --- a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -35,6 +35,7 @@ enum btConstraintSolverType BT_MLCP_SOLVER = 2, BT_NNCG_SOLVER = 4, BT_MULTIBODY_SOLVER = 8, + BT_BLOCK_SOLVER = 16, }; class btConstraintSolver diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index def3227b4..e7a237770 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -394,6 +394,18 @@ btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstr return gResolveSingleConstraintRowLowerLimit_scalar_reference; } +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarSplitPenetrationImpulseGeneric() +{ + return gResolveSplitPenetrationImpulse_scalar_reference; +} + +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2SplitPenetrationImpulseGeneric() +{ + return gResolveSplitPenetrationImpulse_sse2; +} + + + #ifdef USE_SIMD btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric() { @@ -421,6 +433,11 @@ unsigned long btSequentialImpulseConstraintSolver::btRand2() return m_btSeed2; } +unsigned long btSequentialImpulseConstraintSolver::btRand2a(unsigned long& seed) +{ + seed = (1664525L * seed + 1013904223L) & 0xffffffff; + return seed; +} //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) int btSequentialImpulseConstraintSolver::btRandInt2(int n) { @@ -454,42 +471,44 @@ int btSequentialImpulseConstraintSolver::btRandInt2(int n) return (int)(r % un); } -void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep) +int btSequentialImpulseConstraintSolver::btRandInt2a(int n, unsigned long& seed) { - btRigidBody* rb = collisionObject ? btRigidBody::upcast(collisionObject) : 0; + // seems good; xor-fold and modulus + const unsigned long un = static_cast(n); + unsigned long r = btSequentialImpulseConstraintSolver::btRand2a(seed); - solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); - solverBody->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f); - solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); - solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); + // note: probably more aggressive than it needs to be -- might be + // able to get away without one or two of the innermost branches. + if (un <= 0x00010000UL) + { + r ^= (r >> 16); + if (un <= 0x00000100UL) + { + r ^= (r >> 8); + if (un <= 0x00000010UL) + { + r ^= (r >> 4); + if (un <= 0x00000004UL) + { + r ^= (r >> 2); + if (un <= 0x00000002UL) + { + r ^= (r >> 1); + } + } + } + } + } - if (rb) - { - solverBody->m_worldTransform = rb->getWorldTransform(); - solverBody->internalSetInvMass(btVector3(rb->getInvMass(), rb->getInvMass(), rb->getInvMass()) * rb->getLinearFactor()); - solverBody->m_originalBody = rb; - solverBody->m_angularFactor = rb->getAngularFactor(); - solverBody->m_linearFactor = rb->getLinearFactor(); - solverBody->m_linearVelocity = rb->getLinearVelocity(); - solverBody->m_angularVelocity = rb->getAngularVelocity(); - solverBody->m_externalForceImpulse = rb->getTotalForce() * rb->getInvMass() * timeStep; - solverBody->m_externalTorqueImpulse = rb->getTotalTorque() * rb->getInvInertiaTensorWorld() * timeStep; - } - else - { - solverBody->m_worldTransform.setIdentity(); - solverBody->internalSetInvMass(btVector3(0, 0, 0)); - solverBody->m_originalBody = 0; - solverBody->m_angularFactor.setValue(1, 1, 1); - solverBody->m_linearFactor.setValue(1, 1, 1); - solverBody->m_linearVelocity.setValue(0, 0, 0); - solverBody->m_angularVelocity.setValue(0, 0, 0); - solverBody->m_externalForceImpulse.setValue(0, 0, 0); - solverBody->m_externalTorqueImpulse.setValue(0, 0, 0); - } + return (int)(r % un); } -btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold) +void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep) +{ + btSISolverSingleIterationData::initSolverBody(solverBody, collisionObject, timeStep); +} + +btScalar btSequentialImpulseConstraintSolver::restitutionCurveInternal(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold) { //printf("rel_vel =%f\n", rel_vel); if (btFabs(rel_vel) < velocityThreshold) @@ -498,6 +517,10 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar rest = restitution * -rel_vel; return rest; } +btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold) +{ + return btSequentialImpulseConstraintSolver::restitutionCurveInternal(rel_vel, restitution, velocityThreshold); +} void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj, btVector3& frictionDirection, int frictionMode) { @@ -513,13 +536,13 @@ void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb } } -void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +void btSequentialImpulseConstraintSolver::setupFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { - btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; + btSolverBody& solverBodyA = tmpSolverBodyPool[solverBodyIdA]; + btSolverBody& solverBodyB = tmpSolverBodyPool[solverBodyIdB]; - btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; - btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; + btRigidBody* body0 = tmpSolverBodyPool[solverBodyIdA].m_originalBody; + btRigidBody* bodyA = tmpSolverBodyPool[solverBodyIdB].m_originalBody; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -605,6 +628,22 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr } } +void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +{ + btSequentialImpulseConstraintSolver::setupFrictionConstraintInternal(m_tmpSolverBodyPool, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); +} + +btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +{ + btSolverConstraint& solverConstraint = tmpSolverContactFrictionConstraintPool.expandNonInitializing(); + solverConstraint.m_frictionIndex = frictionIndex; + setupFrictionConstraintInternal(tmpSolverBodyPool, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); + return solverConstraint; +} + + + btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); @@ -614,7 +653,8 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c return solverConstraint; } -void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, + +void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) @@ -624,11 +664,11 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSol solverConstraint.m_contactNormal1 = normalAxis; solverConstraint.m_contactNormal2 = -normalAxis; - btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; + btSolverBody& solverBodyA = tmpSolverBodyPool[solverBodyIdA]; + btSolverBody& solverBodyB = tmpSolverBodyPool[solverBodyIdB]; - btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; - btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; + btRigidBody* body0 = tmpSolverBodyPool[solverBodyIdA].m_originalBody; + btRigidBody* bodyA = tmpSolverBodyPool[solverBodyIdB].m_originalBody; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -677,6 +717,30 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSol } } + +void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, + btScalar desiredVelocity, btScalar cfmSlip) + +{ + setupTorsionalFrictionConstraintInternal(m_tmpSolverBodyPool, solverConstraint, normalAxis1,solverBodyIdA, solverBodyIdB, + cp, combinedTorsionalFriction, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, + desiredVelocity, cfmSlip); + +} + +btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) +{ + btSolverConstraint& solverConstraint = tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); + solverConstraint.m_frictionIndex = frictionIndex; + setupTorsionalFrictionConstraintInternal(tmpSolverBodyPool, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); + return solverConstraint; +} + + btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) { btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); @@ -686,6 +750,195 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionCon return solverConstraint; } +int btSISolverSingleIterationData::getOrInitSolverBody(btCollisionObject & body, btScalar timeStep) +{ +#if BT_THREADSAFE + int solverBodyId = -1; + bool isRigidBodyType = btRigidBody::upcast(&body) != NULL; + if (isRigidBodyType && !body.isStaticOrKinematicObject()) + { + // dynamic body + // Dynamic bodies can only be in one island, so it's safe to write to the companionId + solverBodyId = body.getCompanionId(); + if (solverBodyId < 0) + { + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody, &body, timeStep); + body.setCompanionId(solverBodyId); + } + } + else if (isRigidBodyType && body.isKinematicObject()) + { + // + // NOTE: must test for kinematic before static because some kinematic objects also + // identify as "static" + // + // Kinematic bodies can be in multiple islands at once, so it is a + // race condition to write to them, so we use an alternate method + // to record the solverBodyId + int uniqueId = body.getWorldArrayIndex(); + const int INVALID_SOLVER_BODY_ID = -1; + if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size()) + { + m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID); + } + solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId]; + // if no table entry yet, + if (solverBodyId == INVALID_SOLVER_BODY_ID) + { + // create a table entry for this body + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody, &body, timeStep); + m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId; + } + } + else + { + bool isMultiBodyType = (body.getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK); + // Incorrectly set collision object flags can degrade performance in various ways. + if (!isMultiBodyType) + { + btAssert(body.isStaticOrKinematicObject()); + } + //it could be a multibody link collider + // all fixed bodies (inf mass) get mapped to a single solver id + if (m_fixedBodyId < 0) + { + m_fixedBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&fixedBody, 0, timeStep); + } + solverBodyId = m_fixedBodyId; + } + btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size()); + return solverBodyId; +#else // BT_THREADSAFE + + int solverBodyIdA = -1; + + if (body.getCompanionId() >= 0) + { + //body has already been converted + solverBodyIdA = body.getCompanionId(); + btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); + } + else + { + btRigidBody* rb = btRigidBody::upcast(&body); + //convert both active and kinematic objects (for their velocity) + if (rb && (rb->getInvMass() || rb->isKinematicObject())) + { + solverBodyIdA = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody, &body, timeStep); + body.setCompanionId(solverBodyIdA); + } + else + { + if (m_fixedBodyId < 0) + { + m_fixedBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&fixedBody, 0, timeStep); + } + return m_fixedBodyId; + // return 0;//assume first one is a fixed solver body + } + } + + return solverBodyIdA; +#endif // BT_THREADSAFE +} +void btSISolverSingleIterationData::initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep) +{ + btRigidBody* rb = collisionObject ? btRigidBody::upcast(collisionObject) : 0; + + solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); + solverBody->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f); + solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); + solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); + + if (rb) + { + solverBody->m_worldTransform = rb->getWorldTransform(); + solverBody->internalSetInvMass(btVector3(rb->getInvMass(), rb->getInvMass(), rb->getInvMass()) * rb->getLinearFactor()); + solverBody->m_originalBody = rb; + solverBody->m_angularFactor = rb->getAngularFactor(); + solverBody->m_linearFactor = rb->getLinearFactor(); + solverBody->m_linearVelocity = rb->getLinearVelocity(); + solverBody->m_angularVelocity = rb->getAngularVelocity(); + solverBody->m_externalForceImpulse = rb->getTotalForce() * rb->getInvMass() * timeStep; + solverBody->m_externalTorqueImpulse = rb->getTotalTorque() * rb->getInvInertiaTensorWorld() * timeStep; + } + else + { + solverBody->m_worldTransform.setIdentity(); + solverBody->internalSetInvMass(btVector3(0, 0, 0)); + solverBody->m_originalBody = 0; + solverBody->m_angularFactor.setValue(1, 1, 1); + solverBody->m_linearFactor.setValue(1, 1, 1); + solverBody->m_linearVelocity.setValue(0, 0, 0); + solverBody->m_angularVelocity.setValue(0, 0, 0); + solverBody->m_externalForceImpulse.setValue(0, 0, 0); + solverBody->m_externalTorqueImpulse.setValue(0, 0, 0); + } +} + +int btSISolverSingleIterationData::getSolverBody(btCollisionObject& body) const +{ +#if BT_THREADSAFE + int solverBodyId = -1; + bool isRigidBodyType = btRigidBody::upcast(&body) != NULL; + if (isRigidBodyType && !body.isStaticOrKinematicObject()) + { + // dynamic body + // Dynamic bodies can only be in one island, so it's safe to write to the companionId + solverBodyId = body.getCompanionId(); + btAssert(solverBodyId >= 0); + } + else if (isRigidBodyType && body.isKinematicObject()) + { + // + // NOTE: must test for kinematic before static because some kinematic objects also + // identify as "static" + // + // Kinematic bodies can be in multiple islands at once, so it is a + // race condition to write to them, so we use an alternate method + // to record the solverBodyId + int uniqueId = body.getWorldArrayIndex(); + const int INVALID_SOLVER_BODY_ID = -1; + if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size()) + { + m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID); + } + solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId]; + btAssert(solverBodyId != INVALID_SOLVER_BODY_ID); + } + else + { + bool isMultiBodyType = (body.getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK); + // Incorrectly set collision object flags can degrade performance in various ways. + if (!isMultiBodyType) + { + btAssert(body.isStaticOrKinematicObject()); + } + btAssert(m_fixedBodyId >= 0); + solverBodyId = m_fixedBodyId; + } + btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size()); + return solverBodyId; +#else // BT_THREADSAFE + int solverBodyIdA = -1; + btAssert(body.getCompanionId() >= 0); + //body has already been converted + solverBodyIdA = body.getCompanionId(); + btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); + return solverBodyIdA; +#endif // BT_THREADSAFE +} + int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body, btScalar timeStep) { #if BT_THREADSAFE @@ -789,7 +1042,10 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& } #include -void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, + + +void btSequentialImpulseConstraintSolver::setupContactConstraintInternal(btSISolverSingleIterationData& siData, + btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, @@ -798,8 +1054,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra // const btVector3& pos1 = cp.getPositionWorldOnA(); // const btVector3& pos2 = cp.getPositionWorldOnB(); - btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + btSolverBody* bodyA = &siData.m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyB = &siData.m_tmpSolverBodyPool[solverBodyIdB]; btRigidBody* rb0 = bodyA->m_originalBody; btRigidBody* rb1 = bodyB->m_originalBody; @@ -906,7 +1162,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra solverConstraint.m_friction = cp.m_combinedFriction; - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); + restitution = btSequentialImpulseConstraintSolver::restitutionCurveInternal(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); if (restitution <= btScalar(0.)) { restitution = 0.f; @@ -974,18 +1230,52 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra } } -void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverConstraint& solverConstraint, +void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + const btVector3& rel_pos1, const btVector3& rel_pos2) +{ + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations + ); + + + setupContactConstraintInternal(siData, solverConstraint, + solverBodyIdA, solverBodyIdB, + cp, infoGlobal, + relaxation, + rel_pos1, rel_pos2); +} + + +void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulseInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, + btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) { - btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + btSolverBody* bodyA = &tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyB = &tmpSolverBodyPool[solverBodyIdB]; btRigidBody* rb0 = bodyA->m_originalBody; btRigidBody* rb1 = bodyB->m_originalBody; { - btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; + btSolverConstraint& frictionConstraint1 = tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; @@ -1002,7 +1292,7 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverC if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; + btSolverConstraint& frictionConstraint2 = tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; @@ -1018,21 +1308,31 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverC } } -void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) +{ + setFrictionConstraintImpulseInternal(m_tmpSolverBodyPool, m_tmpSolverContactFrictionConstraintPool, + solverConstraint, + solverBodyIdA, solverBodyIdB, + cp, infoGlobal); + +} +void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingleIterationData& siData, btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) { btCollisionObject *colObj0 = 0, *colObj1 = 0; colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); - int solverBodyIdA = getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); - int solverBodyIdB = getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); + int solverBodyIdA = siData.getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = siData.getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); // btRigidBody* bodyA = btRigidBody::upcast(colObj0); // btRigidBody* bodyB = btRigidBody::upcast(colObj1); - btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + btSolverBody* solverBodyA = &siData.m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* solverBodyB = &siData.m_tmpSolverBodyPool[solverBodyIdB]; ///avoid collision response between two static objects if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero()))) @@ -1049,8 +1349,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m btVector3 rel_pos2; btScalar relaxation; - int frictionIndex = m_tmpSolverContactConstraintPool.size(); - btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); + int frictionIndex = siData.m_tmpSolverContactConstraintPool.size(); + btSolverConstraint& solverConstraint = siData.m_tmpSolverContactConstraintPool.expandNonInitializing(); solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -1071,16 +1371,20 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m btVector3 vel = vel1 - vel2; btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); - setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2); + setupContactConstraintInternal(siData, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2); /////setup the friction constraints - solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); + solverConstraint.m_frictionIndex = siData.m_tmpSolverContactFrictionConstraintPool.size(); if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0)) { { - addTorsionalFrictionConstraint(cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + + btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool, + siData.m_tmpSolverContactRollingFrictionConstraintPool, + cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + btVector3 axis0, axis1; btPlaneSpace1(cp.m_normalWorldOnB, axis0, axis1); axis0.normalize(); @@ -1091,11 +1395,17 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m applyAnisotropicFriction(colObj0, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); if (axis0.length() > 0.001) - addTorsionalFrictionConstraint(axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp, - cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + { + btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool, + siData.m_tmpSolverContactRollingFrictionConstraintPool,axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp, + cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + } if (axis1.length() > 0.001) - addTorsionalFrictionConstraint(axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, - cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + { + btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool, + siData.m_tmpSolverContactRollingFrictionConstraintPool,axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, + cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + } } } @@ -1124,7 +1434,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel); applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { @@ -1132,7 +1443,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m cp.m_lateralFrictionDir2.normalize(); //?? applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); } } else @@ -1141,13 +1453,15 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); } if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) @@ -1158,16 +1472,44 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m } else { - addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); + { + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); + } } - setFrictionConstraintImpulse(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); + btSequentialImpulseConstraintSolver::setFrictionConstraintImpulseInternal( + siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); } } } +void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) +{ + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); + + btSequentialImpulseConstraintSolver::convertContactInternal(siData, manifold, infoGlobal); +} + void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) { int i; @@ -1181,22 +1523,24 @@ void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** } } -void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow, - btTypedConstraint* constraint, - const btTypedConstraint::btConstraintInfo1& info1, - int solverBodyIdA, - int solverBodyIdB, - const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::convertJointInternal(btAlignedObjectArray& tmpSolverBodyPool, + int& maxOverrideNumSolverIterations, + btSolverConstraint* currentConstraintRow, + btTypedConstraint* constraint, + const btTypedConstraint::btConstraintInfo1& info1, + int solverBodyIdA, + int solverBodyIdB, + const btContactSolverInfo& infoGlobal) { const btRigidBody& rbA = constraint->getRigidBodyA(); const btRigidBody& rbB = constraint->getRigidBodyB(); - const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; - const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; + const btSolverBody* bodyAPtr = &tmpSolverBodyPool[solverBodyIdA]; + const btSolverBody* bodyBPtr = &tmpSolverBodyPool[solverBodyIdB]; int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; - if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations) - m_maxOverrideNumSolverIterations = overrideNumSolverIterations; + if (overrideNumSolverIterations > maxOverrideNumSolverIterations) + maxOverrideNumSolverIterations = overrideNumSolverIterations; for (int j = 0; j < info1.m_numConstraintRows; j++) { @@ -1313,7 +1657,16 @@ void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* curre } } -void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow, + btTypedConstraint* constraint, + const btTypedConstraint::btConstraintInfo1& info1, + int solverBodyIdA, + int solverBodyIdB, + const btContactSolverInfo& infoGlobal) +{ +} + +void btSequentialImpulseConstraintSolver::convertJointsInternal(btSISolverSingleIterationData& siData, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) { BT_PROFILE("convertJoints"); for (int j = 0; j < numConstraints; j++) @@ -1325,11 +1678,11 @@ void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** cons int totalNumRows = 0; - m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); + siData.m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); //calculate the total number of contraint rows for (int i = 0; i < numConstraints; i++) { - btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + btTypedConstraint::btConstraintInfo1& info1 = siData.m_tmpConstraintSizesPool[i]; btJointFeedback* fb = constraints[i]->getJointFeedback(); if (fb) { @@ -1350,34 +1703,58 @@ void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** cons } totalNumRows += info1.m_numConstraintRows; } - m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); + siData.m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); ///setup the btSolverConstraints int currentRow = 0; for (int i = 0; i < numConstraints; i++) { - const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + const btTypedConstraint::btConstraintInfo1& info1 = siData.m_tmpConstraintSizesPool[i]; if (info1.m_numConstraintRows) { btAssert(currentRow < totalNumRows); - btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; + btSolverConstraint* currentConstraintRow = &siData.m_tmpSolverNonContactConstraintPool[currentRow]; btTypedConstraint* constraint = constraints[i]; btRigidBody& rbA = constraint->getRigidBodyA(); btRigidBody& rbB = constraint->getRigidBodyB(); - int solverBodyIdA = getOrInitSolverBody(rbA, infoGlobal.m_timeStep); - int solverBodyIdB = getOrInitSolverBody(rbB, infoGlobal.m_timeStep); + int solverBodyIdA = siData.getOrInitSolverBody(rbA, infoGlobal.m_timeStep); + int solverBodyIdB = siData.getOrInitSolverBody(rbB, infoGlobal.m_timeStep); - convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal); + convertJointInternal(siData.m_tmpSolverBodyPool, siData.m_maxOverrideNumSolverIterations, + currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal); } currentRow += info1.m_numConstraintRows; } } -void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) +{ + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); + + convertJointsInternal(siData, constraints, numConstraints, infoGlobal); +} + + +void btSequentialImpulseConstraintSolver::convertBodiesInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { BT_PROFILE("convertBodies"); for (int i = 0; i < numBodies; i++) @@ -1385,23 +1762,23 @@ void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodi bodies[i]->setCompanionId(-1); } #if BT_THREADSAFE - m_kinematicBodyUniqueIdToSolverBodyTable.resize(0); + siData.m_kinematicBodyUniqueIdToSolverBodyTable.resize(0); #endif // BT_THREADSAFE - m_tmpSolverBodyPool.reserve(numBodies + 1); - m_tmpSolverBodyPool.resize(0); + siData.m_tmpSolverBodyPool.reserve(numBodies + 1); + siData.m_tmpSolverBodyPool.resize(0); //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); //initSolverBody(&fixedBody,0); for (int i = 0; i < numBodies; i++) { - int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); + int bodyId = siData.getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); btRigidBody* body = btRigidBody::upcast(bodies[i]); if (body && body->getInvMass()) { - btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + btSolverBody& solverBody = siData.m_tmpSolverBodyPool[bodyId]; btVector3 gyroForce(0, 0, 0); if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) { @@ -1422,6 +1799,29 @@ void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodi } } + +void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); + + convertBodiesInternal(siData, bodies, numBodies, infoGlobal); +} + btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { m_fixedBodyId = -1; @@ -1545,14 +1945,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol return 0.f; } -btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/) +btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) { BT_PROFILE("solveSingleIteration"); btScalar leastSquaresResidual = 0.f; - int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); - int numConstraintPool = m_tmpSolverContactConstraintPool.size(); - int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); + int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size(); + int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size(); + int numFrictionPool = siData.m_tmpSolverContactFrictionConstraintPool.size(); if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { @@ -1560,10 +1960,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration { for (int j = 0; j < numNonContactPool; ++j) { - int tmp = m_orderNonContactConstraintPool[j]; - int swapi = btRandInt2(j + 1); - m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; - m_orderNonContactConstraintPool[swapi] = tmp; + int tmp = siData.m_orderNonContactConstraintPool[j]; + int swapi = btRandInt2a(j + 1, siData.m_seed); + siData.m_orderNonContactConstraintPool[j] = siData.m_orderNonContactConstraintPool[swapi]; + siData.m_orderNonContactConstraintPool[swapi] = tmp; } //contact/friction constraints are not solved more than @@ -1571,30 +1971,30 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration { for (int j = 0; j < numConstraintPool; ++j) { - int tmp = m_orderTmpConstraintPool[j]; - int swapi = btRandInt2(j + 1); - m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; - m_orderTmpConstraintPool[swapi] = tmp; + int tmp = siData.m_orderTmpConstraintPool[j]; + int swapi = btRandInt2a(j + 1,siData.m_seed); + siData.m_orderTmpConstraintPool[j] = siData.m_orderTmpConstraintPool[swapi]; + siData.m_orderTmpConstraintPool[swapi] = tmp; } for (int j = 0; j < numFrictionPool; ++j) { - int tmp = m_orderFrictionConstraintPool[j]; - int swapi = btRandInt2(j + 1); - m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; - m_orderFrictionConstraintPool[swapi] = tmp; + int tmp = siData.m_orderFrictionConstraintPool[j]; + int swapi = btRandInt2a(j + 1,siData.m_seed); + siData.m_orderFrictionConstraintPool[j] = siData.m_orderFrictionConstraintPool[swapi]; + siData.m_orderFrictionConstraintPool[swapi] = tmp; } } } } ///solve all joint constraints - for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) + for (int j = 0; j < siData.m_tmpSolverNonContactConstraintPool.size(); j++) { - btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; + btSolverConstraint& constraint = siData.m_tmpSolverNonContactConstraintPool[siData.m_orderNonContactConstraintPool[j]]; if (iteration < constraint.m_overrideNumSolverIterations) { - btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint); + btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[constraint.m_solverBodyIdA], siData.m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } @@ -1605,10 +2005,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration { if (constraints[j]->isEnabled()) { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + int bodyAid = siData.getSolverBody(constraints[j]->getRigidBodyA()); + int bodyBid = siData.getSolverBody(constraints[j]->getRigidBodyB()); + btSolverBody& bodyA = siData.m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = siData.m_tmpSolverBodyPool[bodyBid]; constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep); } } @@ -1616,7 +2016,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration ///solve all contact constraints if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int numPoolConstraints = siData.m_tmpSolverContactConstraintPool.size(); int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1; for (int c = 0; c < numPoolConstraints; c++) @@ -1624,8 +2024,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration btScalar totalImpulse = 0; { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; - btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + const btSolverConstraint& solveManifold = siData.m_tmpSolverContactConstraintPool[siData.m_orderTmpConstraintPool[c]]; + btScalar residual = siData.m_resolveSingleConstraintRowLowerLimit(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); totalImpulse = solveManifold.m_appliedImpulse; @@ -1634,28 +2034,28 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration if (applyFriction) { { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]]; + btSolverConstraint& solveManifold = siData.m_tmpSolverContactFrictionConstraintPool[siData.m_orderFrictionConstraintPool[c * multiplier]]; if (totalImpulse > btScalar(0)) { solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]]; + btSolverConstraint& solveManifold = siData.m_tmpSolverContactFrictionConstraintPool[siData.m_orderFrictionConstraintPool[c * multiplier + 1]]; if (totalImpulse > btScalar(0)) { solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } @@ -1665,40 +2065,40 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration else //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS { //solve the friction constraints after all contact constraints, don't interleave them - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int numPoolConstraints = siData.m_tmpSolverContactConstraintPool.size(); int j; for (j = 0; j < numPoolConstraints; j++) { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + const btSolverConstraint& solveManifold = siData.m_tmpSolverContactConstraintPool[siData.m_orderTmpConstraintPool[j]]; + btScalar residual = siData.m_resolveSingleConstraintRowLowerLimit(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } ///solve all friction constraints - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + int numFrictionPoolConstraints = siData.m_tmpSolverContactFrictionConstraintPool.size(); for (j = 0; j < numFrictionPoolConstraints; j++) { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + btSolverConstraint& solveManifold = siData.m_tmpSolverContactFrictionConstraintPool[siData.m_orderFrictionConstraintPool[j]]; + btScalar totalImpulse = siData.m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; if (totalImpulse > btScalar(0)) { solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } } - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + int numRollingFrictionPoolConstraints = siData.m_tmpSolverContactRollingFrictionConstraintPool.size(); for (int j = 0; j < numRollingFrictionPoolConstraints; j++) { - btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; + btSolverConstraint& rollingFrictionConstraint = siData.m_tmpSolverContactRollingFrictionConstraintPool[j]; + btScalar totalImpulse = siData.m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; if (totalImpulse > btScalar(0)) { btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; @@ -1708,7 +2108,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint); + btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], siData.m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } @@ -1716,7 +2116,55 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration return leastSquaresResidual; } + +btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/) +{ + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); + + btScalar leastSquaresResidual = btSequentialImpulseConstraintSolver::solveSingleIterationInternal(siData, + iteration, constraints, numConstraints, infoGlobal); + return leastSquaresResidual; +} + void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +{ + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); + + solveGroupCacheFriendlySplitImpulseIterationsInternal(siData, + bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + +} +void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterationsInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); int iteration; @@ -1727,13 +2175,13 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte { btScalar leastSquaresResidual = 0.f; { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int numPoolConstraints = siData.m_tmpSolverContactConstraintPool.size(); int j; for (j = 0; j < numPoolConstraints; j++) { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + const btSolverConstraint& solveManifold = siData.m_tmpSolverContactConstraintPool[siData.m_orderTmpConstraintPool[j]]; - btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + btScalar residual = siData.m_resolveSplitPenetrationImpulse(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } @@ -1776,31 +2224,42 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( return 0.f; } -void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::writeBackContactsInternal(btConstraintArray& tmpSolverContactConstraintPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { for (int j = iBegin; j < iEnd; j++) { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; + const btSolverConstraint& solveManifold = tmpSolverContactConstraintPool[j]; btManifoldPoint* pt = (btManifoldPoint*)solveManifold.m_originalContactPoint; btAssert(pt); pt->m_appliedImpulse = solveManifold.m_appliedImpulse; // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; // printf("pt->m_appliedImpulseLateral1 = %f\n", f); - pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + pt->m_appliedImpulseLateral1 = tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse; + pt->m_appliedImpulseLateral2 = tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse; } //do a callback here? } } +void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) +{ + writeBackContactsInternal(m_tmpSolverContactConstraintPool, m_tmpSolverContactFrictionConstraintPool, iBegin, iEnd, infoGlobal); + +} + void btSequentialImpulseConstraintSolver::writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) +{ + writeBackJointsInternal(m_tmpSolverNonContactConstraintPool, iBegin, iEnd, infoGlobal); +} + +void btSequentialImpulseConstraintSolver::writeBackJointsInternal(btConstraintArray& tmpSolverNonContactConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { for (int j = iBegin; j < iEnd; j++) { - const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; + const btSolverConstraint& solverConstr = tmpSolverNonContactConstraintPool[j]; btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; btJointFeedback* fb = constr->getJointFeedback(); if (fb) @@ -1820,54 +2279,80 @@ void btSequentialImpulseConstraintSolver::writeBackJoints(int iBegin, int iEnd, } void btSequentialImpulseConstraintSolver::writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) +{ + writeBackBodiesInternal(m_tmpSolverBodyPool,iBegin, iEnd, infoGlobal); +} +void btSequentialImpulseConstraintSolver::writeBackBodiesInternal(btAlignedObjectArray& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { for (int i = iBegin; i < iEnd; i++) { - btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; + btRigidBody* body = tmpSolverBodyPool[i].m_originalBody; if (body) { if (infoGlobal.m_splitImpulse) - m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); + tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); else - m_tmpSolverBodyPool[i].writebackVelocity(); + tmpSolverBodyPool[i].writebackVelocity(); - m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity( - m_tmpSolverBodyPool[i].m_linearVelocity + - m_tmpSolverBodyPool[i].m_externalForceImpulse); + tmpSolverBodyPool[i].m_originalBody->setLinearVelocity( + tmpSolverBodyPool[i].m_linearVelocity + + tmpSolverBodyPool[i].m_externalForceImpulse); - m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity( - m_tmpSolverBodyPool[i].m_angularVelocity + - m_tmpSolverBodyPool[i].m_externalTorqueImpulse); + tmpSolverBodyPool[i].m_originalBody->setAngularVelocity( + tmpSolverBodyPool[i].m_angularVelocity + + tmpSolverBodyPool[i].m_externalTorqueImpulse); if (infoGlobal.m_splitImpulse) - m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform); + tmpSolverBodyPool[i].m_originalBody->setWorldTransform(tmpSolverBodyPool[i].m_worldTransform); - m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1); + tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1); } } } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { BT_PROFILE("solveGroupCacheFriendlyFinish"); if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { - writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal); + writeBackContactsInternal(siData.m_tmpSolverContactConstraintPool,siData.m_tmpSolverContactFrictionConstraintPool,0, siData.m_tmpSolverContactConstraintPool.size(), infoGlobal); } - writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal); - writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal); + writeBackJointsInternal(siData.m_tmpSolverNonContactConstraintPool, 0, siData.m_tmpSolverNonContactConstraintPool.size(), infoGlobal); + writeBackBodiesInternal(siData.m_tmpSolverBodyPool,0, siData.m_tmpSolverBodyPool.size(), infoGlobal); - m_tmpSolverContactConstraintPool.resizeNoInitialize(0); - m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); - m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); - m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); + siData.m_tmpSolverContactConstraintPool.resizeNoInitialize(0); + siData.m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); + siData.m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); + siData.m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); - m_tmpSolverBodyPool.resizeNoInitialize(0); + siData.m_tmpSolverBodyPool.resizeNoInitialize(0); return 0.f; } +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); + + return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, infoGlobal); +} + /// btSequentialImpulseConstraintSolver Sequentially applies impulses btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btDispatcher* /*dispatcher*/) { diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 70db83b06..f9dc59a40 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -29,6 +29,68 @@ class btCollisionObject; typedef btScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&); +struct btSISolverSingleIterationData +{ + btAlignedObjectArray& m_tmpSolverBodyPool; + btConstraintArray& m_tmpSolverContactConstraintPool; + btConstraintArray& m_tmpSolverNonContactConstraintPool; + btConstraintArray& m_tmpSolverContactFrictionConstraintPool; + btConstraintArray& m_tmpSolverContactRollingFrictionConstraintPool; + + btAlignedObjectArray& m_orderTmpConstraintPool; + btAlignedObjectArray& m_orderNonContactConstraintPool; + btAlignedObjectArray& m_orderFrictionConstraintPool; + btAlignedObjectArray& m_tmpConstraintSizesPool; + unsigned long& m_seed; + + btSingleConstraintRowSolver& m_resolveSingleConstraintRowGeneric; + btSingleConstraintRowSolver& m_resolveSingleConstraintRowLowerLimit; + btSingleConstraintRowSolver& m_resolveSplitPenetrationImpulse; + btAlignedObjectArray& m_kinematicBodyUniqueIdToSolverBodyTable; + int& m_fixedBodyId; + int& m_maxOverrideNumSolverIterations; + int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep); + static void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep); + int getSolverBody(btCollisionObject& body) const; + + + btSISolverSingleIterationData(btAlignedObjectArray& tmpSolverBodyPool, + btConstraintArray& tmpSolverContactConstraintPool, + btConstraintArray& tmpSolverNonContactConstraintPool, + btConstraintArray& tmpSolverContactFrictionConstraintPool, + btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, + btAlignedObjectArray& orderTmpConstraintPool, + btAlignedObjectArray& orderNonContactConstraintPool, + btAlignedObjectArray& orderFrictionConstraintPool, + btAlignedObjectArray& tmpConstraintSizesPool, + btSingleConstraintRowSolver& resolveSingleConstraintRowGeneric, + btSingleConstraintRowSolver& resolveSingleConstraintRowLowerLimit, + btSingleConstraintRowSolver& resolveSplitPenetrationImpulse, + btAlignedObjectArray& kinematicBodyUniqueIdToSolverBodyTable, + unsigned long& seed, + int& fixedBodyId, + int& maxOverrideNumSolverIterations + ) + :m_tmpSolverBodyPool(tmpSolverBodyPool), + m_tmpSolverContactConstraintPool(tmpSolverContactConstraintPool), + m_tmpSolverNonContactConstraintPool(tmpSolverNonContactConstraintPool), + m_tmpSolverContactFrictionConstraintPool(tmpSolverContactFrictionConstraintPool), + m_tmpSolverContactRollingFrictionConstraintPool(tmpSolverContactRollingFrictionConstraintPool), + m_orderTmpConstraintPool(orderTmpConstraintPool), + m_orderNonContactConstraintPool(orderNonContactConstraintPool), + m_orderFrictionConstraintPool(orderFrictionConstraintPool), + m_tmpConstraintSizesPool(tmpConstraintSizesPool), + m_resolveSingleConstraintRowGeneric(resolveSingleConstraintRowGeneric), + m_resolveSingleConstraintRowLowerLimit(resolveSingleConstraintRowLowerLimit), + m_resolveSplitPenetrationImpulse(resolveSplitPenetrationImpulse), + m_kinematicBodyUniqueIdToSolverBodyTable(kinematicBodyUniqueIdToSolverBodyTable), + m_seed(seed), + m_fixedBodyId(fixedBodyId), + m_maxOverrideNumSolverIterations(maxOverrideNumSolverIterations) + { + } +}; + ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver @@ -97,6 +159,7 @@ protected: virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal); void convertJoint(btSolverConstraint * currentConstraintRow, btTypedConstraint * constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal); + virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint) @@ -123,13 +186,15 @@ protected: } protected: + void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - + + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); @@ -141,13 +206,52 @@ public: virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + static btScalar solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal); + static void convertBodiesInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); + static void convertJointsInternal(btSISolverSingleIterationData& siData, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal); + static void convertContactInternal(btSISolverSingleIterationData& siData, btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal); + static void setupContactConstraintInternal(btSISolverSingleIterationData& siData, btSolverConstraint& solverConstraint,int solverBodyIdA, int solverBodyIdB,btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,btScalar& relaxation, + const btVector3& rel_pos1, const btVector3& rel_pos2); + static btScalar restitutionCurveInternal(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold); + static btSolverConstraint& addTorsionalFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.); + static void setupTorsionalFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, + btScalar desiredVelocity, btScalar cfmSlip); + static void setupFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip); + static btSolverConstraint& addFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.); + static void setFrictionConstraintImpulseInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, + + btSolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); + static void convertJointInternal(btAlignedObjectArray& tmpSolverBodyPool, + int& maxOverrideNumSolverIterations, + btSolverConstraint* currentConstraintRow, + btTypedConstraint* constraint, + const btTypedConstraint::btConstraintInfo1& info1, + int solverBodyIdA, + int solverBodyIdB, + const btContactSolverInfo& infoGlobal); + + static btScalar solveGroupCacheFriendlyFinishInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); + + static void writeBackContactsInternal(btConstraintArray& tmpSolverContactConstraintPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + + static void writeBackJointsInternal(btConstraintArray& tmpSolverNonContactConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + static void writeBackBodiesInternal(btAlignedObjectArray& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + static void solveGroupCacheFriendlySplitImpulseIterationsInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + + ///clear internal cached data and reset random seed virtual void reset(); unsigned long btRand2(); - int btRandInt2(int n); + static unsigned long btRand2a(unsigned long& seed); + static int btRandInt2a(int n, unsigned long& seed); + void setRandSeed(unsigned long seed) { m_btSeed2 = seed; @@ -180,14 +284,18 @@ public: } ///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4 - btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric(); - btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric(); - btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric(); + static btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric(); + static btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric(); + static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric(); ///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4 - btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit(); - btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit(); - btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit(); + static btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit(); + static btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit(); + static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit(); + + static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric(); + static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric(); + }; #endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 1557987bc..a81092782 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -362,7 +362,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: }; btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) - : btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), + : btDiscreteDynamicsWorld(dispatcher, pairCache, 0, collisionConfiguration), m_multiBodyConstraintSolver(constraintSolver) { //split impulse is not yet supported for Featherstone hierarchies @@ -380,7 +380,7 @@ void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstrain { m_multiBodyConstraintSolver = solver; m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver); - btDiscreteDynamicsWorld::setConstraintSolver(solver); + //btDiscreteDynamicsWorld::setConstraintSolver(solver); } void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h index 6be36ba14..77fdb86bb 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h @@ -156,7 +156,7 @@ protected: btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, - btIDebugDraw* debugDrawer) BT_OVERRIDE; + btIDebugDraw* debugDrawer) ; public: BT_DECLARE_ALIGNED_ALLOCATOR() From d7e087de169c52235467d8e0f9ecdc8160a974e7 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 26 Feb 2019 23:27:05 -0800 Subject: [PATCH 2/6] prepare small experiment with block solver --- examples/BlockSolver/BlockSolverExample.cpp | 2 +- examples/BlockSolver/btBlockSolver.h | 2 +- examples/ConstraintSolvers/BoxStacks.cpp | 335 ++++ examples/ConstraintSolvers/BoxStacks.h | 7 + examples/ConstraintSolvers/BoxStacks_MLCP.cpp | 224 +++ examples/ConstraintSolvers/BoxStacks_MLCP.h | 7 + examples/ConstraintSolvers/Grasp_Block.cpp | 335 ++++ examples/ConstraintSolvers/Grasp_Block.h | 7 + .../SerialChains.cpp | 51 +- .../SerialChains.h | 0 examples/ExampleBrowser/CMakeLists.txt | 5 +- examples/ExampleBrowser/ExampleEntries.cpp | 12 +- examples/ExampleBrowser/premake4.lua | 1 + src/BulletDynamics/CMakeLists.txt | 1 + .../btSequentialImpulseConstraintSolver.cpp | 83 +- .../btSequentialImpulseConstraintSolver.h | 10 +- .../btMultiBodyBlockConstraintSolver.cpp | 760 +++++++++ .../btMultiBodyBlockConstraintSolver.h | 186 ++ .../btMultiBodyConstraintSolver.cpp | 1518 +++++++++-------- .../btMultiBodyConstraintSolver.h | 131 +- 20 files changed, 2877 insertions(+), 800 deletions(-) create mode 100644 examples/ConstraintSolvers/BoxStacks.cpp create mode 100644 examples/ConstraintSolvers/BoxStacks.h create mode 100644 examples/ConstraintSolvers/BoxStacks_MLCP.cpp create mode 100644 examples/ConstraintSolvers/BoxStacks_MLCP.h create mode 100644 examples/ConstraintSolvers/Grasp_Block.cpp create mode 100644 examples/ConstraintSolvers/Grasp_Block.h rename examples/{MultiBody => ConstraintSolvers}/SerialChains.cpp (88%) rename examples/{MultiBody => ConstraintSolvers}/SerialChains.h (100%) create mode 100644 src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.cpp create mode 100644 src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h diff --git a/examples/BlockSolver/BlockSolverExample.cpp b/examples/BlockSolver/BlockSolverExample.cpp index e0bff80bb..11db6e631 100644 --- a/examples/BlockSolver/BlockSolverExample.cpp +++ b/examples/BlockSolver/BlockSolverExample.cpp @@ -115,7 +115,7 @@ void BlockSolverExample::initPhysics() } if (m_option&BLOCK_SOLVER_BLOCK) { - //m_solver = new btBlockSolver(); + m_solver = new btBlockSolver(); } btAssert(m_solver); diff --git a/examples/BlockSolver/btBlockSolver.h b/examples/BlockSolver/btBlockSolver.h index 8ea3e9742..4d952db92 100644 --- a/examples/BlockSolver/btBlockSolver.h +++ b/examples/BlockSolver/btBlockSolver.h @@ -3,7 +3,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" -class btBlockSolver : public btConstraintSolver +class btBlockSolver : public btMultiBodyConstraintSolver { struct btBlockSolverInternalData* m_data; diff --git a/examples/ConstraintSolvers/BoxStacks.cpp b/examples/ConstraintSolvers/BoxStacks.cpp new file mode 100644 index 000000000..d08c1459b --- /dev/null +++ b/examples/ConstraintSolvers/BoxStacks.cpp @@ -0,0 +1,335 @@ +#include "BoxStacks.h" +#include "../OpenGLWindow/SimpleOpenGL3App.h" +#include "btBulletDynamicsCommon.h" + +#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" +#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" + +#include "BulletDynamics/Featherstone/btMultiBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyLink.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" +#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" + +#include "../OpenGLWindow/GLInstancingRenderer.h" +#include "BulletCollision/CollisionShapes/btShapeHull.h" + +#include "../CommonInterfaces/CommonMultiBodyBase.h" + +class BoxStacks : public CommonMultiBodyBase +{ +public: + BoxStacks(GUIHelperInterface* helper); + virtual ~BoxStacks(); + + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + + virtual void resetCamera() + { + float dist = 1; + float pitch = -35; + float yaw = 50; + float targetPos[3] = {-3, 2.8, -2.5}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void createBoxStack(int numBoxes, btScalar centerX, btScalar centerY); + btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool fixedBase = false); + void createGround(const btVector3& halfExtents = btVector3(50, 50, 50), btScalar zOffSet = btScalar(-1.55)); + void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents); +}; + +static bool g_fixedBase = true; +static bool g_firstInit = true; +static float scaling = 0.4f; +static float friction = 1.; +static int g_constraintSolverType = 0; + +BoxStacks::BoxStacks(GUIHelperInterface* helper) + : CommonMultiBodyBase(helper) +{ + m_guiHelper->setUpAxis(1); +} + +BoxStacks::~BoxStacks() +{ + // Do nothing +} + +void BoxStacks::stepSimulation(float deltaTime) +{ + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep); +} + +void BoxStacks::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + if (g_firstInit) + { + m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10. * scaling)); + m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50); + g_firstInit = false; + } + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + + if (g_constraintSolverType == 3) + { + g_constraintSolverType = 0; + g_fixedBase = !g_fixedBase; + } + + btMLCPSolverInterface* mlcp; + switch (g_constraintSolverType++) + { + case 0: + m_solver = new btMultiBodyConstraintSolver; + b3Printf("Constraint Solver: Sequential Impulse"); + break; + case 1: + mlcp = new btSolveProjectedGaussSeidel(); + m_solver = new btMultiBodyMLCPConstraintSolver(mlcp); + b3Printf("Constraint Solver: MLCP + PGS"); + break; + default: + mlcp = new btDantzigSolver(); + m_solver = new btMultiBodyMLCPConstraintSolver(mlcp); + b3Printf("Constraint Solver: MLCP + Dantzig"); + break; + } + m_solver = new btMultiBodyBlockConstraintSolver(); + + btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); + m_dynamicsWorld = world; + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->setGravity(btVector3(btScalar(0), btScalar(-9.81), btScalar(0))); + m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good? + + /// Create a few basic rigid bodies + btVector3 groundHalfExtents(50, 50, 50); + btCollisionShape* groundShape = new btBoxShape(groundHalfExtents); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -50, 00)); + + btVector3 linkHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1)); + btVector3 baseHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1)); + + createBoxStack(1, 0, 0); + + btScalar groundHeight = btScalar(-51.55); + btScalar mass = btScalar(0.0); + + btVector3 localInertia(0, 0, 0); + groundShape->calculateLocalInertia(mass, localInertia); + + // Using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, groundHeight, 0)); + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + // Add the body to the dynamics world + m_dynamicsWorld->addRigidBody(body, 1, 1 + 2); + + createGround(); + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void BoxStacks::createBoxStack(int numBoxes, btScalar centerX, btScalar centerZ) +{ + //create a few dynamic rigidbodies + // Re-using the same collision is better for memory usage and performance + + const btScalar boxHalfSize = btScalar(0.1); + + btBoxShape* colShape = createBoxShape(btVector3(boxHalfSize, boxHalfSize, boxHalfSize)); + m_collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + btScalar mass(1.0); + + btVector3 localInertia(0, 0, 0); + colShape->calculateLocalInertia(mass,localInertia); + + for (int i = 0; i < numBoxes; ++i) + { + startTransform.setOrigin(btVector3(centerX, 1+btScalar(btScalar(2) * boxHalfSize * i), centerZ)); + createRigidBody(mass, startTransform, colShape); + } +} + +btMultiBody* BoxStacks::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool fixedBase) +{ + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = 1.f; + + if (baseMass) + { + btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); + delete pTempBox; + } + + bool canSleep = false; + + btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, fixedBase, canSleep); + + btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); + pMultiBody->setBasePos(basePosition); + pMultiBody->setWorldToBaseRot(baseOriQuat); + btVector3 vel(0, 0, 0); + + //init the links + btVector3 hingeJointAxis(1, 0, 0); + float linkMass = 1.f; + btVector3 linkInertiaDiag(0.f, 0.f, 0.f); + + btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); + pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); + delete pTempBox; + + //y-axis assumed up + btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + ////// + btScalar q0 = 0.f * SIMD_PI / 180.f; + btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); + quat0.normalize(); + ///// + + for (int i = 0; i < numLinks; ++i) + { + if (!spherical) + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true); + else + //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true); + } + + pMultiBody->finalizeMultiDof(); + + /// + pWorld->addMultiBody(pMultiBody); + /// + return pMultiBody; +} + +void BoxStacks::createGround(const btVector3& halfExtents, btScalar zOffSet) +{ + btCollisionShape* groundShape = new btBoxShape(halfExtents); + m_collisionShapes.push_back(groundShape); + + // rigidbody is dynamic if and only if mass is non zero, otherwise static + btScalar mass(0.); + const bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + // using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -halfExtents.z() + zOffSet, 0)); + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + // add the body to the dynamics world + m_dynamicsWorld->addRigidBody(body, 1, 1 + 2); +} + +void BoxStacks::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) +{ + btAlignedObjectArray world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray local_origin; + local_origin.resize(pMultiBody->getNumLinks() + 1); + world_to_local[0] = pMultiBody->getWorldToBaseRot(); + local_origin[0] = pMultiBody->getBasePos(); + + { + btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; + + if (1) + { + btCollisionShape* box = new btBoxShape(baseHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + + pWorld->addCollisionObject(col, 2, 1 + 2); + + col->setFriction(friction); + pMultiBody->setBaseCollider(col); + } + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + const int parent = pMultiBody->getParent(i); + world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1]; + local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i))); + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + btVector3 posr = local_origin[i + 1]; + + btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()}; + + btCollisionShape* box = new btBoxShape(linkHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); + + col->setCollisionShape(box); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + col->setFriction(friction); + pWorld->addCollisionObject(col, 2, 1 + 2); + + pMultiBody->getLink(i).m_collider = col; + } +} + +CommonExampleInterface* BoxStacksCreateFunc(CommonExampleOptions& options) +{ + return new BoxStacks(options.m_guiHelper); +} diff --git a/examples/ConstraintSolvers/BoxStacks.h b/examples/ConstraintSolvers/BoxStacks.h new file mode 100644 index 000000000..1020c3093 --- /dev/null +++ b/examples/ConstraintSolvers/BoxStacks.h @@ -0,0 +1,7 @@ + +#ifndef CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H +#define CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H + +class CommonExampleInterface* BoxStacksCreateFunc(struct CommonExampleOptions& options); + +#endif // CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H diff --git a/examples/ConstraintSolvers/BoxStacks_MLCP.cpp b/examples/ConstraintSolvers/BoxStacks_MLCP.cpp new file mode 100644 index 000000000..14bff8a5e --- /dev/null +++ b/examples/ConstraintSolvers/BoxStacks_MLCP.cpp @@ -0,0 +1,224 @@ +#include "BoxStacks_MLCP.h" +#include "../OpenGLWindow/SimpleOpenGL3App.h" +#include "btBulletDynamicsCommon.h" +#include "BulletDynamics/Featherstone/btMultiBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "../CommonInterfaces/CommonMultiBodyBase.h" +#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" +#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" +#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" +#include "../Importers/ImportURDFDemo/URDF2Bullet.h" +#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h" +#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" +#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" + + +class BoxStacks_MLCP : public CommonMultiBodyBase +{ +public: + BoxStacks_MLCP(GUIHelperInterface* helper); + virtual ~BoxStacks_MLCP(); + + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + + virtual void resetCamera() + { + float dist = 2; + float pitch = -35; + float yaw = 50; + float targetPos[3] = {0, 0, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + btMultiBody* createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape); + + btMultiBody* loadRobot(std::string filepath = "kuka_iiwa/model.urdf"); +}; + + + +static int g_constraintSolverType = 0; + +BoxStacks_MLCP::BoxStacks_MLCP(GUIHelperInterface* helper) + : CommonMultiBodyBase(helper) +{ + +} + +BoxStacks_MLCP::~BoxStacks_MLCP() +{ + // Do nothing +} + +void BoxStacks_MLCP::stepSimulation(float deltaTime) +{ + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep); + for (int i = 0; i < m_dynamicsWorld->getNumMultibodies(); i++) + { + btVector3 pos = m_dynamicsWorld->getMultiBody(i)->getBaseWorldTransform().getOrigin(); + printf("pos[%d]=%f,%f,%f\n", i, pos.x(), pos.y(), pos.z()); + } +} + +void BoxStacks_MLCP::initPhysics() +{ + m_guiHelper->setUpAxis(2); + + createEmptyDynamicsWorld(); + m_dynamicsWorld->getSolverInfo().m_numIterations = 50; + if (g_constraintSolverType == 5) + { + g_constraintSolverType = 0; + } + + btMultiBodyConstraintSolver* sol = 0; + + btMLCPSolverInterface* mlcp; + switch (g_constraintSolverType++) + { + case 0: + sol = new btMultiBodyConstraintSolver; + b3Printf("Constraint Solver: Sequential Impulse"); + break; + case 1: + mlcp = new btSolveProjectedGaussSeidel(); + sol = new btMultiBodyMLCPConstraintSolver(mlcp); + b3Printf("Constraint Solver: MLCP + PGS"); + break; + case 2: + mlcp = new btDantzigSolver(); + sol = new btMultiBodyMLCPConstraintSolver(mlcp); + b3Printf("Constraint Solver: MLCP + Dantzig"); + break; + case 3: + mlcp = new btLemkeSolver(); + sol = new btMultiBodyMLCPConstraintSolver(mlcp); + + b3Printf("Constraint Solver: MLCP + Lemke"); + break; + + default: + sol = new btMultiBodyBlockConstraintSolver(); + b3Printf("btMultiBodyBlockConstraintSolver"); + break; + } + + m_solver = sol; + + btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration); + m_dynamicsWorld = world; + m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); + + + m_dynamicsWorld->setGravity(btVector3(0,0,-10)); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + if (m_dynamicsWorld->getDebugDrawer()) + m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); + + ///create a few basic rigid bodies + bool loadPlaneFromURDF = true; + if (loadPlaneFromURDF) + { + loadRobot("plane.urdf"); + } else + { + btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.), btScalar(50.), btScalar(50.))); + m_collisionShapes.push_back(groundShape); + btScalar mass = 0; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(0, 0, -50)); + btMultiBody* body = createMultiBody(mass, tr, groundShape); + } + + { + btBoxShape* boxShape = createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1))); + m_collisionShapes.push_back(boxShape); + btScalar mass = 10; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(0, 0, 0.5)); + btMultiBody* body = createMultiBody(mass, tr, boxShape); + } + + { + btMultiBody* mb = loadRobot("cube_small.urdf"); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(0, 0, 1.)); + mb->setBaseWorldTransform(tr); + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + + + +btMultiBody* BoxStacks_MLCP::createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape) +{ + btVector3 inertia; + collisionShape->calculateLocalInertia(mass, inertia); + + bool canSleep = false; + bool isDynamic = mass > 0; + btMultiBody* mb = new btMultiBody(0, mass, inertia, !isDynamic,canSleep); + btMultiBodyLinkCollider* collider = new btMultiBodyLinkCollider(mb, -1); + collider->setWorldTransform(trans); + mb->setBaseWorldTransform(trans); + + collider->setCollisionShape(collisionShape); + + int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); + int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + + + this->m_dynamicsWorld->addCollisionObject(collider, collisionFilterGroup, collisionFilterMask); + mb->setBaseCollider(collider); + + mb->finalizeMultiDof(); + + + this->m_dynamicsWorld->addMultiBody(mb); + m_dynamicsWorld->forwardKinematics(); + return mb; +} + + + +btMultiBody*BoxStacks_MLCP::loadRobot(std::string filepath) +{ + btMultiBody* m_multiBody = 0; + BulletURDFImporter u2b(m_guiHelper,0,0,1,0); + bool loadOk = u2b.loadURDF(filepath.c_str());// lwr / kuka.urdf"); + if (loadOk) + { + int rootLinkIndex = u2b.getRootLinkIndex(); + b3Printf("urdf root link index = %d\n",rootLinkIndex); + MyMultiBodyCreator creation(m_guiHelper); + btTransform identityTrans; + identityTrans.setIdentity(); + ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix()); + for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++) + { + m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i)); + } + m_multiBody = creation.getBulletMultiBody(); + if (m_multiBody) + { + b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex()).c_str()); + } + } + return m_multiBody; +} + +CommonExampleInterface* BoxStacks_MLCPCreateFunc(CommonExampleOptions& options) +{ + return new BoxStacks_MLCP(options.m_guiHelper); +} diff --git a/examples/ConstraintSolvers/BoxStacks_MLCP.h b/examples/ConstraintSolvers/BoxStacks_MLCP.h new file mode 100644 index 000000000..994fb1d92 --- /dev/null +++ b/examples/ConstraintSolvers/BoxStacks_MLCP.h @@ -0,0 +1,7 @@ + +#ifndef CONSTRAINT_SOLVERS_BOX_STACKS_MLCP_DEMO_H +#define CONSTRAINT_SOLVERS_BOX_STACKS_MLCP_DEMO_H + +class CommonExampleInterface* BoxStacks_MLCPCreateFunc(struct CommonExampleOptions& options); + +#endif // CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H diff --git a/examples/ConstraintSolvers/Grasp_Block.cpp b/examples/ConstraintSolvers/Grasp_Block.cpp new file mode 100644 index 000000000..81dd7ffcd --- /dev/null +++ b/examples/ConstraintSolvers/Grasp_Block.cpp @@ -0,0 +1,335 @@ +#include "Grasp_Block.h" +#include "../OpenGLWindow/SimpleOpenGL3App.h" +#include "btBulletDynamicsCommon.h" + +#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" +#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" + +#include "BulletDynamics/Featherstone/btMultiBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyLink.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" +#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" + +#include "../OpenGLWindow/GLInstancingRenderer.h" +#include "BulletCollision/CollisionShapes/btShapeHull.h" + +#include "../CommonInterfaces/CommonMultiBodyBase.h" + +class Grasp_Block : public CommonMultiBodyBase +{ +public: + Grasp_Block(GUIHelperInterface* helper); + virtual ~Grasp_Block(); + + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + + virtual void resetCamera() + { + float dist = 1; + float pitch = -35; + float yaw = 50; + float targetPos[3] = {-3, 2.8, -2.5}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void createBoxStack(int numBoxes, btScalar centerX, btScalar centerY); + btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool fixedBase = false); + void createGround(const btVector3& halfExtents = btVector3(50, 50, 50), btScalar zOffSet = btScalar(-1.55)); + void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents); +}; + +static bool g_fixedBase = true; +static bool g_firstInit = true; +static float scaling = 0.4f; +static float friction = 1.; +static int g_constraintSolverType = 0; + +Grasp_Block::Grasp_Block(GUIHelperInterface* helper) + : CommonMultiBodyBase(helper) +{ + m_guiHelper->setUpAxis(1); +} + +Grasp_Block::~Grasp_Block() +{ + // Do nothing +} + +void Grasp_Block::stepSimulation(float deltaTime) +{ + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep); +} + +void Grasp_Block::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + if (g_firstInit) + { + m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10. * scaling)); + m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50); + g_firstInit = false; + } + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + + if (g_constraintSolverType == 3) + { + g_constraintSolverType = 0; + g_fixedBase = !g_fixedBase; + } + + btMLCPSolverInterface* mlcp; + switch (g_constraintSolverType++) + { + case 0: + m_solver = new btMultiBodyConstraintSolver; + b3Printf("Constraint Solver: Sequential Impulse"); + break; + case 1: + mlcp = new btSolveProjectedGaussSeidel(); + m_solver = new btMultiBodyMLCPConstraintSolver(mlcp); + b3Printf("Constraint Solver: MLCP + PGS"); + break; + default: + mlcp = new btDantzigSolver(); + m_solver = new btMultiBodyMLCPConstraintSolver(mlcp); + b3Printf("Constraint Solver: MLCP + Dantzig"); + break; + } + m_solver = new btMultiBodyBlockConstraintSolver(); + + btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); + m_dynamicsWorld = world; + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->setGravity(btVector3(btScalar(0), btScalar(-9.81), btScalar(0))); + m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good? + + /// Create a few basic rigid bodies + btVector3 groundHalfExtents(50, 50, 50); + btCollisionShape* groundShape = new btBoxShape(groundHalfExtents); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -50, 00)); + + btVector3 linkHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1)); + btVector3 baseHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1)); + +// createBoxStack(5, 0, 0); + + btScalar groundHeight = btScalar(-51.55); + btScalar mass = btScalar(0.0); + + btVector3 localInertia(0, 0, 0); + groundShape->calculateLocalInertia(mass, localInertia); + + // Using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, groundHeight, 0)); + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + // Add the body to the dynamics world + m_dynamicsWorld->addRigidBody(body, 1, 1 + 2); + + createGround(); + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void Grasp_Block::createBoxStack(int numBoxes, btScalar centerX, btScalar centerZ) +{ + //create a few dynamic rigidbodies + // Re-using the same collision is better for memory usage and performance + + const btScalar boxHalfSize = btScalar(0.1); + + btBoxShape* colShape = createBoxShape(btVector3(boxHalfSize, boxHalfSize, boxHalfSize)); + m_collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + btScalar mass(1.0); + + btVector3 localInertia(0, 0, 0); + colShape->calculateLocalInertia(mass,localInertia); + + for (int i = 0; i < numBoxes; ++i) + { + startTransform.setOrigin(btVector3(centerX, btScalar(btScalar(2) * boxHalfSize * i), centerZ)); + createRigidBody(mass, startTransform, colShape); + } +} + +btMultiBody* Grasp_Block::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool fixedBase) +{ + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = 1.f; + + if (baseMass) + { + btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); + delete pTempBox; + } + + bool canSleep = false; + + btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, fixedBase, canSleep); + + btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); + pMultiBody->setBasePos(basePosition); + pMultiBody->setWorldToBaseRot(baseOriQuat); + btVector3 vel(0, 0, 0); + + //init the links + btVector3 hingeJointAxis(1, 0, 0); + float linkMass = 1.f; + btVector3 linkInertiaDiag(0.f, 0.f, 0.f); + + btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); + pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); + delete pTempBox; + + //y-axis assumed up + btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + ////// + btScalar q0 = 0.f * SIMD_PI / 180.f; + btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); + quat0.normalize(); + ///// + + for (int i = 0; i < numLinks; ++i) + { + if (!spherical) + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true); + else + //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true); + } + + pMultiBody->finalizeMultiDof(); + + /// + pWorld->addMultiBody(pMultiBody); + /// + return pMultiBody; +} + +void Grasp_Block::createGround(const btVector3& halfExtents, btScalar zOffSet) +{ + btCollisionShape* groundShape = new btBoxShape(halfExtents); + m_collisionShapes.push_back(groundShape); + + // rigidbody is dynamic if and only if mass is non zero, otherwise static + btScalar mass(0.); + const bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + // using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -halfExtents.z() + zOffSet, 0)); + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + // add the body to the dynamics world + m_dynamicsWorld->addRigidBody(body, 1, 1 + 2); +} + +void Grasp_Block::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) +{ + btAlignedObjectArray world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray local_origin; + local_origin.resize(pMultiBody->getNumLinks() + 1); + world_to_local[0] = pMultiBody->getWorldToBaseRot(); + local_origin[0] = pMultiBody->getBasePos(); + + { + btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; + + if (1) + { + btCollisionShape* box = new btBoxShape(baseHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + + pWorld->addCollisionObject(col, 2, 1 + 2); + + col->setFriction(friction); + pMultiBody->setBaseCollider(col); + } + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + const int parent = pMultiBody->getParent(i); + world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1]; + local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i))); + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + btVector3 posr = local_origin[i + 1]; + + btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()}; + + btCollisionShape* box = new btBoxShape(linkHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); + + col->setCollisionShape(box); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + col->setFriction(friction); + pWorld->addCollisionObject(col, 2, 1 + 2); + + pMultiBody->getLink(i).m_collider = col; + } +} + +CommonExampleInterface* Grasp_BlockCreateFunc(CommonExampleOptions& options) +{ + return new Grasp_Block(options.m_guiHelper); +} diff --git a/examples/ConstraintSolvers/Grasp_Block.h b/examples/ConstraintSolvers/Grasp_Block.h new file mode 100644 index 000000000..ffed47269 --- /dev/null +++ b/examples/ConstraintSolvers/Grasp_Block.h @@ -0,0 +1,7 @@ + +#ifndef CONSTRAINT_SOLVERS_GRASP_BLOCK_DEMO_H +#define CONSTRAINT_SOLVERS_GRASP_BLOCK_DEMO_H + +class CommonExampleInterface* Grasp_BlockCreateFunc(struct CommonExampleOptions& options); + +#endif // CONSTRAINT_SOLVERS_GRASP_BLOCK_DEMO_H diff --git a/examples/MultiBody/SerialChains.cpp b/examples/ConstraintSolvers/SerialChains.cpp similarity index 88% rename from examples/MultiBody/SerialChains.cpp rename to examples/ConstraintSolvers/SerialChains.cpp index b582f0f0f..16ff358a5 100644 --- a/examples/MultiBody/SerialChains.cpp +++ b/examples/ConstraintSolvers/SerialChains.cpp @@ -7,6 +7,7 @@ #include "BulletDynamics/Featherstone/btMultiBody.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" @@ -112,12 +113,13 @@ void SerialChains::initPhysics() b3Printf("Constraint Solver: MLCP + Dantzig"); break; } + m_solver = new btMultiBodyBlockConstraintSolver(); btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); m_dynamicsWorld = world; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); - m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good? + m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good? ///create a few basic rigid bodies btVector3 groundHalfExtents(50, 50, 50); @@ -237,6 +239,53 @@ void SerialChains::initPhysics() createGround(); + { + btVector3 halfExtents(.5,.5,.5); + btBoxShape* colShape = new btBoxShape(halfExtents); + //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); + m_collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + btScalar mass(1.f); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0,0,0); + if (isDynamic) + colShape->calculateLocalInertia(mass,localInertia); + + startTransform.setOrigin(btVector3( + btScalar(0.0), + 0.0, + btScalar(0.0))); + + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia); +// btRigidBody* body = new btRigidBody(rbInfo); + +// m_dynamicsWorld->addRigidBody(body);//,1,1+2); + + { + btVector3 pointInA = -linkHalfExtents; + // btVector3 pointInB = halfExtents; + btMatrix3x3 frameInA; + btMatrix3x3 frameInB; + frameInA.setIdentity(); + frameInB.setIdentity(); + btVector3 jointAxis(1.0,0.0,0.0); + //btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis); + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(mbC1, numLinks- 1 , mbC2, numLinks - 1, pointInA, pointInA); + p2p->setMaxAppliedImpulse(20.0); + m_dynamicsWorld->addMultiBodyConstraint(p2p); + } + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); ///////////////////////////////////////////////////////////////// diff --git a/examples/MultiBody/SerialChains.h b/examples/ConstraintSolvers/SerialChains.h similarity index 100% rename from examples/MultiBody/SerialChains.h rename to examples/ConstraintSolvers/SerialChains.h diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index ba6a3fbd7..cd87eb92a 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -202,6 +202,10 @@ SET(BulletExampleBrowser_SRCS ../MultiThreadedDemo/MultiThreadedDemo.h ../MultiThreadedDemo/CommonRigidBodyMTBase.cpp ../MultiThreadedDemo/CommonRigidBodyMTBase.h + ../ConstraintSolvers/BoxStacks.cpp + ../ConstraintSolvers/BoxStacks_MLCP.cpp + ../ConstraintSolvers/Grasp_Block.cpp + ../ConstraintSolvers/SerialChains.cpp ../BlockSolver/btBlockSolver.cpp ../BlockSolver/btBlockSolver.h ../BlockSolver/BlockSolverExample.cpp @@ -337,7 +341,6 @@ SET(BulletExampleBrowser_SRCS ../MultiBody/InvertedPendulumPDControl.cpp ../MultiBody/InvertedPendulumPDControl.h ../MultiBody/MultiBodyConstraintFeedback.cpp - ../MultiBody/SerialChains.cpp ../MultiBody/MultiDofDemo.cpp ../MultiBody/MultiDofDemo.h ../RigidBody/RigidBodySoftContact.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index cf8ad2f6b..c055cbeea 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -21,6 +21,10 @@ #include "../Importers/ImportSDFDemo/ImportSDFSetup.h" #include "../Importers/ImportMJCFDemo/ImportMJCFSetup.h" #include "../Collision/CollisionTutorialBullet2.h" +#include "../ConstraintSolvers/SerialChains.h" +#include "../ConstraintSolvers/BoxStacks.h" +#include "../ConstraintSolvers/BoxStacks_MLCP.h" +#include "../ConstraintSolvers/Grasp_Block.h" #include "../GyroscopicDemo/GyroscopicSetup.h" #include "../Constraints/Dof6Spring2Setup.h" #include "../Constraints/ConstraintPhysicsSetup.h" @@ -30,7 +34,7 @@ #include "../MultiBody/MultiBodyConstraintFeedback.h" #include "../MultiBody/MultiDofDemo.h" #include "../MultiBody/InvertedPendulumPDControl.h" -#include "../MultiBody/SerialChains.h" + #include "../RigidBody/RigidBodySoftContact.h" #include "../VoronoiFracture/VoronoiFractureDemo.h" #include "../SoftDemo/SoftDemo.h" @@ -137,7 +141,13 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc), ExampleEntry(1, "Inverted Pendulum PD", "Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc), ExampleEntry(1, "MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", MultiBodySoftContactCreateFunc, 0), + + ExampleEntry(0, "Constraint Solvers"), ExampleEntry(1, "Serial Chains", "Show colliding two serial chains using different constraint solvers.", SerialChainsCreateFunc, 0), + ExampleEntry(1, "Box Stack", "Show box stacks with different constraint solvers for each stack.", BoxStacksCreateFunc, 0), + ExampleEntry(1, "Box Stack MLCP", "Show box stacks with different constraint solvers for each stack.", BoxStacks_MLCPCreateFunc, 0), + ExampleEntry(1, "Grasp Block", "Show box stacks with different constraint solvers for each stack.", Grasp_BlockCreateFunc, 0), + ExampleEntry(0, "Physics Client-Server"), ExampleEntry(1, "Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.", diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 222ad911e..c610632d1 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -163,6 +163,7 @@ project "App_BulletExampleBrowser" "../Collision/*", "../RoboticsLearning/*", "../BlockSolver/*", + "../ConstraintSolvers/*", "../Collision/Internal/*", "../Benchmarks/*", "../MultiThreadedDemo/*", diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index 3332440f2..68459578c 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -32,6 +32,7 @@ SET(BulletDynamics_SRCS Vehicle/btWheelInfo.cpp Featherstone/btMultiBody.cpp Featherstone/btMultiBodyConstraint.cpp + Featherstone/btMultiBodyBlockConstraintSolver.cpp Featherstone/btMultiBodyConstraintSolver.cpp Featherstone/btMultiBodyDynamicsWorld.cpp Featherstone/btMultiBodyFixedConstraint.cpp diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index e7a237770..5c76918eb 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -931,10 +931,32 @@ int btSISolverSingleIterationData::getSolverBody(btCollisionObject& body) const return solverBodyId; #else // BT_THREADSAFE int solverBodyIdA = -1; - btAssert(body.getCompanionId() >= 0); - //body has already been converted - solverBodyIdA = body.getCompanionId(); - btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); + + if (body.getCompanionId() >= 0) + { + //body has already been converted + solverBodyIdA = body.getCompanionId(); + btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); + } + else + { + btRigidBody* rb = btRigidBody::upcast(&body); + //convert both active and kinematic objects (for their velocity) + if (rb && (rb->getInvMass() || rb->isKinematicObject())) + { + btAssert(0); + } + else + { + if (m_fixedBodyId < 0) + { + btAssert(0); + } + return m_fixedBodyId; + // return 0;//assume first one is a fixed solver body + } + } + return solverBodyIdA; #endif // BT_THREADSAFE } @@ -1822,7 +1844,16 @@ void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodi convertBodiesInternal(siData, bodies, numBodies, infoGlobal); } + btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +{ + solveGroupConvertConstraintPrestep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + solveGroupConvertConstraints(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + solveGroupConvertConstraintPoststep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + return 0.; +} + +btScalar btSequentialImpulseConstraintSolver::solveGroupConvertConstraints(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { m_fixedBodyId = -1; BT_PROFILE("solveGroupCacheFriendlySetup"); @@ -1912,6 +1943,17 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol convertContacts(manifoldPtr, numManifolds, infoGlobal); + + return 0.f; +} + +btScalar btSequentialImpulseConstraintSolver::solveGroupConvertConstraintPrestep(btCollisionObject** /*bodies*/, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** /*constraints*/, int /*numConstraints*/, const btContactSolverInfo& /*infoGlobal*/, btIDebugDraw* /*debugDrawer*/) +{ + return 0; +} + +btScalar btSequentialImpulseConstraintSolver::solveGroupConvertConstraintPoststep(btCollisionObject** /*bodies*/, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** /*constraints*/, int /*numConstraints*/, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/) +{ // btContactSolverInfo info = infoGlobal; int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); @@ -1942,9 +1984,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } - return 0.f; + return 0; } - btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) { BT_PROFILE("solveSingleIteration"); @@ -2372,3 +2413,33 @@ void btSequentialImpulseConstraintSolver::reset() { m_btSeed2 = 0; } + +btScalar btSequentialImpulseConstraintSolver::solveGroupConvertBackPrestep(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + return btScalar(0); +} + +btScalar btSequentialImpulseConstraintSolver::solveGroupConvertBack(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal); + } + + writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal); + writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal); + + return btScalar(0); +} + +btScalar btSequentialImpulseConstraintSolver::solveGroupConvertBackPoststep(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + m_tmpSolverContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); + + m_tmpSolverBodyPool.resizeNoInitialize(0); + + return btScalar(0); +} diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index f9dc59a40..6eacf043b 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -185,7 +185,7 @@ protected: return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint); } -protected: +public: void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); @@ -296,6 +296,14 @@ public: static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric(); static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric(); + virtual btScalar solveGroupConvertConstraintPrestep(btCollisionObject** /*bodies*/, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** /*constraints*/, int /*numConstraints*/, const btContactSolverInfo& /*infoGlobal*/, btIDebugDraw* /*debugDrawer*/); + virtual btScalar solveGroupConvertConstraintPoststep(btCollisionObject** /*bodies*/, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** /*constraints*/, int /*numConstraints*/, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/); + virtual btScalar solveGroupConvertConstraints(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + + virtual btScalar solveGroupConvertBackPrestep(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); + virtual btScalar solveGroupConvertBack(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); + virtual btScalar solveGroupConvertBackPoststep(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); + }; #endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.cpp new file mode 100644 index 000000000..bca5048cb --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.cpp @@ -0,0 +1,760 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 Google Inc. http://bulletphysics.org + +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 "btMultiBodyBlockConstraintSolver.h" + +#include + +#include "LinearMath/btQuickprof.h" +#include "btMultiBodyMLCPConstraintSolver.h" +#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" + +static void rigidBodyNotSupported() +{ + printf("Attempts to use rigid body for block solver, which is not supported yet.\n"); + btAssert(false); +} + +btMultiBodyConstraintBlock::btMultiBodyConstraintBlock() + : m_constraintConfigId(-1) +{ + // Do nothing +} + +btMultiBodyConstraintBlock::btMultiBodyConstraintBlock( + btTypedConstraint** m_constraints, + int m_numConstraints, + btAlignedObjectArray* m_solverBodyPool, + btConstraintArray& m_nonContactConstraints, + btConstraintArray& m_normalContactConstraints, + btConstraintArray& m_frictionContactConstraints, + btConstraintArray& m_rollingFrictionContactConstraints, + btMultiBodyConstraint** m_multiBodyConstraints, + int m_numMultiBodyConstraints, + btAlignedObjectArray& m_multiBodyNonContactConstraints, + btAlignedObjectArray& m_multiBodyNormalContactConstraints, + btAlignedObjectArray& m_multiBodyFrictionContactConstraints, + btAlignedObjectArray& m_multiBodyTorsionalFrictionContactConstraints, + btMultiBodyJacobianData* m_data) + : m_constraintConfigId(-1) +{ + // Do nothing +} + +static void copyConstraintDynamicDataToBlock(btAlignedObjectArray& originalConstraints, const btAlignedObjectArray& blockConstraints) +{ + btAssert(originalConstraints.size() == blockConstraints.size()); + for (int i = 0; i < blockConstraints.size(); ++i) + { + btMultiBodySolverConstraint& originalConstraint = *originalConstraints[i]; + const btMultiBodySolverConstraint& blockConstraint = blockConstraints[i]; + + blockConstraint.m_appliedImpulse = originalConstraint.m_appliedImpulse; + blockConstraint.m_appliedPushImpulse = originalConstraint.m_appliedPushImpulse; + } +} + +void debugPrint(const btScalar* data, int size) +{ + for (int i = 0; i < size; ++i) + { + printf("\t%.5f", data[i]); + if (i != size - 1) + printf(","); + } + printf("\n"); +} + +void debugPrintDiff(const btScalar* dataA, const btScalar* dataB, int size, bool ignoreZero = false) +{ + for (int i = 0; i < size; ++i) + { + if (ignoreZero) + { + if (btFabs(dataA[i] - dataB[i]) < 1e-9) + continue; + } + printf("\t%f", dataA[i] - dataB[i]); + if (i != size - 1) + printf(","); + } + printf("\n"); +} + +void btMultiBodyConstraintBlock::copyDynamicDataFromOriginalToBlock() +{ + copyConstraintDynamicDataToBlock(m_originalMultiBodyNormalContactConstraintPtrs, m_internalData.m_multiBodyNormalContactConstraints); + copyConstraintDynamicDataToBlock(m_originalMultiBodyFrictionContactConstraintPtrs, m_internalData.m_multiBodyFrictionContactConstraints); + copyConstraintDynamicDataToBlock(m_originalMultiBodyTorsionalFrictionContactConstraintPtrs, m_internalData.m_multiBodyTorsionalFrictionContactConstraints); + + btAssert(m_multiBodies.size() == m_originalDeltaVelIndices.size()); + btAssert(m_multiBodies.size() == m_deltaVelIndices.size()); + for (int i = 0; i < m_multiBodies.size(); ++i) + { + btMultiBody* multiBody = m_multiBodies[i]; + const int ndof = multiBody->getNumDofs() + 6; + + btMultiBodyJacobianData& originalData = m_internalData.m_data; // TODO(JS): WRONG !! + btAlignedObjectArray& originaDeltaVelocities = originalData.m_deltaVelocities; + + btAlignedObjectArray& blockDeltaVelocities = m_internalData.m_data.m_deltaVelocities; + + const int originalIndex = m_originalDeltaVelIndices[i]; + const int blockIndex = m_deltaVelIndices[i]; + + const btScalar* originalDeltaVelocitiesPtr = &originaDeltaVelocities[originalIndex]; + btScalar* blockDeltaVelocitiesPtr = &blockDeltaVelocities[blockIndex]; + + // printf("[ original --> block ]\n"); + // printf("original: "); + // debugPrint(originalDeltaVelocitiesPtr, ndof); + // printf("block: "); + // debugPrint(blockDeltaVelocitiesPtr, ndof); + // printf("diff: "); + // debugPrintDiff(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof, true); + // printf("\n"); + + memcpy(blockDeltaVelocitiesPtr, originalDeltaVelocitiesPtr, ndof * sizeof(btScalar)); + } +} + +static void copyConstraintDynamicDataFromToOriginal(btAlignedObjectArray& originalConstraints, const btAlignedObjectArray& blockConstraints) +{ + btAssert(originalConstraints.size() == blockConstraints.size()); + for (int i = 0; i < blockConstraints.size(); ++i) + { + btMultiBodySolverConstraint& originalConstraint = *originalConstraints[i]; + const btMultiBodySolverConstraint& blockConstraint = blockConstraints[i]; + + originalConstraint.m_appliedImpulse = blockConstraint.m_appliedImpulse; + originalConstraint.m_appliedPushImpulse = blockConstraint.m_appliedPushImpulse; + } +} + +void btMultiBodyConstraintBlock::copyDynamicDataFromBlockToOriginal() +{ + copyConstraintDynamicDataFromToOriginal(m_originalMultiBodyNormalContactConstraintPtrs, m_internalData.m_multiBodyNormalContactConstraints); + copyConstraintDynamicDataFromToOriginal(m_originalMultiBodyFrictionContactConstraintPtrs, m_internalData.m_multiBodyFrictionContactConstraints); + copyConstraintDynamicDataFromToOriginal(m_originalMultiBodyTorsionalFrictionContactConstraintPtrs, m_internalData.m_multiBodyTorsionalFrictionContactConstraints); + + btAssert(m_multiBodies.size() == m_originalDeltaVelIndices.size()); + btAssert(m_multiBodies.size() == m_deltaVelIndices.size()); + for (int i = 0; i < m_multiBodies.size(); ++i) + { + btMultiBody* multiBody = m_multiBodies[i]; + const int ndof = multiBody->getNumDofs() + 6; + + btMultiBodyJacobianData& originalData = m_internalData.m_data; + btAlignedObjectArray& originaDeltaVelocities = originalData.m_deltaVelocities; + + btAlignedObjectArray& blockDeltaVelocities = m_internalData.m_data.m_deltaVelocities; + + const int originalIndex = m_originalDeltaVelIndices[i]; + const int blockIndex = m_deltaVelIndices[i]; + + btScalar* originalDeltaVelocitiesPtr = &originaDeltaVelocities[originalIndex]; + const btScalar* blockDeltaVelocitiesPtr = &blockDeltaVelocities[blockIndex]; + + // printf("[ block --> original ]\n"); + // printf("original: "); + // debugPrint(originalDeltaVelocitiesPtr, ndof); + // printf("block: "); + // debugPrint(blockDeltaVelocitiesPtr, ndof); + // printf("diff: "); + // debugPrintDiff(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof, true); + // printf("\n"); + + memcpy(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof * sizeof(btScalar)); + } +} + +btSingleBlockSplittingPolicy::btSingleBlockSplittingPolicy(btMultiBodyConstraintSolver* solver) + : m_solver(solver) +{ + // Do nothing +} + +btSingleBlockSplittingPolicy::~btSingleBlockSplittingPolicy() +{ + // Do nothing +} + +void btSingleBlockSplittingPolicy::split(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInput, const btAlignedObjectArray& availableConfigs, btAlignedObjectArray& blocksOutput) +{ + btMultiBodyConstraintBlock newBlock; + // newBlock.m_originalInternalDataBlock = blockInput; + // m_solver->setMultiBodyInternalConstraintData(newBlock.m_originalInternalDataBlock); + newBlock.m_solver = m_solver; + // newBlock.m_constraints = blockInput.m_constraints; + // newBlock.m_numConstraints = blockInput.m_numConstraints; + // newBlock.m_multiBodyConstraints = blockInput.m_multiBodyConstraints; + // newBlock.m_numMultiBodyConstraints = blockInput.m_numMultiBodyConstraints; + + blocksOutput.push_back(newBlock); +} + +btDoubleBlockSplittingPolicy::btDoubleBlockSplittingPolicy(btMultiBodyConstraintSolver* solver) + : m_solver(solver) +{ + // Do nothing +} + +btDoubleBlockSplittingPolicy::~btDoubleBlockSplittingPolicy() +{ + // Do nothing +} + +template +void splitContactConstraints(const ArrayT& input, ArrayT& output1, ArrayT& output2) +{ + const int totalSize = input.size(); + const int halfSize = totalSize / 2; + + output1.resize(halfSize); + output2.resize(totalSize - halfSize); + + for (int i = 0; i < halfSize; ++i) + { + output1[i] = input[i]; + } + + for (int i = halfSize; i < totalSize; ++i) + { + output2[i - halfSize] = input[i]; + } +} + +btMultiBodyConstraintBlock initializeConstraintBlock(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& input) +{ + btMultiBodyConstraintBlock output; + + // MultiBody + + output.m_internalData.m_multiBodyConstraints = input.m_multiBodyConstraints; + output.m_internalData.m_numMultiBodyConstraints = input.m_numMultiBodyConstraints; + //output.m_multiBodyConstraintSet.m_data = input.m_multiBodyConstraintSet.m_data; + + btAssert(output.m_internalData.m_multiBodyNormalContactConstraints.size() == 0); + btAssert(output.m_internalData.m_multiBodyFrictionContactConstraints.size() == 0); + + return output; +} + +static void setupBlockMultiBodyJacobianData( + btMultiBody* multiBody, + btAlignedObjectArray& multiBodySet, + btAlignedObjectArray& blockDeltaVelIndices, + int& blockDeltaVelIndex, + int& blockJacobianIndex, + btMultiBodyJacobianData& blockJacobianData, + btAlignedObjectArray& originalDeltaVelIndices, + const int originalDeltaVelIndex, + const int originalJacobianIndex, + const btMultiBodyJacobianData& originalJacobianData) +{ + const int ndof = multiBody->getNumDofs() + 6; + + btAlignedObjectArray& blockJacobians = blockJacobianData.m_jacobians; + btAlignedObjectArray& blockDeltaVelocities = blockJacobianData.m_deltaVelocities; + btAlignedObjectArray& blockDeltaVelocitiesUnitImpulse = blockJacobianData.m_deltaVelocitiesUnitImpulse; + + const btAlignedObjectArray& originalJacobians = originalJacobianData.m_jacobians; + const btAlignedObjectArray& originalDeltaVelocitiesUnitImpulse = originalJacobianData.m_deltaVelocitiesUnitImpulse; + + int indexInBlock = -1; + for (int i = 0; i < multiBodySet.size(); ++i) + { + if (multiBody == multiBodySet[i]) + { + indexInBlock = i; + break; + } + } + + if (indexInBlock == -1) + { + blockDeltaVelIndex = blockDeltaVelocities.size(); + blockDeltaVelocities.resize(blockDeltaVelocities.size() + ndof); + multiBodySet.push_back(multiBody); + originalDeltaVelIndices.push_back(originalDeltaVelIndex); + blockDeltaVelIndices.push_back(blockDeltaVelIndex); + } + else + { + blockDeltaVelIndex = blockDeltaVelIndices[indexInBlock]; + } + + blockJacobianIndex = blockJacobians.size(); + blockJacobians.resize(blockJacobians.size() + ndof); + blockDeltaVelocitiesUnitImpulse.resize(blockDeltaVelocitiesUnitImpulse.size() + ndof); + btAssert(blockJacobians.size() == blockDeltaVelocitiesUnitImpulse.size()); + + btScalar* blockJacobiansRawPtr = &blockJacobians[blockJacobianIndex]; + const btScalar* originalJacobiansRawPtr = &originalJacobians[originalJacobianIndex]; + memcpy(blockJacobiansRawPtr, originalJacobiansRawPtr, ndof * sizeof(btScalar)); + + btScalar* blockDeltaVelUnitImp = &blockDeltaVelocitiesUnitImpulse[blockJacobianIndex]; + const btScalar* originalDeltaVelUnitImp = &originalDeltaVelocitiesUnitImpulse[originalJacobianIndex]; + memcpy(blockDeltaVelUnitImp, originalDeltaVelUnitImp, ndof * sizeof(btScalar)); + + btAssert(blockJacobians.size() >= blockDeltaVelocities.size()); +} + +static void setupMultiBodyBlockConstraintData( + btMultiBodyConstraintBlock& block, + btAlignedObjectArray& blockMultiBodySet, + btAlignedObjectArray& blockDeltaVelIndices, + btMultiBodySolverConstraint& blockContactConstraint, + btMultiBodyJacobianData& blockJacobianData, + int blockFrictionIndex, + btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, + btAlignedObjectArray& originalDeltaVelIndices, + const btMultiBodySolverConstraint& originalContactConstraint, + const btMultiBodyJacobianData& originalJacobianData) +{ + // Copy all the values. Some values will be updated below if necessary. + blockContactConstraint = originalContactConstraint; + blockContactConstraint.m_frictionIndex = blockFrictionIndex; + + btMultiBody* multiBodyA = blockContactConstraint.m_multiBodyA; + if (multiBodyA) + { + setupBlockMultiBodyJacobianData( + multiBodyA, + blockMultiBodySet, + blockDeltaVelIndices, + blockContactConstraint.m_deltaVelAindex, + blockContactConstraint.m_jacAindex, + blockJacobianData, + originalDeltaVelIndices, + originalContactConstraint.m_deltaVelAindex, + originalContactConstraint.m_jacAindex, + originalJacobianData); + } + else + { + rigidBodyNotSupported(); + } + + btMultiBody* multiBodyB = blockContactConstraint.m_multiBodyB; + if (multiBodyB) + { + setupBlockMultiBodyJacobianData( + multiBodyB, + blockMultiBodySet, + blockDeltaVelIndices, + blockContactConstraint.m_deltaVelBindex, + blockContactConstraint.m_jacBindex, + blockJacobianData, + originalDeltaVelIndices, + originalContactConstraint.m_deltaVelBindex, + originalContactConstraint.m_jacBindex, + originalJacobianData); + } + else + { + rigidBodyNotSupported(); + } +} + +void btDoubleBlockSplittingPolicy::split( + btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, + const btAlignedObjectArray& availableConfigs, + btAlignedObjectArray& subBlocks) +{ + btMultiBodyConstraintBlock constraintBlock1 = initializeConstraintBlock(originalInternalData); + btMultiBodyConstraintBlock constraintBlock2 = initializeConstraintBlock(originalInternalData); + btMultiBodyConstraintBlock constraintBlock3 = initializeConstraintBlock(originalInternalData); + + constraintBlock1.m_solver = m_solver; + constraintBlock2.m_solver = m_solver; + constraintBlock3.m_solver = m_solver; + + btDantzigSolver* mlcp = new btDantzigSolver(); + btMultiBodyMLCPConstraintSolver* sol = new btMultiBodyMLCPConstraintSolver(mlcp); +// constraintBlock2.m_solver = sol; + + const int totalMultiBodyContactConstraintSize = originalInternalData.m_multiBodyNormalContactConstraints.size(); + const int halfMultiBodyContactConstraintSize = totalMultiBodyContactConstraintSize / 2; + + for (int i = 0; i < halfMultiBodyContactConstraintSize; ++i) + { + copyMultiBodyContactConstraint(constraintBlock1, originalInternalData, i); + } + + for (int i = halfMultiBodyContactConstraintSize; i < totalMultiBodyContactConstraintSize; ++i) + { + copyMultiBodyContactConstraint(constraintBlock2, originalInternalData, i); + } + + const int totalMultiBodyNonContactConstraintSize = originalInternalData.m_multiBodyNonContactConstraints.size(); + + for (int i = 0; i < totalMultiBodyNonContactConstraintSize; ++i) + { +// copyMultiBodyNonContactConstraint(constraintBlock3, originalInternalData, i); + } + + +// for (int i = 0; i < totalMultiBodyContactConstraintSize; ++i) +// { +// if (strcmp(originalInternalData.m_multiBodyNormalContactConstraints[i].m_multiBodyA->getBaseName(), "group1") == 0) +// copyMultiBodyContactConstraint(constraintBlock1, originalInternalData, i); +// else +// copyMultiBodyContactConstraint(constraintBlock2, originalInternalData, i); +// } + + subBlocks.push_back(constraintBlock1); + subBlocks.push_back(constraintBlock2); + subBlocks.push_back(constraintBlock3); +} + +btMultiBodyBlockSplittingPolicy::~btMultiBodyBlockSplittingPolicy() +{ + // Do nothing +} + +void btMultiBodyBlockSplittingPolicy::copyMultiBodyNonContactConstraint(btMultiBodyConstraintBlock& block, btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, int originalNonContactConstraintIndex) +{ + btAlignedObjectArray& originalNonContactConstraints = originalInternalData.m_multiBodyNonContactConstraints; + const btMultiBodyJacobianData& originalJacobianData = originalInternalData.m_data; + + btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInternalData = block.m_internalData; + btAlignedObjectArray& blockNonContactConstraints = blockInternalData.m_multiBodyNonContactConstraints; + + btAlignedObjectArray& blockOriginalNonContactConstraintPtrs = block.m_originalMultiBodyNonContactConstraintPtrs; + + btMultiBodyJacobianData& blockJacobianData = block.m_internalData.m_data; + + btAlignedObjectArray& blockMultiBodySet = block.m_multiBodies; + + const int blockFrictionIndex = blockNonContactConstraints.size(); + + btMultiBodySolverConstraint& originalNonContactConstraint = originalNonContactConstraints[originalNonContactConstraintIndex]; + + btMultiBodySolverConstraint& blockNonContactConstraint = blockNonContactConstraints.expandNonInitializing(); + blockOriginalNonContactConstraintPtrs.push_back(&originalNonContactConstraint); + + setupMultiBodyBlockConstraintData( + block, + blockMultiBodySet, + block.m_deltaVelIndices, + blockNonContactConstraint, + blockJacobianData, + blockFrictionIndex, + originalInternalData, + block.m_originalDeltaVelIndices, + originalNonContactConstraint, + originalJacobianData); +} + +void btMultiBodyBlockSplittingPolicy::copyMultiBodyContactConstraint(btMultiBodyConstraintBlock& block, btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, int originalNormalContactConstraintIndex) +{ + btAlignedObjectArray& originalNormalContactConstraints = originalInternalData.m_multiBodyNormalContactConstraints; + btAlignedObjectArray& originalFrictionContactConstraints = originalInternalData.m_multiBodyFrictionContactConstraints; + btAlignedObjectArray& originalTortionalFrictionContactConstraints = originalInternalData.m_multiBodyTorsionalFrictionContactConstraints; + const btMultiBodyJacobianData& originalJacobianData = originalInternalData.m_data; + + btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInternalData = block.m_internalData; + btAlignedObjectArray& blockNormalContactConstraints = blockInternalData.m_multiBodyNormalContactConstraints; + btAlignedObjectArray& blockFrictionContactConstraints = blockInternalData.m_multiBodyFrictionContactConstraints; + btAlignedObjectArray& blockTortionalFrictionContactConstraints = blockInternalData.m_multiBodyTorsionalFrictionContactConstraints; + + btAlignedObjectArray& blockOriginalNormalContactConstraintPtrs = block.m_originalMultiBodyNormalContactConstraintPtrs; + btAlignedObjectArray& blockOriginalFrictionContactConstraintPtrs = block.m_originalMultiBodyFrictionContactConstraintPtrs; + btAlignedObjectArray& blockOriginalTorsionalFrictionContactConstraintPtrs = block.m_originalMultiBodyTorsionalFrictionContactConstraintPtrs; + + btMultiBodyJacobianData& blockJacobianData = block.m_internalData.m_data; + + const int numFrictionPerContact = originalNormalContactConstraints.size() == originalFrictionContactConstraints.size() ? 1 : 2; + + btAlignedObjectArray& blockMultiBodySet = block.m_multiBodies; + + const int blockFrictionIndex = blockNormalContactConstraints.size(); + + //-- 1. Normal contact + + btMultiBodySolverConstraint& originalNormalContactConstraint = originalNormalContactConstraints[originalNormalContactConstraintIndex]; + + btMultiBodySolverConstraint& blockNormalContactConstraint = blockNormalContactConstraints.expandNonInitializing(); + blockOriginalNormalContactConstraintPtrs.push_back(&originalNormalContactConstraint); + + setupMultiBodyBlockConstraintData( + block, + blockMultiBodySet, + block.m_deltaVelIndices, + blockNormalContactConstraint, + blockJacobianData, + blockFrictionIndex, + originalInternalData, + block.m_originalDeltaVelIndices, + originalNormalContactConstraint, + originalJacobianData); + + //-- 2. Friction contacts + + btAssert(originalFrictionContactConstraints.size() != 0); + const int originalFrictionContactConstraintIndex1 = originalNormalContactConstraintIndex * numFrictionPerContact; + btMultiBodySolverConstraint& originalFrictionContactConstraint = originalFrictionContactConstraints[originalFrictionContactConstraintIndex1]; + + blockOriginalFrictionContactConstraintPtrs.push_back(&originalFrictionContactConstraint); + btMultiBodySolverConstraint& blockFrictionContactConstraint1 = blockFrictionContactConstraints.expandNonInitializing(); + setupMultiBodyBlockConstraintData( + block, + blockMultiBodySet, + block.m_deltaVelIndices, + blockFrictionContactConstraint1, + blockJacobianData, + blockFrictionIndex, + originalInternalData, + block.m_originalDeltaVelIndices, + originalFrictionContactConstraint, + originalJacobianData); + + if (numFrictionPerContact == 2) + { + const int originalFrictionContactConstraintIndex2 = originalFrictionContactConstraintIndex1 + 1; + btMultiBodySolverConstraint& originalFrictionContactConstraint = originalFrictionContactConstraints[originalFrictionContactConstraintIndex2]; + + blockOriginalFrictionContactConstraintPtrs.push_back(&originalFrictionContactConstraint); + btMultiBodySolverConstraint& blockFrictionContactConstraint2 = blockFrictionContactConstraints.expandNonInitializing(); + setupMultiBodyBlockConstraintData( + block, + blockMultiBodySet, + block.m_deltaVelIndices, + blockFrictionContactConstraint2, + blockJacobianData, + blockFrictionIndex, + originalInternalData, + block.m_originalDeltaVelIndices, + originalFrictionContactConstraint, + originalJacobianData); + } + + // TODO(JS): Torsional friction contact constraints +} + +btMultiBodyBlockConstraintSolver::btMultiBodyBlockConstraintSolver() +{ + // Do nothing +} + +btMultiBodyBlockConstraintSolver::~btMultiBodyBlockConstraintSolver() +{ + // Do nothing +} + +btScalar btMultiBodyBlockConstraintSolver::solveGroupConvertConstraintPoststep(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +{ + return btMultiBodyConstraintSolver::solveGroupConvertConstraintPoststep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); +} + +void btMultiBodyBlockConstraintSolver::solveMultiBodyGroup( + btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifold, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + btMultiBodyConstraint** multiBodyConstraints, + int numMultiBodyConstraints, + const btContactSolverInfo& info, + btIDebugDraw* debugDrawer, + btDispatcher* /*dispatcher*/) +{ + m_tmpMultiBodyConstraints = multiBodyConstraints; + m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; + + // 1. Convert rigid bodies/multibodies, joints, contacts into constraints. + solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + + // 2. Split constraints into constraint blocks + btMultiBodyInternalConstraintData originalInternalDataCopy; + getMultiBodyInternalConstraintData(originalInternalDataCopy); + + btAlignedObjectArray configs; + // TODO(JS): This is just for test + //m_splittingPolicy = new btSingleBlockSplittingPolicy(new btMultiBodyConstraintSolver()); + +// btDantzigSolver* mlcp = new btDantzigSolver(); +// btMultiBodyMLCPConstraintSolver* sol = new btMultiBodyMLCPConstraintSolver(mlcp); +// m_splittingPolicy = new btDoubleBlockSplittingPolicy(sol); + + m_splittingPolicy = new btDoubleBlockSplittingPolicy(new btMultiBodyConstraintSolver()); + + btAssert(m_splittingPolicy); + m_blocks.resize(0); + m_splittingPolicy->split(originalInternalDataCopy, configs, m_blocks); + +// for (int i = 0; i < m_blocks.size(); ++i) +// { +// btMultiBodyConstraintBlock& block = m_blocks[i]; +// btMultiBodyConstraintSolver* solver = block.m_solver; +// btAssert(solver); + +// solver->solveGroupConvertConstraintPrestep(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); +// copyDynamicDataFromOriginalToBlock(block); +//// block.copyDynamicDataFromOriginalToBlock(); +// solver->setMultiBodyInternalConstraintData(block.m_internalData, false); +// solver->solveGroupConvertConstraintPoststep(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); +// } + + // 3. Gauss-Seidel iterations + + const int maxIterations = m_maxOverrideNumSolverIterations > info.m_numIterations ? m_maxOverrideNumSolverIterations : info.m_numIterations; + + m_leastSquaresResidual = 0; + + for (int iteration = 0; iteration < maxIterations; ++iteration) + { + for (int i = 0; i < m_blocks.size(); ++i) + { + // Change the sweep direction every iteration + const int index = iteration & 1 ? m_blocks.size() - 1 - i : i; + + btMultiBodyConstraintBlock& block = m_blocks[index]; + btMultiBodyConstraintSolver* solver = block.m_solver; + btAssert(solver); + + solver->solveGroupConvertConstraintPrestep(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + copyDynamicDataFromOriginalToBlock(block); + solver->setMultiBodyInternalConstraintData(block.m_internalData, false); + solver->solveGroupConvertConstraintPoststep(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + + // TODO(JS): Add split impulse + btScalar newSquaredResidual = solver->solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + m_leastSquaresResidual = btMax(m_leastSquaresResidual, newSquaredResidual); + + solver->solveGroupConvertBackPrestep(bodies, numBodies, info); + solver->solveGroupConvertBack(bodies, numBodies, info); + solver->getMultiBodyInternalConstraintData(block.m_internalData, false); + copyDynamicDataFromBlockToOriginal(block); + solver->solveGroupConvertBackPoststep(bodies, numBodies, info); + } + + if (m_leastSquaresResidual <= info.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) + { +#ifdef VERBOSE_RESIDUAL_PRINTF + printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); +#endif + break; + } + } + +// solveGroupConvertBackPrestep(bodies, numBodies, info); +// solveGroupConvertBack(bodies, numBodies, info); +// getMultiBodyInternalConstraintData(block.m_internalData, false); +// copyDynamicDataFromBlockToOriginal(block); +// solveGroupConvertBackPoststep(bodies, numBodies, info); + + solveGroupCacheFriendlyFinish(bodies, numBodies, info); + + m_tmpMultiBodyConstraints = 0; + m_tmpNumMultiBodyConstraints = 0; +} + +void btMultiBodyBlockConstraintSolver::setSplittingPolicy(btMultiBodyBlockSplittingPolicy* policy) +{ + m_splittingPolicy = policy; +} + +void btMultiBodyBlockConstraintSolver::copyDynamicDataFromOriginalToBlock(btMultiBodyConstraintBlock& block) +{ + copyConstraintDynamicDataToBlock(block.m_originalMultiBodyNormalContactConstraintPtrs, block.m_internalData.m_multiBodyNormalContactConstraints); + copyConstraintDynamicDataToBlock(block.m_originalMultiBodyFrictionContactConstraintPtrs, block.m_internalData.m_multiBodyFrictionContactConstraints); + copyConstraintDynamicDataToBlock(block.m_originalMultiBodyTorsionalFrictionContactConstraintPtrs, block.m_internalData.m_multiBodyTorsionalFrictionContactConstraints); + + btAssert(block.m_multiBodies.size() == block.m_originalDeltaVelIndices.size()); + btAssert(block.m_multiBodies.size() == block.m_deltaVelIndices.size()); + for (int i = 0; i < block.m_multiBodies.size(); ++i) + { + btMultiBody* multiBody = block.m_multiBodies[i]; + const int ndof = multiBody->getNumDofs() + 6; + + btMultiBodyJacobianData& originalData = m_data; // TODO(JS): WRONG !! + btAlignedObjectArray& originaDeltaVelocities = originalData.m_deltaVelocities; + + btAlignedObjectArray& blockDeltaVelocities = block.m_internalData.m_data.m_deltaVelocities; + + const int originalIndex = block.m_originalDeltaVelIndices[i]; + const int blockIndex = block.m_deltaVelIndices[i]; + + const btScalar* originalDeltaVelocitiesPtr = &originaDeltaVelocities[originalIndex]; + btScalar* blockDeltaVelocitiesPtr = &blockDeltaVelocities[blockIndex]; + + // printf("[ original --> block ]\n"); + // printf("original: "); + // debugPrint(originalDeltaVelocitiesPtr, ndof); + // printf("block: "); + // debugPrint(blockDeltaVelocitiesPtr, ndof); + // printf("diff: "); + // debugPrintDiff(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof, true); + // printf("\n"); + + memcpy(blockDeltaVelocitiesPtr, originalDeltaVelocitiesPtr, ndof * sizeof(btScalar)); + } +} + +void btMultiBodyBlockConstraintSolver::copyDynamicDataFromBlockToOriginal(btMultiBodyConstraintBlock& block) +{ + copyConstraintDynamicDataFromToOriginal(block.m_originalMultiBodyNormalContactConstraintPtrs, block.m_internalData.m_multiBodyNormalContactConstraints); + copyConstraintDynamicDataFromToOriginal(block.m_originalMultiBodyFrictionContactConstraintPtrs, block.m_internalData.m_multiBodyFrictionContactConstraints); + copyConstraintDynamicDataFromToOriginal(block.m_originalMultiBodyTorsionalFrictionContactConstraintPtrs, block.m_internalData.m_multiBodyTorsionalFrictionContactConstraints); + + btAssert(block.m_multiBodies.size() == block.m_originalDeltaVelIndices.size()); + btAssert(block.m_multiBodies.size() == block.m_deltaVelIndices.size()); + for (int i = 0; i < block.m_multiBodies.size(); ++i) + { + btMultiBody* multiBody = block.m_multiBodies[i]; + const int ndof = multiBody->getNumDofs() + 6; + + btMultiBodyJacobianData& originalData = m_data; + btAlignedObjectArray& originaDeltaVelocities = originalData.m_deltaVelocities; + + btAlignedObjectArray& blockDeltaVelocities = block.m_internalData.m_data.m_deltaVelocities; + + const int originalIndex = block.m_originalDeltaVelIndices[i]; + const int blockIndex = block.m_deltaVelIndices[i]; + + btScalar* originalDeltaVelocitiesPtr = &originaDeltaVelocities[originalIndex]; + const btScalar* blockDeltaVelocitiesPtr = &blockDeltaVelocities[blockIndex]; + + // printf("[ block --> original ]\n"); + // printf("original: "); + // debugPrint(originalDeltaVelocitiesPtr, ndof); + // printf("block: "); + // debugPrint(blockDeltaVelocitiesPtr, ndof); + // printf("diff: "); + // debugPrintDiff(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof, true); + // printf("\n"); + + memcpy(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof * sizeof(btScalar)); + } +} + +int btMultiBodyBlockConstraintSolver::addConfig(btBlockConstraintSolverConfig& config) +{ + m_configs.push_back(config); + return m_configs.size(); +} + +int btMultiBodyBlockConstraintSolver::getNumConfigs() const +{ + return m_configs.size(); +} + +void btMultiBodyBlockConstraintSolver::removeConfig(int configIndex) +{ + m_configs.removeAtIndex(configIndex); +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h new file mode 100644 index 000000000..9fecd1fa5 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h @@ -0,0 +1,186 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 Google Inc. http://bulletphysics.org + +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 BT_MULTIBODY_BLOCK_CONSTRAINT_SOLVER_H +#define BT_MULTIBODY_BLOCK_CONSTRAINT_SOLVER_H + +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" + +struct btBlockConstraintSolverConfig +{ + int m_solverType; //SI or MLCP Dantzig + //to be decided: full or subset of + + btContactSolverInfo m_info; +}; + +struct btMultiBodyConstraintBlock +{ + /// \{ \name Multi-body Data + + btAlignedObjectArray m_multiBodies; + btAlignedObjectArray m_originalDeltaVelIndices; + btAlignedObjectArray m_deltaVelIndices; + +// btMultiBodyJacobianData* m_originalDataPtr; + + btAlignedObjectArray m_originalMultiBodyNonContactConstraintPtrs; + btAlignedObjectArray m_originalMultiBodyNormalContactConstraintPtrs; + btAlignedObjectArray m_originalMultiBodyFrictionContactConstraintPtrs; + btAlignedObjectArray m_originalMultiBodyTorsionalFrictionContactConstraintPtrs; + + /// \} + + btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData m_internalData; + + /// Constraint solver + btMultiBodyConstraintSolver* m_solver; + + bool m_ownSolver = false; + // TODO(JS): If this is true, then don't copy all the constraint data, but + // only dynamic data + // TODO(JS): not utilized yet + + /// Index to constraint solver configuration + int m_constraintConfigId; + + /// Default constructor + btMultiBodyConstraintBlock(); + + /// Constructor + btMultiBodyConstraintBlock( + btTypedConstraint** m_constraints, + int m_numConstraints, + btAlignedObjectArray* m_solverBodyPool, + btConstraintArray& m_originalNonContactConstraints, + btConstraintArray& m_originalNormalContactConstraints, + btConstraintArray& m_originalFrictionContactConstraints, + btConstraintArray& m_orginalRollingFrictionContactConstraints, + btMultiBodyConstraint** m_multiBodyConstraints, + int m_numMultiBodyConstraints, + btAlignedObjectArray& m_multiBodyNonContactConstraints, + btAlignedObjectArray& m_multiBodyNormalContactConstraints, + btAlignedObjectArray& m_multiBodyFrictionContactConstraints, + btAlignedObjectArray& m_multiBodyTorsionalFrictionContactConstraints, + btMultiBodyJacobianData* m_data); + + void copyDynamicDataFromOriginalToBlock(); + void copyDynamicDataFromBlockToOriginal(); +}; + +class btMultiBodyBlockSplittingPolicy +{ +public: + /// Destructor + virtual ~btMultiBodyBlockSplittingPolicy(); + + /// Splits a set of constraints into multiple subsets. + /// + /// \param[in] blockInput + /// \param[in] availableConfigs + /// \param[in,out] blocksOutput The splitted blocks. This function adds blocks without clearning the array + /// beforehand. Clearning the array is the caller's responsibility. + virtual void split(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInput, const btAlignedObjectArray& availableConfigs, btAlignedObjectArray& blocksOutput) = 0; + +protected: + void copyMultiBodyNonContactConstraint( + btMultiBodyConstraintBlock& block, + btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, + int originalNonContactConstraintIndex); + + void copyMultiBodyContactConstraint( + btMultiBodyConstraintBlock& block, + btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, + int originalNormalContactConstraintIndex); +}; + +class btSingleBlockSplittingPolicy : public btMultiBodyBlockSplittingPolicy +{ +protected: + btMultiBodyConstraintSolver* m_solver; + +public: + /// Constructor + btSingleBlockSplittingPolicy(btMultiBodyConstraintSolver* solver); + + /// Destructor + virtual ~btSingleBlockSplittingPolicy(); + + // Documentation inherited + virtual void split(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInput, const btAlignedObjectArray& availableConfigs, btAlignedObjectArray& blocksOutput); +}; + +class btDoubleBlockSplittingPolicy : public btMultiBodyBlockSplittingPolicy +{ +protected: + btMultiBodyConstraintSolver* m_solver; + +public: + /// Constructor + btDoubleBlockSplittingPolicy(btMultiBodyConstraintSolver* solver); + + /// Destructor + virtual ~btDoubleBlockSplittingPolicy(); + + // Documentation inherited + virtual void split(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInput, const btAlignedObjectArray& availableConfigs, btAlignedObjectArray& blocksOutput); +}; + +class btMultiBodyBlockConstraintSolver : public btMultiBodyConstraintSolver +{ +protected: + /// Splitting policy. Assumed not a null. + btMultiBodyBlockSplittingPolicy* m_splittingPolicy; + + /// Array of constraint configurations for constraint blocks. + btAlignedObjectArray m_configs; + + /// Array of constraint blocks. + btAlignedObjectArray m_blocks; + +public: + /// Constructor + btMultiBodyBlockConstraintSolver(); + + /// Destructor + virtual ~btMultiBodyBlockConstraintSolver(); + + virtual btScalar solveGroupConvertConstraintPoststep(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + + +protected: + // Documentation inherited. + virtual void solveMultiBodyGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + + /// Sets the splitting policy. + virtual void setSplittingPolicy(btMultiBodyBlockSplittingPolicy* policy); + + void copyDynamicDataFromOriginalToBlock(btMultiBodyConstraintBlock& block); + void copyDynamicDataFromBlockToOriginal(btMultiBodyConstraintBlock& block); + + /// Adds a constraint block configuration and returns the total number of configurations added to this solver. + virtual int addConfig(btBlockConstraintSolverConfig& config); + + /// Returns the number of configurations added to this solver. + virtual int getNumConfigs() const; + + /// Removes an configuration at \c configIndex + /// + /// \param[in] configIndex The configuration indext in the range of [0, numConfigs). Passing out of the range is an + /// undefined behavior. + virtual void removeConfig(int configIndex); +}; + +#endif // BT_MULTIBODY_BLOCK_CONSTRAINT_SOLVER_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index e97bd71cc..6300d7688 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -23,33 +23,33 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" -btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { - btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - + btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + //solve featherstone non-contact constraints //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); - for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++) + for (int j=0;jsetPosUpdated(false); - if (constraint.m_multiBodyB) + if(constraint.m_multiBodyB) constraint.m_multiBodyB->setPosUpdated(false); } //solve featherstone normal contact - for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++) + for (int j0=0;j0setPosUpdated(false); - if (constraint.m_multiBodyB) + if(constraint.m_multiBodyB) constraint.m_multiBodyB->setPosUpdated(false); } //solve featherstone frictional contact - if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)) + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)) { - for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++) + for (int j1 = 0; j1m_multiBodyTorsionalFrictionContactConstraints.size(); j1++) { if (iteration < infoGlobal.m_numIterations) { - int index = j1; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1; + int index = j1;//iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1; btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index]; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; //adjust friction limits here - if (totalImpulse > btScalar(0)) + if (totalImpulse>btScalar(0)) { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); - leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); + leastSquaredResidual = btMax(leastSquaredResidual , residual*residual); if (frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->setPosUpdated(false); @@ -98,29 +98,29 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl { if (iteration < infoGlobal.m_numIterations) { - int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; j1++; - int index2 = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + int index2 = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyFrictionContactConstraints[index2]; btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex); if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex) { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; - frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse); - frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse; + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction*totalImpulse); + frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction*totalImpulse; btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB); - leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); - + leastSquaredResidual = btMax(leastSquaredResidual, residual*residual); + if (frictionConstraintB.m_multiBodyA) frictionConstraintB.m_multiBodyA->setPosUpdated(false); if (frictionConstraintB.m_multiBodyB) frictionConstraintB.m_multiBodyB->setPosUpdated(false); - + if (frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->setPosUpdated(false); if (frictionConstraint.m_multiBodyB) @@ -131,21 +131,21 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl } else { - for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++) + for (int j1 = 0; j1m_multiBodyFrictionContactConstraints.size(); j1++) { if (iteration < infoGlobal.m_numIterations) { - int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; //adjust friction limits here - if (totalImpulse > btScalar(0)) + if (totalImpulse>btScalar(0)) { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); - leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); + leastSquaredResidual = btMax(leastSquaredResidual, residual*residual); if (frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->setPosUpdated(false); @@ -158,7 +158,17 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl return leastSquaredResidual; } -btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +btScalar btMultiBodyConstraintSolver::solveSingleIterationNew(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + return solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); +} + +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); +} + +btScalar btMultiBodyConstraintSolver::solveGroupConvertConstraintPrestep(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { m_multiBodyNonContactConstraints.resize(0); m_multiBodyNormalContactConstraints.resize(0); @@ -169,7 +179,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb m_data.m_deltaVelocitiesUnitImpulse.resize(0); m_data.m_deltaVelocities.resize(0); - for (int i = 0; i < numBodies; i++) + for (int i = 0; i < numBodies; ++i) { const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]); if (fcA) @@ -178,20 +188,26 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb } } - btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + btScalar val = btSequentialImpulseConstraintSolver::solveGroupConvertConstraintPrestep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); return val; } -void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +btScalar btMultiBodyConstraintSolver::solveGroupConvertConstraintPoststep(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { - for (int i = 0; i < ndof; ++i) - m_data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse; + return btSequentialImpulseConstraintSolver::solveGroupConvertConstraintPoststep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); +} + +void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +{ + for (int i = 0; i < ndof; ++i) + m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; } btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) { - btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm; + + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; btScalar deltaVelADotn = 0; btScalar deltaVelBDotn = 0; btSolverBody* bodyA = 0; @@ -223,8 +239,9 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); } - deltaImpulse -= deltaVelADotn * c.m_jacDiagABInv; //m_jacDiagABInv = 1./denom - deltaImpulse -= deltaVelBDotn * c.m_jacDiagABInv; + + deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; if (sum < c.m_lowerLimit) @@ -241,7 +258,7 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt { c.m_appliedImpulse = sum; } - + if (c.m_multiBodyA) { applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA); @@ -249,11 +266,12 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS } else if (c.m_solverBodyIdA >= 0) { - bodyA->internalApplyImpulse(c.m_contactNormal1 * bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + } if (c.m_multiBodyB) { @@ -262,54 +280,54 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS } else if (c.m_solverBodyIdB >= 0) { - bodyB->internalApplyImpulse(c.m_contactNormal2 * bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse); } - btScalar deltaVel = deltaImpulse / c.m_jacDiagABInv; + btScalar deltaVel =deltaImpulse/c.m_jacDiagABInv; return deltaVel; } -btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB) + +btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1,const btMultiBodySolverConstraint& cB) { - int ndofA = 0; - int ndofB = 0; + int ndofA=0; + int ndofB=0; btSolverBody* bodyA = 0; btSolverBody* bodyB = 0; btScalar deltaImpulseB = 0.f; btScalar sumB = 0.f; { - deltaImpulseB = cB.m_rhs - btScalar(cB.m_appliedImpulse) * cB.m_cfm; - btScalar deltaVelADotn = 0; - btScalar deltaVelBDotn = 0; + deltaImpulseB = cB.m_rhs-btScalar(cB.m_appliedImpulse)*cB.m_cfm; + btScalar deltaVelADotn=0; + btScalar deltaVelBDotn=0; if (cB.m_multiBodyA) { - ndofA = cB.m_multiBodyA->getNumDofs() + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex + i]; - } - else if (cB.m_solverBodyIdA >= 0) + ndofA = cB.m_multiBodyA->getNumDofs() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex+i]; + } else if(cB.m_solverBodyIdA >= 0) { bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA]; - deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); } if (cB.m_multiBodyB) { - ndofB = cB.m_multiBodyB->getNumDofs() + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex + i]; - } - else if (cB.m_solverBodyIdB >= 0) + ndofB = cB.m_multiBodyB->getNumDofs() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex+i]; + } else if(cB.m_solverBodyIdB >= 0) { bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB]; - deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); } - deltaImpulseB -= deltaVelADotn * cB.m_jacDiagABInv; //m_jacDiagABInv = 1./denom - deltaImpulseB -= deltaVelBDotn * cB.m_jacDiagABInv; + + deltaImpulseB -= deltaVelADotn*cB.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulseB -= deltaVelBDotn*cB.m_jacDiagABInv; sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB; } @@ -318,45 +336,45 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt const btMultiBodySolverConstraint& cA = cA1; { { - deltaImpulseA = cA.m_rhs - btScalar(cA.m_appliedImpulse) * cA.m_cfm; - btScalar deltaVelADotn = 0; - btScalar deltaVelBDotn = 0; + deltaImpulseA = cA.m_rhs-btScalar(cA.m_appliedImpulse)*cA.m_cfm; + btScalar deltaVelADotn=0; + btScalar deltaVelBDotn=0; if (cA.m_multiBodyA) { - ndofA = cA.m_multiBodyA->getNumDofs() + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex + i]; - } - else if (cA.m_solverBodyIdA >= 0) + ndofA = cA.m_multiBodyA->getNumDofs() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex+i]; + } else if(cA.m_solverBodyIdA >= 0) { bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA]; - deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); } if (cA.m_multiBodyB) { - ndofB = cA.m_multiBodyB->getNumDofs() + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex + i]; - } - else if (cA.m_solverBodyIdB >= 0) + ndofB = cA.m_multiBodyB->getNumDofs() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex+i]; + } else if(cA.m_solverBodyIdB >= 0) { bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB]; - deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); } - deltaImpulseA -= deltaVelADotn * cA.m_jacDiagABInv; //m_jacDiagABInv = 1./denom - deltaImpulseA -= deltaVelBDotn * cA.m_jacDiagABInv; + + deltaImpulseA -= deltaVelADotn*cA.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulseA -= deltaVelBDotn*cA.m_jacDiagABInv; sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA; } } - if (sumA * sumA + sumB * sumB >= cA.m_lowerLimit * cB.m_lowerLimit) + if (sumA*sumA+sumB*sumB>=cA.m_lowerLimit*cB.m_lowerLimit) { - btScalar angle = btAtan2(sumA, sumB); - btScalar sumAclipped = btFabs(cA.m_lowerLimit * btSin(angle)); - btScalar sumBclipped = btFabs(cB.m_lowerLimit * btCos(angle)); + btScalar angle = btAtan2(sumA,sumB); + btScalar sumAclipped = btFabs(cA.m_lowerLimit*btSin(angle)); + btScalar sumBclipped = btFabs(cB.m_lowerLimit*btCos(angle)); + if (sumA < -sumAclipped) { deltaImpulseA = -sumAclipped - cA.m_appliedImpulse; @@ -390,77 +408,78 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt //cA.m_appliedImpulse = sumAclipped; //deltaImpulseB = sumBclipped-cB.m_appliedImpulse; //cB.m_appliedImpulse = sumBclipped; - } + } else { cA.m_appliedImpulse = sumA; cB.m_appliedImpulse = sumB; } - + if (cA.m_multiBodyA) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA, cA.m_deltaVelAindex, ndofA); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA,cA.m_deltaVelAindex,ndofA); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } - else if (cA.m_solverBodyIdA >= 0) + cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } else if(cA.m_solverBodyIdA >= 0) { - bodyA->internalApplyImpulse(cA.m_contactNormal1 * bodyA->internalGetInvMass(), cA.m_angularComponentA, deltaImpulseA); + bodyA->internalApplyImpulse(cA.m_contactNormal1*bodyA->internalGetInvMass(),cA.m_angularComponentA,deltaImpulseA); + } if (cA.m_multiBodyB) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA, cA.m_deltaVelBindex, ndofB); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA,cA.m_deltaVelBindex,ndofB); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } - else if (cA.m_solverBodyIdB >= 0) + cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } else if(cA.m_solverBodyIdB >= 0) { - bodyB->internalApplyImpulse(cA.m_contactNormal2 * bodyB->internalGetInvMass(), cA.m_angularComponentB, deltaImpulseA); + bodyB->internalApplyImpulse(cA.m_contactNormal2*bodyB->internalGetInvMass(),cA.m_angularComponentB,deltaImpulseA); } if (cB.m_multiBodyA) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB, cB.m_deltaVelAindex, ndofA); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB,cB.m_deltaVelAindex,ndofA); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } - else if (cB.m_solverBodyIdA >= 0) + cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } else if(cB.m_solverBodyIdA >= 0) { - bodyA->internalApplyImpulse(cB.m_contactNormal1 * bodyA->internalGetInvMass(), cB.m_angularComponentA, deltaImpulseB); + bodyA->internalApplyImpulse(cB.m_contactNormal1*bodyA->internalGetInvMass(),cB.m_angularComponentA,deltaImpulseB); } if (cB.m_multiBodyB) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB, cB.m_deltaVelBindex, ndofB); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB,cB.m_deltaVelBindex,ndofB); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } - else if (cB.m_solverBodyIdB >= 0) + cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } else if(cB.m_solverBodyIdB >= 0) { - bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB); + bodyB->internalApplyImpulse(cB.m_contactNormal2*bodyB->internalGetInvMass(),cB.m_angularComponentB,deltaImpulseB); } - btScalar deltaVel = deltaImpulseA / cA.m_jacDiagABInv + deltaImpulseB / cB.m_jacDiagABInv; - return deltaVel; + btScalar deltaVel =deltaImpulseA/cA.m_jacDiagABInv+deltaImpulseB/cB.m_jacDiagABInv; + return deltaVel; } -void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) + + + +void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { + BT_PROFILE("setupMultiBodyContactConstraint"); btVector3 rel_pos1; btVector3 rel_pos2; @@ -478,46 +497,44 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); if (bodyB) rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); relaxation = infoGlobal.m_sor; - - btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep; - - //cfm = 1 / ( dt * kp + kd ) - //erp = dt * kp / ( dt * kp + kd ) - - btScalar cfm; + + btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; + + //cfm = 1 / ( dt * kp + kd ) + //erp = dt * kp / ( dt * kp + kd ) + + btScalar cfm; btScalar erp; if (isFriction) { cfm = infoGlobal.m_frictionCFM; erp = infoGlobal.m_frictionERP; - } - else + } else { cfm = infoGlobal.m_globalCfm; erp = infoGlobal.m_erp2; - if ((cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP)) + if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)) { - if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) - cfm = cp.m_contactCFM; - if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP) - erp = cp.m_contactERP; - } - else + if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) + cfm = cp.m_contactCFM; + if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP) + erp = cp.m_contactERP; + } else { if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING) { - btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1); + btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 ); if (denom < SIMD_EPSILON) { denom = SIMD_EPSILON; } - cfm = btScalar(1) / denom; + cfm = btScalar(1) / denom; erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; } } @@ -527,185 +544,188 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (multiBodyA) { - if (solverConstraint.m_linkA < 0) + if (solverConstraint.m_linkA<0) { rel_pos1 = pos1 - multiBodyA->getBasePos(); - } - else + } else { rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); } - const int ndofA = multiBodyA->getNumDofs() + 6; + const int ndofA = multiBodyA->getNumDofs() + 6; solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - if (solverConstraint.m_deltaVelAindex < 0) + if (solverConstraint.m_deltaVelAindex <0) { solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA); - } - else + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); + } else { - btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA); + btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); } solverConstraint.m_jacAindex = m_data.m_jacobians.size(); - m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA); + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v); + multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = contactNormal; - } - else + } else { btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = contactNormal; - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); } + + if (multiBodyB) { - if (solverConstraint.m_linkB < 0) + if (solverConstraint.m_linkB<0) { rel_pos2 = pos2 - multiBodyB->getBasePos(); - } - else + } else { rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); } - const int ndofB = multiBodyB->getNumDofs() + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex < 0) + if (solverConstraint.m_deltaVelBindex <0) { solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); } solverConstraint.m_jacBindex = m_data.m_jacobians.size(); - m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB); + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v); - - btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); + multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); + + btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; solverConstraint.m_contactNormal2 = -contactNormal; - } - else + + } else { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); + btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; solverConstraint.m_contactNormal2 = -contactNormal; - - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0); + + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); } { + btVector3 vec; btScalar denom0 = 0.f; btScalar denom1 = 0.f; btScalar* jacB = 0; btScalar* jacA = 0; - btScalar* lambdaA = 0; - btScalar* lambdaB = 0; - int ndofA = 0; + btScalar* lambdaA =0; + btScalar* lambdaB =0; + int ndofA = 0; if (multiBodyA) { - ndofA = multiBodyA->getNumDofs() + 6; + ndofA = multiBodyA->getNumDofs() + 6; jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA; ++i) { - btScalar j = jacA[i]; - btScalar l = lambdaA[i]; - denom0 += j * l; + btScalar j = jacA[i] ; + btScalar l =lambdaA[i]; + denom0 += j*l; } - } - else + } else { if (rb0) { - vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); denom0 = rb0->getInvMass() + contactNormal.dot(vec); } } if (multiBodyB) { - const int ndofB = multiBodyB->getNumDofs() + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB; ++i) { - btScalar j = jacB[i]; - btScalar l = lambdaB[i]; - denom1 += j * l; + btScalar j = jacB[i] ; + btScalar l =lambdaB[i]; + denom1 += j*l; } - } - else + + } else { if (rb1) { - vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2); + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); denom1 = rb1->getInvMass() + contactNormal.dot(vec); } } - btScalar d = denom0 + denom1 + cfm; - if (d > SIMD_EPSILON) - { - solverConstraint.m_jacDiagABInv = relaxation / (d); - } - else - { + + + btScalar d = denom0+denom1+cfm; + if (d>SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation/(d); + } else + { //disable the constraint row to handle singularity/redundant constraint - solverConstraint.m_jacDiagABInv = 0.f; - } + solverConstraint.m_jacDiagABInv = 0.f; + } + } + //compute rhs and remaining solverConstraint fields - btScalar restitution = 0.f; - btScalar distance = 0; - if (!isFriction) - { - distance = cp.getDistance() + infoGlobal.m_linearSlop; - } - else - { - if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) - { - distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal); - } - } + + btScalar restitution = 0.f; + btScalar distance = 0; + if (!isFriction) + { + distance = cp.getDistance()+infoGlobal.m_linearSlop; + } else + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) + { + distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal); + } + } + + btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; + int ndofA = 0; + int ndofB = 0; { - btVector3 vel1, vel2; + + btVector3 vel1,vel2; if (multiBodyA) { - ndofA = multiBodyA->getNumDofs() + 6; + ndofA = multiBodyA->getNumDofs() + 6; btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) + for (int i = 0; i < ndofA ; ++i) rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } - else + } else { if (rb0) { @@ -717,12 +737,12 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } if (multiBodyB) { - ndofB = multiBodyB->getNumDofs() + 6; + ndofB = multiBodyB->getNumDofs() + 6; btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) + for (int i = 0; i < ndofB ; ++i) rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - } - else + + } else { if (rb1) { @@ -735,9 +755,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol solverConstraint.m_friction = cp.m_combinedFriction; - if (!isFriction) + if(!isFriction) { - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); if (restitution <= btScalar(0.)) { restitution = 0.f; @@ -745,9 +765,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } } + ///warm starting (or zero if disabled) //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) - if (0) //infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; @@ -757,30 +778,27 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { btScalar impulse = solverConstraint.m_appliedImpulse; btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVeeMultiDof(deltaV, impulse); - - applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA); - } - else + multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse); + + applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); + } else { if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); } if (multiBodyB) { btScalar impulse = solverConstraint.m_appliedImpulse; btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVeeMultiDof(deltaV, impulse); - applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB); - } - else + multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse); + applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); + } else { if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse); + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); } } - } - else + } else { solverConstraint.m_appliedImpulse = 0.f; } @@ -788,37 +806,38 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol solverConstraint.m_appliedPushImpulse = 0.f; { + btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction + btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction if (isFriction) { - positionalError = -distance * erp / infoGlobal.m_timeStep; - } - else + positionalError = -distance * erp/infoGlobal.m_timeStep; + } else { - if (distance > 0) + if (distance>0) { positionalError = 0; velocityError -= distance / infoGlobal.m_timeStep; - } - else + + } else { - positionalError = -distance * erp / infoGlobal.m_timeStep; + positionalError = -distance * erp/infoGlobal.m_timeStep; } } - btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - if (!isFriction) + if(!isFriction) { - // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; + } - /*else + /*else { //split position and velocity into rhs and m_rhsPenetration solverConstraint.m_rhs = velocityImpulse; @@ -830,288 +849,305 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } else { - solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; } - solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv; + solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv; + + + } + } void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& constraintNormal, - btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) + const btVector3& constraintNormal, + btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { - BT_PROFILE("setupMultiBodyRollingFrictionConstraint"); - btVector3 rel_pos1; - btVector3 rel_pos2; - - btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; - btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - - btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; - btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; - - btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; - btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; - - if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - - relaxation = infoGlobal.m_sor; - - // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; - - if (multiBodyA) - { - if (solverConstraint.m_linkA < 0) - { - rel_pos1 = pos1 - multiBodyA->getBasePos(); - } - else - { - rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); - } - const int ndofA = multiBodyA->getNumDofs() + 6; - - solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (solverConstraint.m_deltaVelAindex < 0) - { - solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA); - } - else - { - btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA); - } - - solverConstraint.m_jacAindex = m_data.m_jacobians.size(); - m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA); - btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - - btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex]; - multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0, 0, 0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v); - - btVector3 torqueAxis0 = -constraintNormal; - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = btVector3(0, 0, 0); - } - else - { - btVector3 torqueAxis0 = -constraintNormal; - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = btVector3(0, 0, 0); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0); - } - - if (multiBodyB) - { - if (solverConstraint.m_linkB < 0) - { - rel_pos2 = pos2 - multiBodyB->getBasePos(); - } - else - { - rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); - } - - const int ndofB = multiBodyB->getNumDofs() + 6; - - solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex < 0) - { - solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB); - } - - solverConstraint.m_jacBindex = m_data.m_jacobians.size(); - - m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB); - btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - - multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0, 0, 0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v); - - btVector3 torqueAxis1 = constraintNormal; - solverConstraint.m_relpos2CrossNormal = torqueAxis1; - solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0); - } - else - { - btVector3 torqueAxis1 = constraintNormal; - solverConstraint.m_relpos2CrossNormal = torqueAxis1; - solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0); - - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0); - } - - { - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* lambdaA = 0; - btScalar* lambdaB = 0; - int ndofA = 0; - if (multiBodyA) - { - ndofA = multiBodyA->getNumDofs() + 6; - jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; - lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i]; - btScalar l = lambdaA[i]; - denom0 += j * l; - } - } - else - { - if (rb0) - { - btVector3 iMJaA = rb0 ? rb0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0); + + BT_PROFILE("setupMultiBodyRollingFrictionConstraint"); + btVector3 rel_pos1; + btVector3 rel_pos2; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; + btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = infoGlobal.m_sor; + + // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; + + + if (multiBodyA) + { + if (solverConstraint.m_linkA<0) + { + rel_pos1 = pos1 - multiBodyA->getBasePos(); + } else + { + rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); + } + const int ndofA = multiBodyA->getNumDofs() + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + solverConstraint.m_jacAindex = m_data.m_jacobians.size(); + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; + multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0,0,0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); + + btVector3 torqueAxis0 = -constraintNormal; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = btVector3(0,0,0); + } else + { + btVector3 torqueAxis0 = -constraintNormal; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = btVector3(0,0,0); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + } + + + + if (multiBodyB) + { + if (solverConstraint.m_linkB<0) + { + rel_pos2 = pos2 - multiBodyB->getBasePos(); + } else + { + rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); + } + + const int ndofB = multiBodyB->getNumDofs() + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); + } + + solverConstraint.m_jacBindex = m_data.m_jacobians.size(); + + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0,0,0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); + + btVector3 torqueAxis1 = constraintNormal; + solverConstraint.m_relpos2CrossNormal = torqueAxis1; + solverConstraint.m_contactNormal2 = -btVector3(0,0,0); + + } else + { + btVector3 torqueAxis1 = constraintNormal; + solverConstraint.m_relpos2CrossNormal = torqueAxis1; + solverConstraint.m_contactNormal2 = -btVector3(0,0,0); + + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + } + + { + + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA =0; + btScalar* lambdaB =0; + int ndofA = 0; + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l =lambdaA[i]; + denom0 += j*l; + } + } else + { + if (rb0) + { + btVector3 iMJaA = rb0?rb0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0); denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal); - } - } - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumDofs() + 6; - jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; - lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i]; - btScalar l = lambdaB[i]; - denom1 += j * l; - } - } - else - { - if (rb1) - { - btVector3 iMJaB = rb1 ? rb1->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0); + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l =lambdaB[i]; + denom1 += j*l; + } + + } else + { + if (rb1) + { + btVector3 iMJaB = rb1?rb1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - } - } - - btScalar d = denom0 + denom1 + infoGlobal.m_globalCfm; - if (d > SIMD_EPSILON) - { - solverConstraint.m_jacDiagABInv = relaxation / (d); - } - else - { - //disable the constraint row to handle singularity/redundant constraint - solverConstraint.m_jacDiagABInv = 0.f; - } - } - - //compute rhs and remaining solverConstraint fields - - btScalar restitution = 0.f; - btScalar penetration = isFriction ? 0 : cp.getDistance(); - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - btVector3 vel1, vel2; - if (multiBodyA) - { - ndofA = multiBodyA->getNumDofs() + 6; - btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } - else + } + } + + + + btScalar d = denom0+denom1+infoGlobal.m_globalCfm; + if (d>SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation/(d); + } else + { + //disable the constraint row to handle singularity/redundant constraint + solverConstraint.m_jacDiagABInv = 0.f; + } + + } + + + //compute rhs and remaining solverConstraint fields + + + + btScalar restitution = 0.f; + btScalar penetration = isFriction? 0 : cp.getDistance(); + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } else { if (rb0) { btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; rel_vel += solverConstraint.m_contactNormal1.dot(rb0 ? solverBodyA->m_linearVelocity + solverBodyA->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? solverBodyA->m_angularVelocity : btVector3(0, 0, 0)); } - } - if (multiBodyB) - { - ndofB = multiBodyB->getNumDofs() + 6; - btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - } - else - { + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumDofs() + 6; + btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } else + { if (rb1) { btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; rel_vel += solverConstraint.m_contactNormal2.dot(rb1 ? solverBodyB->m_linearVelocity + solverBodyB->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? solverBodyB->m_angularVelocity : btVector3(0, 0, 0)); - } - } + } + } - solverConstraint.m_friction = combinedTorsionalFriction; - - if (!isFriction) - { - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - } - } - } - - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - - { - btScalar velocityError = 0 - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction - - btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; - - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - solverConstraint.m_lowerLimit = -solverConstraint.m_friction; - solverConstraint.m_upperLimit = solverConstraint.m_friction; - - solverConstraint.m_cfm = infoGlobal.m_globalCfm * solverConstraint.m_jacDiagABInv; - } + solverConstraint.m_friction =combinedTorsionalFriction; + + if(!isFriction) + { + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + } + } + } + + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + + btScalar velocityError = 0 - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction + + + + btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv; + + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + solverConstraint.m_lowerLimit = -solverConstraint.m_friction; + solverConstraint.m_upperLimit = solverConstraint.m_friction; + + solverConstraint.m_cfm = infoGlobal.m_globalCfm*solverConstraint.m_jacDiagABInv; + + + + } + } -btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { BT_PROFILE("addMultiBodyFrictionConstraint"); btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; - + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; + solverConstraint.m_frictionIndex = frictionIndex; bool isFriction = true; const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB? fcB->m_multiBody : 0; - btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; - - int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); + int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -1125,92 +1161,95 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo solverConstraint.m_originalContactPoint = &cp; - setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); + setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); return solverConstraint; } -btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { - BT_PROFILE("addMultiBodyRollingFrictionConstraint"); + BT_PROFILE("addMultiBodyRollingFrictionConstraint"); - bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)); + bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)); - btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction ? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing(); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; - - solverConstraint.m_frictionIndex = frictionIndex; - bool isFriction = true; - - const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); - const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - - btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; - - int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); - - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_multiBodyA = mbA; - if (mbA) - solverConstraint.m_linkA = fcA->m_link; - - solverConstraint.m_multiBodyB = mbB; - if (mbB) - solverConstraint.m_linkB = fcB->m_link; - - solverConstraint.m_originalContactPoint = &cp; - - setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); - return solverConstraint; + btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing(); + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; + + solverConstraint.m_frictionIndex = frictionIndex; + bool isFriction = true; + + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB? fcB->m_multiBody : 0; + + int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction,infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); + return solverConstraint; } -void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) +void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) { const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB? fcB->m_multiBody : 0; - btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; - - btCollisionObject *colObj0 = 0, *colObj1 = 0; + btCollisionObject* colObj0=0,*colObj1=0; colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); - int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); + int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + +// btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; +// btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; - // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; - // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; ///avoid collision response between two static objects - // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) +// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) // return; - //only a single rollingFriction per manifold - int rollingFriction = 1; - - for (int j = 0; j < manifold->getNumContacts(); j++) + //only a single rollingFriction per manifold + int rollingFriction=1; + + for (int j=0;jgetNumContacts();j++) { + btManifoldPoint& cp = manifold->getContactPoint(j); if (cp.getDistance() <= manifold->getContactProcessingThreshold()) { + btScalar relaxation; int frictionIndex = m_multiBodyNormalContactConstraints.size(); btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); - // btRigidBody* rb0 = btRigidBody::upcast(colObj0); - // btRigidBody* rb1 = btRigidBody::upcast(colObj1); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; + // btRigidBody* rb0 = btRigidBody::upcast(colObj0); + // btRigidBody* rb1 = btRigidBody::upcast(colObj1); + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_multiBodyA = mbA; @@ -1224,10 +1263,10 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* solverConstraint.m_originalContactPoint = &cp; bool isFriction = false; - setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp, infoGlobal, relaxation, isFriction); + setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction); - // const btVector3& pos1 = cp.getPositionWorldOnA(); - // const btVector3& pos2 = cp.getPositionWorldOnB(); +// const btVector3& pos1 = cp.getPositionWorldOnA(); +// const btVector3& pos2 = cp.getPositionWorldOnB(); /////setup the friction constraints #define ENABLE_FRICTION @@ -1238,45 +1277,46 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* ///By default, each contact has only a single friction direction that is recomputed automatically every frame ///based on the relative linear velocity. ///If the relative velocity is zero, it will automatically compute a friction direction. - + ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. /// ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. /// - ///The user can manually override the friction directions for certain contacts using a contact callback, + ///The user can manually override the friction directions for certain contacts using a contact callback, ///and set the cp.m_lateralFrictionInitialized to true ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect /// - btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2); + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); cp.m_lateralFrictionDir1.normalize(); cp.m_lateralFrictionDir2.normalize(); - if (rollingFriction > 0) - { - if (cp.m_combinedSpinningFriction > 0) - { - addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal); - } - if (cp.m_combinedRollingFriction > 0) - { - applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + if (rollingFriction > 0 ) + { + if (cp.m_combinedSpinningFriction>0) + { + addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal); + } + if (cp.m_combinedRollingFriction>0) + { - if (cp.m_lateralFrictionDir1.length() > 0.001) - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + + if (cp.m_lateralFrictionDir1.length()>0.001) + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); - if (cp.m_lateralFrictionDir2.length() > 0.001) - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); - } - rollingFriction--; - } - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) - { /* + if (cp.m_lateralFrictionDir2.length()>0.001) + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); + } + rollingFriction--; + } + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) + {/* cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) @@ -1299,77 +1339,155 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* } else */ { - applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); + + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); } if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) { - cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; + cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; } } - } - else + + } else { - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_frictionCFM); //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); //todo: solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; } + + +#endif //ENABLE_FRICTION -#endif //ENABLE_FRICTION } } } -void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) +void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) { //btPersistentManifold* manifold = 0; - for (int i = 0; i < numManifolds; i++) + for (int i=0;igetBody0()); const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); if (!fcA && !fcB) { //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case - convertContact(manifold, infoGlobal); - } - else + convertContact(manifold,infoGlobal); + } else { - convertMultiBodyContact(manifold, infoGlobal); + convertMultiBodyContact(manifold,infoGlobal); } } //also convert the multibody constraints, if any - for (int i = 0; i < m_tmpNumMultiBodyConstraints; i++) + + for (int i=0;icreateConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal); + } - c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal); +} + +static void copyConstraintsFromProxy(btAlignedObjectArray& constraints, const btAlignedObjectArray& constraintsProxy, bool onlyDynamicData) +{ + if (onlyDynamicData) + { + btAssert(constraints.size() == constraintsProxy.size()); + for (int i = 0; i < constraintsProxy.size(); ++i) + { + btMultiBodySolverConstraint& destConstraint = constraints[i]; + const btMultiBodySolverConstraint& srcConstraint = constraintsProxy[i]; + + destConstraint.m_appliedImpulse = srcConstraint.m_appliedImpulse; + destConstraint.m_appliedPushImpulse = srcConstraint.m_appliedPushImpulse; + } + return; + } + + constraints.resize(constraintsProxy.size()); + for (int i = 0; i < constraintsProxy.size(); ++i) + { + constraints[i] = constraintsProxy[i]; } } -btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) +static void copyConstraintsToProxy(btAlignedObjectArray& constraintsProxy, btAlignedObjectArray& constraints, bool onlyDynamicData) +{ + if (onlyDynamicData) + { + btAssert(constraintsProxy.size() == constraints.size()); + for (int i = 0; i < constraints.size(); ++i) + { + btMultiBodySolverConstraint& destConstraint = constraintsProxy[i]; + const btMultiBodySolverConstraint& srcConstraint = constraints[i]; + + destConstraint.m_appliedImpulse = srcConstraint.m_appliedImpulse; + destConstraint.m_appliedPushImpulse = srcConstraint.m_appliedPushImpulse; + } + return; + } + + constraintsProxy.resize(constraints.size()); + for (int i = 0; i < constraints.size(); ++i) + { + constraintsProxy[i] = constraints[i]; + } +} + +void btMultiBodyConstraintSolver::setMultiBodyInternalConstraintData(const btMultiBodyInternalConstraintData& data, bool onlyDynamicData) +{ + copyConstraintsFromProxy(m_multiBodyNonContactConstraints, data.m_multiBodyNonContactConstraints, onlyDynamicData); + copyConstraintsFromProxy(m_multiBodyNormalContactConstraints, data.m_multiBodyNormalContactConstraints, onlyDynamicData); + copyConstraintsFromProxy(m_multiBodyFrictionContactConstraints, data.m_multiBodyFrictionContactConstraints, onlyDynamicData); + copyConstraintsFromProxy(m_multiBodyTorsionalFrictionContactConstraints, data.m_multiBodyTorsionalFrictionContactConstraints, onlyDynamicData); + + if (onlyDynamicData) + m_data.m_deltaVelocities = data.m_data.m_deltaVelocities; + else + m_data = data.m_data; +} + +void btMultiBodyConstraintSolver::getMultiBodyInternalConstraintData(btMultiBodyInternalConstraintData& data, bool onlyDynamicData) +{ + copyConstraintsToProxy(data.m_multiBodyNonContactConstraints, m_multiBodyNonContactConstraints, onlyDynamicData); + copyConstraintsToProxy(data.m_multiBodyNormalContactConstraints, m_multiBodyNormalContactConstraints, onlyDynamicData); + copyConstraintsToProxy(data.m_multiBodyFrictionContactConstraints, m_multiBodyFrictionContactConstraints, onlyDynamicData); + copyConstraintsToProxy(data.m_multiBodyTorsionalFrictionContactConstraints, m_multiBodyTorsionalFrictionContactConstraints, onlyDynamicData); + + if (onlyDynamicData) + data.m_data.m_deltaVelocities = m_data.m_deltaVelocities; + else + data.m_data = m_data; +} + +btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) { //printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints); - return btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher); + return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); } #if 0 @@ -1393,54 +1511,56 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS } #endif + void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime) { -#if 1 - +#if 1 + //bod->addBaseForce(m_gravity * bod->getBaseMass()); //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); if (c.m_orgConstraint) { - c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex, c.m_appliedImpulse); + c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse); } + if (c.m_multiBodyA) { + c.m_multiBodyA->setCompanionId(-1); - btVector3 force = c.m_contactNormal1 * (c.m_appliedImpulse / deltaTime); - btVector3 torque = c.m_relpos1CrossNormal * (c.m_appliedImpulse / deltaTime); - if (c.m_linkA < 0) + btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime); + btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime); + if (c.m_linkA<0) { c.m_multiBodyA->addBaseConstraintForce(force); c.m_multiBodyA->addBaseConstraintTorque(torque); - } - else + } else { - c.m_multiBodyA->addLinkConstraintForce(c.m_linkA, force); - //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); - c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA, torque); + c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force); + //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); + c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque); } } - + if (c.m_multiBodyB) { { c.m_multiBodyB->setCompanionId(-1); - btVector3 force = c.m_contactNormal2 * (c.m_appliedImpulse / deltaTime); - btVector3 torque = c.m_relpos2CrossNormal * (c.m_appliedImpulse / deltaTime); - if (c.m_linkB < 0) + btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime); + btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime); + if (c.m_linkB<0) { c.m_multiBodyB->addBaseConstraintForce(force); c.m_multiBodyB->addBaseConstraintTorque(torque); - } - else + } else { { - c.m_multiBodyB->addLinkConstraintForce(c.m_linkB, force); + c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force); //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); - c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB, torque); + c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque); } + } } } @@ -1450,53 +1570,59 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv if (c.m_multiBodyA) { - c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], c.m_appliedImpulse); + c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); } - + if (c.m_multiBodyB) { - c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], c.m_appliedImpulse); + c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); } #endif + + + } -btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) { BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish"); int numPoolConstraints = m_multiBodyNormalContactConstraints.size(); + - //write back the delta v to the multi bodies, either as applied impulse (direct velocity change) + //write back the delta v to the multi bodies, either as applied impulse (direct velocity change) //or as applied force, so we can measure the joint reaction forces easier - for (int i = 0; i < numPoolConstraints; i++) + for (int i=0;im_appliedImpulse = solverConstraint.m_appliedImpulse; pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse; - + //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { @@ -1509,119 +1635,23 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO //do a callback here? } -#if 0 - //multibody joint feedback - { - BT_PROFILE("multi body joint feedback"); - for (int j=0;jisMultiDof()) - { - applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); - } - if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof()) - { - applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep)); - } -#if 0 - if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof()) - { - applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex], - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); - - } - if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof()) - { - applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex], - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep)); - } - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof()) - { - applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1], - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); - } - - if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof()) - { - applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1], - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep)); - } - } -#endif - } - - for (int i=0;iisMultiDof()) - { - applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); - } - if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof()) - { - applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); - } - } - } - - numPoolConstraints = m_multiBodyNonContactConstraints.size(); - -#if 0 - //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint - for (int i=0;igetJointFeedback(); - if (fb) - { - fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; - fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ - - } - - constr->internalSetAppliedImpulse(c.m_appliedImpulse); - if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) - { - constr->setEnabled(false); - } - - } -#endif -#endif return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); } -void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) + +void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) { //printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints); //printf("solveMultiBodyGroup start\n"); m_tmpMultiBodyConstraints = multiBodyConstraints; m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher); + + btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); m_tmpMultiBodyConstraints = 0; m_tmpNumMultiBodyConstraints = 0; + + } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index f39f2879f..057ededbe 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -28,68 +28,111 @@ class btMultiBody; ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver { + protected: - btMultiBodyConstraintArray m_multiBodyNonContactConstraints; - btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; - btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; - btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints; + btMultiBodyConstraintArray m_multiBodyNonContactConstraints; - btMultiBodyJacobianData m_data; + btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; + btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; + btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints; + btMultiBodyJacobianData m_data; + //temp storage for multi body constraints for a specific island/group called by 'solveGroup' - btMultiBodyConstraint** m_tmpMultiBodyConstraints; - int m_tmpNumMultiBodyConstraints; + btMultiBodyConstraint** m_tmpMultiBodyConstraints; + int m_tmpNumMultiBodyConstraints; btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); - + //solve 2 friction directions and clamp against the implicit friction cone btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB); + - void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); + void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); + + btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); - btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); - btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, + btScalar* jacA,btScalar* jacB, + btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, + const btContactSolverInfo& infoGlobal); - void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint & constraintRow, - btScalar * jacA, btScalar * jacB, - btScalar penetration, btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, - const btContactSolverInfo& infoGlobal); + void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + //either rolling or spinning friction + void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); - void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); - - //either rolling or spinning friction - void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint & solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); - - void convertMultiBodyContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal); - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - // virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - - virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof); - void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime); + void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); +// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof); + void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime); public: + + struct btMultiBodyInternalConstraintData + { + /// Multibody (joint) constraints. This is shared by all the blocks. + btMultiBodyConstraint** m_multiBodyConstraints; + + /// Number of multibody (joint) constraints. This is shared by all the + /// blocks. + int m_numMultiBodyConstraints; + + /// Array of multibody non-contact constraints + btAlignedObjectArray m_multiBodyNonContactConstraints; + + /// Array of multibody normal contact constraints + btAlignedObjectArray m_multiBodyNormalContactConstraints; + + /// Array of multibody friction contact constraints + btAlignedObjectArray m_multiBodyFrictionContactConstraints; + + /// Array of multibody rolling friction contact constraints + btAlignedObjectArray m_multiBodyTorsionalFrictionContactConstraints; + + /// Pointer to the block constraint solver's multi body Jacobian data, which + /// is shared by all the constraint blocks. + btMultiBodyJacobianData m_data; + }; + BT_DECLARE_ALIGNED_ALLOCATOR(); - ///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints) - virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); - virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); + /// Copies internal constraint data from \p proxy + virtual void setMultiBodyInternalConstraintData(const btMultiBodyInternalConstraintData& proxy, bool onlyDynamicData = false); - virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + /// Copies internal constraint data to \p proxy + virtual void getMultiBodyInternalConstraintData(btMultiBodyInternalConstraintData& data, bool onlyDynamicData = false); + + ///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints) + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); + virtual btScalar solveGroupConvertConstraintPrestep(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + virtual btScalar solveGroupConvertConstraintPoststep(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + virtual btScalar solveSingleIterationNew(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); + + virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); }; -#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H + + + + +#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H + From 48d84e78993c0e9b8e1a47af67fe558eac2a67c3 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 27 Feb 2019 16:00:56 -0800 Subject: [PATCH 3/6] revert experiment not passing the constraint solver back down from btMultiBodyDynamicsWorld to btDiscreteDynamicsWorld. --- src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index a81092782..1557987bc 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -362,7 +362,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: }; btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) - : btDiscreteDynamicsWorld(dispatcher, pairCache, 0, collisionConfiguration), + : btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), m_multiBodyConstraintSolver(constraintSolver) { //split impulse is not yet supported for Featherstone hierarchies @@ -380,7 +380,7 @@ void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstrain { m_multiBodyConstraintSolver = solver; m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver); - //btDiscreteDynamicsWorld::setConstraintSolver(solver); + btDiscreteDynamicsWorld::setConstraintSolver(solver); } void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) From 36a9dcf3688ff8e8d354af25b20512db6665c640 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 27 Feb 2019 17:10:17 -0800 Subject: [PATCH 4/6] remove src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.cpp and examples/ConstraintSolvers/* code revert changes to btMultiBodyConstraintSolver/btSequentialImpulseConstraintSolver related to btMultiBodyBlockConstraintSolver --- examples/BlockSolver/BlockSolverExample.cpp | 389 ++--- examples/BlockSolver/BlockSolverExample.h | 2 +- examples/BlockSolver/btBlockSolver.cpp | 3 +- examples/ConstraintSolvers/BoxStacks.cpp | 335 ---- examples/ConstraintSolvers/BoxStacks.h | 7 - examples/ConstraintSolvers/BoxStacks_MLCP.cpp | 224 --- examples/ConstraintSolvers/BoxStacks_MLCP.h | 7 - examples/ConstraintSolvers/Grasp_Block.cpp | 335 ---- examples/ConstraintSolvers/Grasp_Block.h | 7 - examples/ExampleBrowser/CMakeLists.txt | 4 - examples/ExampleBrowser/ExampleEntries.cpp | 20 +- examples/ExampleBrowser/premake4.lua | 1 - .../SerialChains.cpp | 51 +- .../SerialChains.h | 0 src/BulletDynamics/CMakeLists.txt | 1 - .../btSequentialImpulseConstraintSolver.cpp | 163 +- .../btSequentialImpulseConstraintSolver.h | 84 +- .../btMultiBodyBlockConstraintSolver.cpp | 760 --------- .../btMultiBodyBlockConstraintSolver.h | 186 -- .../btMultiBodyConstraintSolver.cpp | 1520 ++++++++--------- .../btMultiBodyConstraintSolver.h | 131 +- 21 files changed, 1006 insertions(+), 3224 deletions(-) delete mode 100644 examples/ConstraintSolvers/BoxStacks.cpp delete mode 100644 examples/ConstraintSolvers/BoxStacks.h delete mode 100644 examples/ConstraintSolvers/BoxStacks_MLCP.cpp delete mode 100644 examples/ConstraintSolvers/BoxStacks_MLCP.h delete mode 100644 examples/ConstraintSolvers/Grasp_Block.cpp delete mode 100644 examples/ConstraintSolvers/Grasp_Block.h rename examples/{ConstraintSolvers => MultiBody}/SerialChains.cpp (88%) rename examples/{ConstraintSolvers => MultiBody}/SerialChains.h (100%) delete mode 100644 src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.cpp delete mode 100644 src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h diff --git a/examples/BlockSolver/BlockSolverExample.cpp b/examples/BlockSolver/BlockSolverExample.cpp index 11db6e631..f3ab0cfed 100644 --- a/examples/BlockSolver/BlockSolverExample.cpp +++ b/examples/BlockSolver/BlockSolverExample.cpp @@ -1,26 +1,13 @@ #include "BlockSolverExample.h" -#include "../OpenGLWindow/SimpleOpenGL3App.h" -#include "btBulletDynamicsCommon.h" - #include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" - -#include "BulletDynamics/Featherstone/btMultiBody.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" -#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" -#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" -#include "BulletDynamics/Featherstone/btMultiBodyLink.h" -#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" -#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" -#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" -#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" -#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" #include "btBlockSolver.h" - -#include "../OpenGLWindow/GLInstancingRenderer.h" -#include "BulletCollision/CollisionShapes/btShapeHull.h" - +//for URDF import support +#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" +#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" +#include "../Importers/ImportURDFDemo/URDF2Bullet.h" #include "../CommonInterfaces/CommonMultiBodyBase.h" class BlockSolverExample : public CommonMultiBodyBase @@ -36,29 +23,24 @@ public: virtual void resetCamera() { - float dist = 1; + float dist = 3; float pitch = -35; float yaw = 50; - float targetPos[3] = {-3, 2.8, -2.5}; + float targetPos[3] = {0, 0, .1}; m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } - btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool fixedBase = false); - void createGround(const btVector3& halfExtents = btVector3(50, 50, 50), btScalar zOffSet = btScalar(-1.55)); - void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents); + void createMultiBodyStack(); + btMultiBody* createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape); + btMultiBody* loadRobot(std::string filepath); }; -static bool g_fixedBase = true; -static bool g_firstInit = true; -static float scaling = 0.4f; -static float friction = 1.; - BlockSolverExample::BlockSolverExample(GUIHelperInterface* helper, int option) : CommonMultiBodyBase(helper), m_option(option) { - m_guiHelper->setUpAxis(1); + m_guiHelper->setUpAxis(2); } BlockSolverExample::~BlockSolverExample() @@ -69,20 +51,13 @@ BlockSolverExample::~BlockSolverExample() void BlockSolverExample::stepSimulation(float deltaTime) { //use a smaller internal timestep, there are stability issues - float internalTimeStep = 1. / 240.f; + btScalar internalTimeStep = 1./240.f; m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep); } void BlockSolverExample::initPhysics() { - m_guiHelper->setUpAxis(1); - - if (g_firstInit) - { - m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10. * scaling)); - m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50); - g_firstInit = false; - } + ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new btDefaultCollisionConfiguration(); @@ -91,7 +66,7 @@ void BlockSolverExample::initPhysics() m_broadphase = new btDbvtBroadphase(); - + btMLCPSolverInterface* mlcp; if (m_option&BLOCK_SOLVER_SI) { @@ -117,261 +92,125 @@ void BlockSolverExample::initPhysics() { m_solver = new btBlockSolver(); } - + btAssert(m_solver); + + btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); m_dynamicsWorld = world; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); - m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good? + m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); + m_dynamicsWorld->getSolverInfo().m_numIterations = 50; + m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-6); //todo: what value is good? + if (m_option&BLOCK_SOLVER_SCENE_MB_STACK) + { + createMultiBodyStack(); + } - ///////////////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////////////// - - bool damping = true; - bool gyro = true; - int numLinks = 5; - bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals - bool multibodyOnly = true; //false - bool canSleep = true; - bool selfCollide = true; - btVector3 linkHalfExtents(0.05, 0.37, 0.1); - btVector3 baseHalfExtents(0.05, 0.37, 0.1); - - btMultiBody* mbC1 = createFeatherstoneMultiBody(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase); - btMultiBody* mbC2 = createFeatherstoneMultiBody(world, numLinks, btVector3(-0.4f, 3.0f, 0.5f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase); - - mbC1->setCanSleep(canSleep); - mbC1->setHasSelfCollision(selfCollide); - mbC1->setUseGyroTerm(gyro); - - if (!damping) - { - mbC1->setLinearDamping(0.f); - mbC1->setAngularDamping(0.f); - } - else - { - mbC1->setLinearDamping(0.1f); - mbC1->setAngularDamping(0.9f); - } - // - m_dynamicsWorld->setGravity(btVector3(0, -9.81, 0)); - ////////////////////////////////////////////// - if (numLinks > 0) - { - btScalar q0 = 45.f * SIMD_PI / 180.f; - if (!spherical) - { - mbC1->setJointPosMultiDof(0, &q0); - } - else - { - btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); - quat0.normalize(); - mbC1->setJointPosMultiDof(0, quat0); - } - } - /// - addColliders(mbC1, world, baseHalfExtents, linkHalfExtents); - - mbC2->setCanSleep(canSleep); - mbC2->setHasSelfCollision(selfCollide); - mbC2->setUseGyroTerm(gyro); - // - if (!damping) - { - mbC2->setLinearDamping(0.f); - mbC2->setAngularDamping(0.f); - } - else - { - mbC2->setLinearDamping(0.1f); - mbC2->setAngularDamping(0.9f); - } - // - m_dynamicsWorld->setGravity(btVector3(0, -9.81, 0)); - ////////////////////////////////////////////// - if (numLinks > 0) - { - btScalar q0 = -45.f * SIMD_PI / 180.f; - if (!spherical) - { - mbC2->setJointPosMultiDof(0, &q0); - } - else - { - btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); - quat0.normalize(); - mbC2->setJointPosMultiDof(0, quat0); - } - } - /// - addColliders(mbC2, world, baseHalfExtents, linkHalfExtents); - - ///////////////////////////////////////////////////////////////// - btScalar groundHeight = -51.55; - btScalar mass(0.); - - //rigidbody is dynamic if and only if mass is non zero, otherwise static - bool isDynamic = (mass != 0.f); - - btVector3 localInertia(0, 0, 0); - - - createGround(); - m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); - - ///////////////////////////////////////////////////////////////// } -btMultiBody* BlockSolverExample::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool fixedBase) + +void BlockSolverExample::createMultiBodyStack() { - //init the base - btVector3 baseInertiaDiag(0.f, 0.f, 0.f); - float baseMass = 1.f; - - if (baseMass) + ///create a few basic rigid bodies + bool loadPlaneFromURDF = false; + if (loadPlaneFromURDF) { - btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); - pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); - delete pTempBox; + btMultiBody* mb = loadRobot("plane.urdf"); + printf("!\n"); } - - bool canSleep = false; - - btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, fixedBase, canSleep); - - btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); - pMultiBody->setBasePos(basePosition); - pMultiBody->setWorldToBaseRot(baseOriQuat); - btVector3 vel(0, 0, 0); - - //init the links - btVector3 hingeJointAxis(1, 0, 0); - float linkMass = 1.f; - btVector3 linkInertiaDiag(0.f, 0.f, 0.f); - - btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); - pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); - delete pTempBox; - - //y-axis assumed up - btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset - btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset - btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset - - ////// - btScalar q0 = 0.f * SIMD_PI / 180.f; - btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); - quat0.normalize(); - ///// - - for (int i = 0; i < numLinks; ++i) + else { - if (!spherical) - pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true); - else - //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); - pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true); - } - - pMultiBody->finalizeMultiDof(); - - /// - pWorld->addMultiBody(pMultiBody); - /// - return pMultiBody; -} - -void BlockSolverExample::createGround(const btVector3& halfExtents, btScalar zOffSet) -{ - btCollisionShape* groundShape = new btBoxShape(halfExtents); - m_collisionShapes.push_back(groundShape); - - // rigidbody is dynamic if and only if mass is non zero, otherwise static - btScalar mass(0.); - const bool isDynamic = (mass != 0.f); - - btVector3 localInertia(0, 0, 0); - if (isDynamic) - groundShape->calculateLocalInertia(mass, localInertia); - - // using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects - btTransform groundTransform; - groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -halfExtents.z() + zOffSet, 0)); - btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); - btRigidBody* body = new btRigidBody(rbInfo); - - // add the body to the dynamics world - m_dynamicsWorld->addRigidBody(body, 1, 1 + 2); -} - -void BlockSolverExample::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) -{ - btAlignedObjectArray world_to_local; - world_to_local.resize(pMultiBody->getNumLinks() + 1); - - btAlignedObjectArray local_origin; - local_origin.resize(pMultiBody->getNumLinks() + 1); - world_to_local[0] = pMultiBody->getWorldToBaseRot(); - local_origin[0] = pMultiBody->getBasePos(); - - { - btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; - - if (1) - { - btCollisionShape* box = new btBoxShape(baseHalfExtents); - btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); - col->setCollisionShape(box); - - btTransform tr; - tr.setIdentity(); - tr.setOrigin(local_origin[0]); - tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); - col->setWorldTransform(tr); - - pWorld->addCollisionObject(col, 2, 1 + 2); - - col->setFriction(friction); - pMultiBody->setBaseCollider(col); - } - } - - for (int i = 0; i < pMultiBody->getNumLinks(); ++i) - { - const int parent = pMultiBody->getParent(i); - world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1]; - local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i))); - } - - for (int i = 0; i < pMultiBody->getNumLinks(); ++i) - { - btVector3 posr = local_origin[i + 1]; - - btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()}; - - btCollisionShape* box = new btBoxShape(linkHalfExtents); - btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); - - col->setCollisionShape(box); + btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.), btScalar(50.), btScalar(50.))); + m_collisionShapes.push_back(groundShape); + btScalar mass = 0; btTransform tr; tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); - col->setWorldTransform(tr); - col->setFriction(friction); - pWorld->addCollisionObject(col, 2, 1 + 2); - - pMultiBody->getLink(i).m_collider = col; + tr.setOrigin(btVector3(0, 0, -50)); + btMultiBody* body = createMultiBody(mass, tr, groundShape); } + + for (int i=0;i<10;i++) + { + btBoxShape* boxShape = createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1))); + m_collisionShapes.push_back(boxShape); + btScalar mass = 1; + if (i == 9) + mass = 100; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(0, 0, 0.1+i*0.2)); + btMultiBody* body = createMultiBody(mass, tr, boxShape); + } + if(0) + { + btMultiBody* mb = loadRobot("cube_small.urdf"); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(0, 0, 1.)); + mb->setBaseWorldTransform(tr); + } +} + +btMultiBody* BlockSolverExample::createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape) +{ + btVector3 inertia; + collisionShape->calculateLocalInertia(mass, inertia); + + bool canSleep = false; + bool isDynamic = mass > 0; + btMultiBody* mb = new btMultiBody(0, mass, inertia, !isDynamic, canSleep); + btMultiBodyLinkCollider* collider = new btMultiBodyLinkCollider(mb, -1); + collider->setWorldTransform(trans); + mb->setBaseWorldTransform(trans); + + collider->setCollisionShape(collisionShape); + + int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); + int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + + + this->m_dynamicsWorld->addCollisionObject(collider, collisionFilterGroup, collisionFilterMask); + mb->setBaseCollider(collider); + + mb->finalizeMultiDof(); + + + this->m_dynamicsWorld->addMultiBody(mb); + m_dynamicsWorld->forwardKinematics(); + return mb; +} + + + +btMultiBody* BlockSolverExample::loadRobot(std::string filepath) +{ + btMultiBody* m_multiBody = 0; + BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0); + bool loadOk = u2b.loadURDF(filepath.c_str());// lwr / kuka.urdf"); + if (loadOk) + { + int rootLinkIndex = u2b.getRootLinkIndex(); + b3Printf("urdf root link index = %d\n", rootLinkIndex); + MyMultiBodyCreator creation(m_guiHelper); + btTransform identityTrans; + identityTrans.setIdentity(); + ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, true, u2b.getPathPrefix()); + for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++) + { + m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i)); + } + m_multiBody = creation.getBulletMultiBody(); + if (m_multiBody) + { + b3Printf("Root link name = %s", u2b.getLinkName(u2b.getRootLinkIndex()).c_str()); + } + } + return m_multiBody; } CommonExampleInterface* BlockSolverExampleCreateFunc(CommonExampleOptions& options) diff --git a/examples/BlockSolver/BlockSolverExample.h b/examples/BlockSolver/BlockSolverExample.h index 0b49e8f71..9d2c137be 100644 --- a/examples/BlockSolver/BlockSolverExample.h +++ b/examples/BlockSolver/BlockSolverExample.h @@ -9,7 +9,7 @@ enum BlockSolverOptions BLOCK_SOLVER_MLCP_DANTZIG = 1 << 2, BLOCK_SOLVER_BLOCK = 1 << 3, - BLOCK_SOLVER_SCENE_STACK= 1 << 5, + BLOCK_SOLVER_SCENE_MB_STACK= 1 << 5, BLOCK_SOLVER_SCENE_CHAIN = 1<< 6, }; diff --git a/examples/BlockSolver/btBlockSolver.cpp b/examples/BlockSolver/btBlockSolver.cpp index 14f5f45a8..4ada11333 100644 --- a/examples/BlockSolver/btBlockSolver.cpp +++ b/examples/BlockSolver/btBlockSolver.cpp @@ -128,7 +128,6 @@ btScalar btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, b int maxIterations = siData.m_maxOverrideNumSolverIterations > info.m_numIterations ? siData.m_maxOverrideNumSolverIterations : info.m_numIterations; for (int iteration = 0; iteration < maxIterations; iteration++) - //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) { leastSquaresResidual = btSequentialImpulseConstraintSolver::solveSingleIterationInternal(siData, iteration, constraints, numConstraints, info); @@ -150,7 +149,7 @@ btScalar btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, b void btBlockSolver::solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) { - //btMultiBodyConstraintSolver::solveMultiBodyGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, multiBodyConstraints, numMultiBodyConstraints, info, debugDrawer, dispatcher); + btMultiBodyConstraintSolver::solveMultiBodyGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, multiBodyConstraints, numMultiBodyConstraints, info, debugDrawer, dispatcher); } void btBlockSolver::reset() diff --git a/examples/ConstraintSolvers/BoxStacks.cpp b/examples/ConstraintSolvers/BoxStacks.cpp deleted file mode 100644 index d08c1459b..000000000 --- a/examples/ConstraintSolvers/BoxStacks.cpp +++ /dev/null @@ -1,335 +0,0 @@ -#include "BoxStacks.h" -#include "../OpenGLWindow/SimpleOpenGL3App.h" -#include "btBulletDynamicsCommon.h" - -#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" -#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" - -#include "BulletDynamics/Featherstone/btMultiBody.h" -#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" -#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h" -#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" -#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" -#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" -#include "BulletDynamics/Featherstone/btMultiBodyLink.h" -#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" -#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" -#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" -#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" -#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" - -#include "../OpenGLWindow/GLInstancingRenderer.h" -#include "BulletCollision/CollisionShapes/btShapeHull.h" - -#include "../CommonInterfaces/CommonMultiBodyBase.h" - -class BoxStacks : public CommonMultiBodyBase -{ -public: - BoxStacks(GUIHelperInterface* helper); - virtual ~BoxStacks(); - - virtual void initPhysics(); - - virtual void stepSimulation(float deltaTime); - - virtual void resetCamera() - { - float dist = 1; - float pitch = -35; - float yaw = 50; - float targetPos[3] = {-3, 2.8, -2.5}; - m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); - } - - void createBoxStack(int numBoxes, btScalar centerX, btScalar centerY); - btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool fixedBase = false); - void createGround(const btVector3& halfExtents = btVector3(50, 50, 50), btScalar zOffSet = btScalar(-1.55)); - void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents); -}; - -static bool g_fixedBase = true; -static bool g_firstInit = true; -static float scaling = 0.4f; -static float friction = 1.; -static int g_constraintSolverType = 0; - -BoxStacks::BoxStacks(GUIHelperInterface* helper) - : CommonMultiBodyBase(helper) -{ - m_guiHelper->setUpAxis(1); -} - -BoxStacks::~BoxStacks() -{ - // Do nothing -} - -void BoxStacks::stepSimulation(float deltaTime) -{ - //use a smaller internal timestep, there are stability issues - float internalTimeStep = 1. / 240.f; - m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep); -} - -void BoxStacks::initPhysics() -{ - m_guiHelper->setUpAxis(1); - - if (g_firstInit) - { - m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10. * scaling)); - m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50); - g_firstInit = false; - } - ///collision configuration contains default setup for memory, collision setup - m_collisionConfiguration = new btDefaultCollisionConfiguration(); - - ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) - m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); - - m_broadphase = new btDbvtBroadphase(); - - if (g_constraintSolverType == 3) - { - g_constraintSolverType = 0; - g_fixedBase = !g_fixedBase; - } - - btMLCPSolverInterface* mlcp; - switch (g_constraintSolverType++) - { - case 0: - m_solver = new btMultiBodyConstraintSolver; - b3Printf("Constraint Solver: Sequential Impulse"); - break; - case 1: - mlcp = new btSolveProjectedGaussSeidel(); - m_solver = new btMultiBodyMLCPConstraintSolver(mlcp); - b3Printf("Constraint Solver: MLCP + PGS"); - break; - default: - mlcp = new btDantzigSolver(); - m_solver = new btMultiBodyMLCPConstraintSolver(mlcp); - b3Printf("Constraint Solver: MLCP + Dantzig"); - break; - } - m_solver = new btMultiBodyBlockConstraintSolver(); - - btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); - m_dynamicsWorld = world; - m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - m_dynamicsWorld->setGravity(btVector3(btScalar(0), btScalar(-9.81), btScalar(0))); - m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good? - - /// Create a few basic rigid bodies - btVector3 groundHalfExtents(50, 50, 50); - btCollisionShape* groundShape = new btBoxShape(groundHalfExtents); - - m_collisionShapes.push_back(groundShape); - - btTransform groundTransform; - groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -50, 00)); - - btVector3 linkHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1)); - btVector3 baseHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1)); - - createBoxStack(1, 0, 0); - - btScalar groundHeight = btScalar(-51.55); - btScalar mass = btScalar(0.0); - - btVector3 localInertia(0, 0, 0); - groundShape->calculateLocalInertia(mass, localInertia); - - // Using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects - groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, groundHeight, 0)); - btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); - btRigidBody* body = new btRigidBody(rbInfo); - - // Add the body to the dynamics world - m_dynamicsWorld->addRigidBody(body, 1, 1 + 2); - - createGround(); - - m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); -} - -void BoxStacks::createBoxStack(int numBoxes, btScalar centerX, btScalar centerZ) -{ - //create a few dynamic rigidbodies - // Re-using the same collision is better for memory usage and performance - - const btScalar boxHalfSize = btScalar(0.1); - - btBoxShape* colShape = createBoxShape(btVector3(boxHalfSize, boxHalfSize, boxHalfSize)); - m_collisionShapes.push_back(colShape); - - /// Create Dynamic Objects - btTransform startTransform; - startTransform.setIdentity(); - - btScalar mass(1.0); - - btVector3 localInertia(0, 0, 0); - colShape->calculateLocalInertia(mass,localInertia); - - for (int i = 0; i < numBoxes; ++i) - { - startTransform.setOrigin(btVector3(centerX, 1+btScalar(btScalar(2) * boxHalfSize * i), centerZ)); - createRigidBody(mass, startTransform, colShape); - } -} - -btMultiBody* BoxStacks::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool fixedBase) -{ - //init the base - btVector3 baseInertiaDiag(0.f, 0.f, 0.f); - float baseMass = 1.f; - - if (baseMass) - { - btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); - pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); - delete pTempBox; - } - - bool canSleep = false; - - btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, fixedBase, canSleep); - - btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); - pMultiBody->setBasePos(basePosition); - pMultiBody->setWorldToBaseRot(baseOriQuat); - btVector3 vel(0, 0, 0); - - //init the links - btVector3 hingeJointAxis(1, 0, 0); - float linkMass = 1.f; - btVector3 linkInertiaDiag(0.f, 0.f, 0.f); - - btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); - pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); - delete pTempBox; - - //y-axis assumed up - btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset - btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset - btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset - - ////// - btScalar q0 = 0.f * SIMD_PI / 180.f; - btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); - quat0.normalize(); - ///// - - for (int i = 0; i < numLinks; ++i) - { - if (!spherical) - pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true); - else - //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); - pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true); - } - - pMultiBody->finalizeMultiDof(); - - /// - pWorld->addMultiBody(pMultiBody); - /// - return pMultiBody; -} - -void BoxStacks::createGround(const btVector3& halfExtents, btScalar zOffSet) -{ - btCollisionShape* groundShape = new btBoxShape(halfExtents); - m_collisionShapes.push_back(groundShape); - - // rigidbody is dynamic if and only if mass is non zero, otherwise static - btScalar mass(0.); - const bool isDynamic = (mass != 0.f); - - btVector3 localInertia(0, 0, 0); - if (isDynamic) - groundShape->calculateLocalInertia(mass, localInertia); - - // using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects - btTransform groundTransform; - groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -halfExtents.z() + zOffSet, 0)); - btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); - btRigidBody* body = new btRigidBody(rbInfo); - - // add the body to the dynamics world - m_dynamicsWorld->addRigidBody(body, 1, 1 + 2); -} - -void BoxStacks::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) -{ - btAlignedObjectArray world_to_local; - world_to_local.resize(pMultiBody->getNumLinks() + 1); - - btAlignedObjectArray local_origin; - local_origin.resize(pMultiBody->getNumLinks() + 1); - world_to_local[0] = pMultiBody->getWorldToBaseRot(); - local_origin[0] = pMultiBody->getBasePos(); - - { - btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; - - if (1) - { - btCollisionShape* box = new btBoxShape(baseHalfExtents); - btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); - col->setCollisionShape(box); - - btTransform tr; - tr.setIdentity(); - tr.setOrigin(local_origin[0]); - tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); - col->setWorldTransform(tr); - - pWorld->addCollisionObject(col, 2, 1 + 2); - - col->setFriction(friction); - pMultiBody->setBaseCollider(col); - } - } - - for (int i = 0; i < pMultiBody->getNumLinks(); ++i) - { - const int parent = pMultiBody->getParent(i); - world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1]; - local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i))); - } - - for (int i = 0; i < pMultiBody->getNumLinks(); ++i) - { - btVector3 posr = local_origin[i + 1]; - - btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()}; - - btCollisionShape* box = new btBoxShape(linkHalfExtents); - btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); - - col->setCollisionShape(box); - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); - col->setWorldTransform(tr); - col->setFriction(friction); - pWorld->addCollisionObject(col, 2, 1 + 2); - - pMultiBody->getLink(i).m_collider = col; - } -} - -CommonExampleInterface* BoxStacksCreateFunc(CommonExampleOptions& options) -{ - return new BoxStacks(options.m_guiHelper); -} diff --git a/examples/ConstraintSolvers/BoxStacks.h b/examples/ConstraintSolvers/BoxStacks.h deleted file mode 100644 index 1020c3093..000000000 --- a/examples/ConstraintSolvers/BoxStacks.h +++ /dev/null @@ -1,7 +0,0 @@ - -#ifndef CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H -#define CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H - -class CommonExampleInterface* BoxStacksCreateFunc(struct CommonExampleOptions& options); - -#endif // CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H diff --git a/examples/ConstraintSolvers/BoxStacks_MLCP.cpp b/examples/ConstraintSolvers/BoxStacks_MLCP.cpp deleted file mode 100644 index 14bff8a5e..000000000 --- a/examples/ConstraintSolvers/BoxStacks_MLCP.cpp +++ /dev/null @@ -1,224 +0,0 @@ -#include "BoxStacks_MLCP.h" -#include "../OpenGLWindow/SimpleOpenGL3App.h" -#include "btBulletDynamicsCommon.h" -#include "BulletDynamics/Featherstone/btMultiBody.h" -#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" -#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h" -#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" -#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" -#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" -#include "../CommonInterfaces/CommonMultiBodyBase.h" -#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" -#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" -#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" -#include "../Importers/ImportURDFDemo/URDF2Bullet.h" -#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h" -#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" -#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" - - -class BoxStacks_MLCP : public CommonMultiBodyBase -{ -public: - BoxStacks_MLCP(GUIHelperInterface* helper); - virtual ~BoxStacks_MLCP(); - - virtual void initPhysics(); - - virtual void stepSimulation(float deltaTime); - - virtual void resetCamera() - { - float dist = 2; - float pitch = -35; - float yaw = 50; - float targetPos[3] = {0, 0, 0}; - m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); - } - - btMultiBody* createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape); - - btMultiBody* loadRobot(std::string filepath = "kuka_iiwa/model.urdf"); -}; - - - -static int g_constraintSolverType = 0; - -BoxStacks_MLCP::BoxStacks_MLCP(GUIHelperInterface* helper) - : CommonMultiBodyBase(helper) -{ - -} - -BoxStacks_MLCP::~BoxStacks_MLCP() -{ - // Do nothing -} - -void BoxStacks_MLCP::stepSimulation(float deltaTime) -{ - float internalTimeStep = 1. / 240.f; - m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep); - for (int i = 0; i < m_dynamicsWorld->getNumMultibodies(); i++) - { - btVector3 pos = m_dynamicsWorld->getMultiBody(i)->getBaseWorldTransform().getOrigin(); - printf("pos[%d]=%f,%f,%f\n", i, pos.x(), pos.y(), pos.z()); - } -} - -void BoxStacks_MLCP::initPhysics() -{ - m_guiHelper->setUpAxis(2); - - createEmptyDynamicsWorld(); - m_dynamicsWorld->getSolverInfo().m_numIterations = 50; - if (g_constraintSolverType == 5) - { - g_constraintSolverType = 0; - } - - btMultiBodyConstraintSolver* sol = 0; - - btMLCPSolverInterface* mlcp; - switch (g_constraintSolverType++) - { - case 0: - sol = new btMultiBodyConstraintSolver; - b3Printf("Constraint Solver: Sequential Impulse"); - break; - case 1: - mlcp = new btSolveProjectedGaussSeidel(); - sol = new btMultiBodyMLCPConstraintSolver(mlcp); - b3Printf("Constraint Solver: MLCP + PGS"); - break; - case 2: - mlcp = new btDantzigSolver(); - sol = new btMultiBodyMLCPConstraintSolver(mlcp); - b3Printf("Constraint Solver: MLCP + Dantzig"); - break; - case 3: - mlcp = new btLemkeSolver(); - sol = new btMultiBodyMLCPConstraintSolver(mlcp); - - b3Printf("Constraint Solver: MLCP + Lemke"); - break; - - default: - sol = new btMultiBodyBlockConstraintSolver(); - b3Printf("btMultiBodyBlockConstraintSolver"); - break; - } - - m_solver = sol; - - btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration); - m_dynamicsWorld = world; - m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); - - - m_dynamicsWorld->setGravity(btVector3(0,0,-10)); - m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - - if (m_dynamicsWorld->getDebugDrawer()) - m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); - - ///create a few basic rigid bodies - bool loadPlaneFromURDF = true; - if (loadPlaneFromURDF) - { - loadRobot("plane.urdf"); - } else - { - btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.), btScalar(50.), btScalar(50.))); - m_collisionShapes.push_back(groundShape); - btScalar mass = 0; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(btVector3(0, 0, -50)); - btMultiBody* body = createMultiBody(mass, tr, groundShape); - } - - { - btBoxShape* boxShape = createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1))); - m_collisionShapes.push_back(boxShape); - btScalar mass = 10; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(btVector3(0, 0, 0.5)); - btMultiBody* body = createMultiBody(mass, tr, boxShape); - } - - { - btMultiBody* mb = loadRobot("cube_small.urdf"); - btTransform tr; - tr.setIdentity(); - tr.setOrigin(btVector3(0, 0, 1.)); - mb->setBaseWorldTransform(tr); - } - m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); -} - - - -btMultiBody* BoxStacks_MLCP::createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape) -{ - btVector3 inertia; - collisionShape->calculateLocalInertia(mass, inertia); - - bool canSleep = false; - bool isDynamic = mass > 0; - btMultiBody* mb = new btMultiBody(0, mass, inertia, !isDynamic,canSleep); - btMultiBodyLinkCollider* collider = new btMultiBodyLinkCollider(mb, -1); - collider->setWorldTransform(trans); - mb->setBaseWorldTransform(trans); - - collider->setCollisionShape(collisionShape); - - int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); - int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - - this->m_dynamicsWorld->addCollisionObject(collider, collisionFilterGroup, collisionFilterMask); - mb->setBaseCollider(collider); - - mb->finalizeMultiDof(); - - - this->m_dynamicsWorld->addMultiBody(mb); - m_dynamicsWorld->forwardKinematics(); - return mb; -} - - - -btMultiBody*BoxStacks_MLCP::loadRobot(std::string filepath) -{ - btMultiBody* m_multiBody = 0; - BulletURDFImporter u2b(m_guiHelper,0,0,1,0); - bool loadOk = u2b.loadURDF(filepath.c_str());// lwr / kuka.urdf"); - if (loadOk) - { - int rootLinkIndex = u2b.getRootLinkIndex(); - b3Printf("urdf root link index = %d\n",rootLinkIndex); - MyMultiBodyCreator creation(m_guiHelper); - btTransform identityTrans; - identityTrans.setIdentity(); - ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix()); - for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++) - { - m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i)); - } - m_multiBody = creation.getBulletMultiBody(); - if (m_multiBody) - { - b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex()).c_str()); - } - } - return m_multiBody; -} - -CommonExampleInterface* BoxStacks_MLCPCreateFunc(CommonExampleOptions& options) -{ - return new BoxStacks_MLCP(options.m_guiHelper); -} diff --git a/examples/ConstraintSolvers/BoxStacks_MLCP.h b/examples/ConstraintSolvers/BoxStacks_MLCP.h deleted file mode 100644 index 994fb1d92..000000000 --- a/examples/ConstraintSolvers/BoxStacks_MLCP.h +++ /dev/null @@ -1,7 +0,0 @@ - -#ifndef CONSTRAINT_SOLVERS_BOX_STACKS_MLCP_DEMO_H -#define CONSTRAINT_SOLVERS_BOX_STACKS_MLCP_DEMO_H - -class CommonExampleInterface* BoxStacks_MLCPCreateFunc(struct CommonExampleOptions& options); - -#endif // CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H diff --git a/examples/ConstraintSolvers/Grasp_Block.cpp b/examples/ConstraintSolvers/Grasp_Block.cpp deleted file mode 100644 index 81dd7ffcd..000000000 --- a/examples/ConstraintSolvers/Grasp_Block.cpp +++ /dev/null @@ -1,335 +0,0 @@ -#include "Grasp_Block.h" -#include "../OpenGLWindow/SimpleOpenGL3App.h" -#include "btBulletDynamicsCommon.h" - -#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" -#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" - -#include "BulletDynamics/Featherstone/btMultiBody.h" -#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" -#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h" -#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" -#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" -#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" -#include "BulletDynamics/Featherstone/btMultiBodyLink.h" -#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" -#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" -#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" -#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" -#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" - -#include "../OpenGLWindow/GLInstancingRenderer.h" -#include "BulletCollision/CollisionShapes/btShapeHull.h" - -#include "../CommonInterfaces/CommonMultiBodyBase.h" - -class Grasp_Block : public CommonMultiBodyBase -{ -public: - Grasp_Block(GUIHelperInterface* helper); - virtual ~Grasp_Block(); - - virtual void initPhysics(); - - virtual void stepSimulation(float deltaTime); - - virtual void resetCamera() - { - float dist = 1; - float pitch = -35; - float yaw = 50; - float targetPos[3] = {-3, 2.8, -2.5}; - m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); - } - - void createBoxStack(int numBoxes, btScalar centerX, btScalar centerY); - btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool fixedBase = false); - void createGround(const btVector3& halfExtents = btVector3(50, 50, 50), btScalar zOffSet = btScalar(-1.55)); - void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents); -}; - -static bool g_fixedBase = true; -static bool g_firstInit = true; -static float scaling = 0.4f; -static float friction = 1.; -static int g_constraintSolverType = 0; - -Grasp_Block::Grasp_Block(GUIHelperInterface* helper) - : CommonMultiBodyBase(helper) -{ - m_guiHelper->setUpAxis(1); -} - -Grasp_Block::~Grasp_Block() -{ - // Do nothing -} - -void Grasp_Block::stepSimulation(float deltaTime) -{ - //use a smaller internal timestep, there are stability issues - float internalTimeStep = 1. / 240.f; - m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep); -} - -void Grasp_Block::initPhysics() -{ - m_guiHelper->setUpAxis(1); - - if (g_firstInit) - { - m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10. * scaling)); - m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50); - g_firstInit = false; - } - ///collision configuration contains default setup for memory, collision setup - m_collisionConfiguration = new btDefaultCollisionConfiguration(); - - ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) - m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); - - m_broadphase = new btDbvtBroadphase(); - - if (g_constraintSolverType == 3) - { - g_constraintSolverType = 0; - g_fixedBase = !g_fixedBase; - } - - btMLCPSolverInterface* mlcp; - switch (g_constraintSolverType++) - { - case 0: - m_solver = new btMultiBodyConstraintSolver; - b3Printf("Constraint Solver: Sequential Impulse"); - break; - case 1: - mlcp = new btSolveProjectedGaussSeidel(); - m_solver = new btMultiBodyMLCPConstraintSolver(mlcp); - b3Printf("Constraint Solver: MLCP + PGS"); - break; - default: - mlcp = new btDantzigSolver(); - m_solver = new btMultiBodyMLCPConstraintSolver(mlcp); - b3Printf("Constraint Solver: MLCP + Dantzig"); - break; - } - m_solver = new btMultiBodyBlockConstraintSolver(); - - btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); - m_dynamicsWorld = world; - m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - m_dynamicsWorld->setGravity(btVector3(btScalar(0), btScalar(-9.81), btScalar(0))); - m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good? - - /// Create a few basic rigid bodies - btVector3 groundHalfExtents(50, 50, 50); - btCollisionShape* groundShape = new btBoxShape(groundHalfExtents); - - m_collisionShapes.push_back(groundShape); - - btTransform groundTransform; - groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -50, 00)); - - btVector3 linkHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1)); - btVector3 baseHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1)); - -// createBoxStack(5, 0, 0); - - btScalar groundHeight = btScalar(-51.55); - btScalar mass = btScalar(0.0); - - btVector3 localInertia(0, 0, 0); - groundShape->calculateLocalInertia(mass, localInertia); - - // Using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects - groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, groundHeight, 0)); - btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); - btRigidBody* body = new btRigidBody(rbInfo); - - // Add the body to the dynamics world - m_dynamicsWorld->addRigidBody(body, 1, 1 + 2); - - createGround(); - - m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); -} - -void Grasp_Block::createBoxStack(int numBoxes, btScalar centerX, btScalar centerZ) -{ - //create a few dynamic rigidbodies - // Re-using the same collision is better for memory usage and performance - - const btScalar boxHalfSize = btScalar(0.1); - - btBoxShape* colShape = createBoxShape(btVector3(boxHalfSize, boxHalfSize, boxHalfSize)); - m_collisionShapes.push_back(colShape); - - /// Create Dynamic Objects - btTransform startTransform; - startTransform.setIdentity(); - - btScalar mass(1.0); - - btVector3 localInertia(0, 0, 0); - colShape->calculateLocalInertia(mass,localInertia); - - for (int i = 0; i < numBoxes; ++i) - { - startTransform.setOrigin(btVector3(centerX, btScalar(btScalar(2) * boxHalfSize * i), centerZ)); - createRigidBody(mass, startTransform, colShape); - } -} - -btMultiBody* Grasp_Block::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool fixedBase) -{ - //init the base - btVector3 baseInertiaDiag(0.f, 0.f, 0.f); - float baseMass = 1.f; - - if (baseMass) - { - btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); - pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); - delete pTempBox; - } - - bool canSleep = false; - - btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, fixedBase, canSleep); - - btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); - pMultiBody->setBasePos(basePosition); - pMultiBody->setWorldToBaseRot(baseOriQuat); - btVector3 vel(0, 0, 0); - - //init the links - btVector3 hingeJointAxis(1, 0, 0); - float linkMass = 1.f; - btVector3 linkInertiaDiag(0.f, 0.f, 0.f); - - btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); - pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); - delete pTempBox; - - //y-axis assumed up - btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset - btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset - btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset - - ////// - btScalar q0 = 0.f * SIMD_PI / 180.f; - btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); - quat0.normalize(); - ///// - - for (int i = 0; i < numLinks; ++i) - { - if (!spherical) - pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true); - else - //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); - pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true); - } - - pMultiBody->finalizeMultiDof(); - - /// - pWorld->addMultiBody(pMultiBody); - /// - return pMultiBody; -} - -void Grasp_Block::createGround(const btVector3& halfExtents, btScalar zOffSet) -{ - btCollisionShape* groundShape = new btBoxShape(halfExtents); - m_collisionShapes.push_back(groundShape); - - // rigidbody is dynamic if and only if mass is non zero, otherwise static - btScalar mass(0.); - const bool isDynamic = (mass != 0.f); - - btVector3 localInertia(0, 0, 0); - if (isDynamic) - groundShape->calculateLocalInertia(mass, localInertia); - - // using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects - btTransform groundTransform; - groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -halfExtents.z() + zOffSet, 0)); - btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); - btRigidBody* body = new btRigidBody(rbInfo); - - // add the body to the dynamics world - m_dynamicsWorld->addRigidBody(body, 1, 1 + 2); -} - -void Grasp_Block::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) -{ - btAlignedObjectArray world_to_local; - world_to_local.resize(pMultiBody->getNumLinks() + 1); - - btAlignedObjectArray local_origin; - local_origin.resize(pMultiBody->getNumLinks() + 1); - world_to_local[0] = pMultiBody->getWorldToBaseRot(); - local_origin[0] = pMultiBody->getBasePos(); - - { - btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; - - if (1) - { - btCollisionShape* box = new btBoxShape(baseHalfExtents); - btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); - col->setCollisionShape(box); - - btTransform tr; - tr.setIdentity(); - tr.setOrigin(local_origin[0]); - tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); - col->setWorldTransform(tr); - - pWorld->addCollisionObject(col, 2, 1 + 2); - - col->setFriction(friction); - pMultiBody->setBaseCollider(col); - } - } - - for (int i = 0; i < pMultiBody->getNumLinks(); ++i) - { - const int parent = pMultiBody->getParent(i); - world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1]; - local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i))); - } - - for (int i = 0; i < pMultiBody->getNumLinks(); ++i) - { - btVector3 posr = local_origin[i + 1]; - - btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()}; - - btCollisionShape* box = new btBoxShape(linkHalfExtents); - btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); - - col->setCollisionShape(box); - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); - col->setWorldTransform(tr); - col->setFriction(friction); - pWorld->addCollisionObject(col, 2, 1 + 2); - - pMultiBody->getLink(i).m_collider = col; - } -} - -CommonExampleInterface* Grasp_BlockCreateFunc(CommonExampleOptions& options) -{ - return new Grasp_Block(options.m_guiHelper); -} diff --git a/examples/ConstraintSolvers/Grasp_Block.h b/examples/ConstraintSolvers/Grasp_Block.h deleted file mode 100644 index ffed47269..000000000 --- a/examples/ConstraintSolvers/Grasp_Block.h +++ /dev/null @@ -1,7 +0,0 @@ - -#ifndef CONSTRAINT_SOLVERS_GRASP_BLOCK_DEMO_H -#define CONSTRAINT_SOLVERS_GRASP_BLOCK_DEMO_H - -class CommonExampleInterface* Grasp_BlockCreateFunc(struct CommonExampleOptions& options); - -#endif // CONSTRAINT_SOLVERS_GRASP_BLOCK_DEMO_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index cd87eb92a..2678fa1b9 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -202,10 +202,6 @@ SET(BulletExampleBrowser_SRCS ../MultiThreadedDemo/MultiThreadedDemo.h ../MultiThreadedDemo/CommonRigidBodyMTBase.cpp ../MultiThreadedDemo/CommonRigidBodyMTBase.h - ../ConstraintSolvers/BoxStacks.cpp - ../ConstraintSolvers/BoxStacks_MLCP.cpp - ../ConstraintSolvers/Grasp_Block.cpp - ../ConstraintSolvers/SerialChains.cpp ../BlockSolver/btBlockSolver.cpp ../BlockSolver/btBlockSolver.h ../BlockSolver/BlockSolverExample.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index c055cbeea..02789d034 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -21,10 +21,6 @@ #include "../Importers/ImportSDFDemo/ImportSDFSetup.h" #include "../Importers/ImportMJCFDemo/ImportMJCFSetup.h" #include "../Collision/CollisionTutorialBullet2.h" -#include "../ConstraintSolvers/SerialChains.h" -#include "../ConstraintSolvers/BoxStacks.h" -#include "../ConstraintSolvers/BoxStacks_MLCP.h" -#include "../ConstraintSolvers/Grasp_Block.h" #include "../GyroscopicDemo/GyroscopicSetup.h" #include "../Constraints/Dof6Spring2Setup.h" #include "../Constraints/ConstraintPhysicsSetup.h" @@ -142,13 +138,7 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Inverted Pendulum PD", "Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc), ExampleEntry(1, "MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", MultiBodySoftContactCreateFunc, 0), - ExampleEntry(0, "Constraint Solvers"), - ExampleEntry(1, "Serial Chains", "Show colliding two serial chains using different constraint solvers.", SerialChainsCreateFunc, 0), - ExampleEntry(1, "Box Stack", "Show box stacks with different constraint solvers for each stack.", BoxStacksCreateFunc, 0), - ExampleEntry(1, "Box Stack MLCP", "Show box stacks with different constraint solvers for each stack.", BoxStacks_MLCPCreateFunc, 0), - ExampleEntry(1, "Grasp Block", "Show box stacks with different constraint solvers for each stack.", Grasp_BlockCreateFunc, 0), - - + ExampleEntry(0, "Physics Client-Server"), ExampleEntry(1, "Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.", PhysicsServerCreateFuncBullet2), @@ -163,10 +153,10 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(0, "BlockSolver"), - ExampleEntry(1, "Stack MultiBody SI", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_STACK+ BLOCK_SOLVER_SI), - ExampleEntry(1, "Stack MultiBody MLCP PGS", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_STACK + BLOCK_SOLVER_MLCP_PGS), - ExampleEntry(1, "Stack MultiBody MLCP Dantzig", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_STACK + BLOCK_SOLVER_MLCP_DANTZIG), - ExampleEntry(1, "Stack MultiBody Block", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_STACK + BLOCK_SOLVER_BLOCK), + ExampleEntry(1, "Stack MultiBody SI", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK+ BLOCK_SOLVER_SI), + ExampleEntry(1, "Stack MultiBody MLCP PGS", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_PGS), + ExampleEntry(1, "Stack MultiBody MLCP Dantzig", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_DANTZIG), + ExampleEntry(1, "Stack MultiBody Block", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_BLOCK), ExampleEntry(0, "Inverse Dynamics"), ExampleEntry(1, "Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc, BT_ID_LOAD_URDF), diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index c610632d1..222ad911e 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -163,7 +163,6 @@ project "App_BulletExampleBrowser" "../Collision/*", "../RoboticsLearning/*", "../BlockSolver/*", - "../ConstraintSolvers/*", "../Collision/Internal/*", "../Benchmarks/*", "../MultiThreadedDemo/*", diff --git a/examples/ConstraintSolvers/SerialChains.cpp b/examples/MultiBody/SerialChains.cpp similarity index 88% rename from examples/ConstraintSolvers/SerialChains.cpp rename to examples/MultiBody/SerialChains.cpp index 16ff358a5..b582f0f0f 100644 --- a/examples/ConstraintSolvers/SerialChains.cpp +++ b/examples/MultiBody/SerialChains.cpp @@ -7,7 +7,6 @@ #include "BulletDynamics/Featherstone/btMultiBody.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" -#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" @@ -113,13 +112,12 @@ void SerialChains::initPhysics() b3Printf("Constraint Solver: MLCP + Dantzig"); break; } - m_solver = new btMultiBodyBlockConstraintSolver(); btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); m_dynamicsWorld = world; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); - m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good? + m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good? ///create a few basic rigid bodies btVector3 groundHalfExtents(50, 50, 50); @@ -239,53 +237,6 @@ void SerialChains::initPhysics() createGround(); - { - btVector3 halfExtents(.5,.5,.5); - btBoxShape* colShape = new btBoxShape(halfExtents); - //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); - m_collisionShapes.push_back(colShape); - - /// Create Dynamic Objects - btTransform startTransform; - startTransform.setIdentity(); - - btScalar mass(1.f); - - //rigidbody is dynamic if and only if mass is non zero, otherwise static - bool isDynamic = (mass != 0.f); - - btVector3 localInertia(0,0,0); - if (isDynamic) - colShape->calculateLocalInertia(mass,localInertia); - - startTransform.setOrigin(btVector3( - btScalar(0.0), - 0.0, - btScalar(0.0))); - - - //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects - btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia); -// btRigidBody* body = new btRigidBody(rbInfo); - -// m_dynamicsWorld->addRigidBody(body);//,1,1+2); - - { - btVector3 pointInA = -linkHalfExtents; - // btVector3 pointInB = halfExtents; - btMatrix3x3 frameInA; - btMatrix3x3 frameInB; - frameInA.setIdentity(); - frameInB.setIdentity(); - btVector3 jointAxis(1.0,0.0,0.0); - //btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis); - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(mbC1, numLinks- 1 , mbC2, numLinks - 1, pointInA, pointInA); - p2p->setMaxAppliedImpulse(20.0); - m_dynamicsWorld->addMultiBodyConstraint(p2p); - } - } - m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); ///////////////////////////////////////////////////////////////// diff --git a/examples/ConstraintSolvers/SerialChains.h b/examples/MultiBody/SerialChains.h similarity index 100% rename from examples/ConstraintSolvers/SerialChains.h rename to examples/MultiBody/SerialChains.h diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index 68459578c..3332440f2 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -32,7 +32,6 @@ SET(BulletDynamics_SRCS Vehicle/btWheelInfo.cpp Featherstone/btMultiBody.cpp Featherstone/btMultiBodyConstraint.cpp - Featherstone/btMultiBodyBlockConstraintSolver.cpp Featherstone/btMultiBodyConstraintSolver.cpp Featherstone/btMultiBodyDynamicsWorld.cpp Featherstone/btMultiBodyFixedConstraint.cpp diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 5c76918eb..9f76713db 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -649,15 +649,15 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, - colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); + colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); return solverConstraint; } void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, - btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, - btScalar desiredVelocity, btScalar cfmSlip) + btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, + btScalar desiredVelocity, btScalar cfmSlip) { btVector3 normalAxis(0, 0, 0); @@ -724,7 +724,7 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSol btScalar desiredVelocity, btScalar cfmSlip) { - setupTorsionalFrictionConstraintInternal(m_tmpSolverBodyPool, solverConstraint, normalAxis1,solverBodyIdA, solverBodyIdB, + setupTorsionalFrictionConstraintInternal(m_tmpSolverBodyPool, solverConstraint, normalAxis1, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); @@ -746,7 +746,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionCon btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2, - colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); + colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); return solverConstraint; } @@ -1067,11 +1067,11 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& void btSequentialImpulseConstraintSolver::setupContactConstraintInternal(btSISolverSingleIterationData& siData, - btSolverConstraint& solverConstraint, - int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - const btVector3& rel_pos1, const btVector3& rel_pos2) + btSolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + const btVector3& rel_pos1, const btVector3& rel_pos2) { // const btVector3& pos1 = cp.getPositionWorldOnA(); // const btVector3& pos2 = cp.getPositionWorldOnB(); @@ -1198,7 +1198,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraintInternal(btSISol if (rb0) bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() , -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse); + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse); } else { @@ -1274,7 +1274,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra m_btSeed2, m_fixedBodyId, m_maxOverrideNumSolverIterations - ); + ); setupContactConstraintInternal(siData, solverConstraint, @@ -1285,10 +1285,10 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra } -void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulseInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, - btSolverConstraint& solverConstraint, - int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulseInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, + btSolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) { btSolverBody* bodyA = &tmpSolverBodyPool[solverBodyIdA]; btSolverBody* bodyB = &tmpSolverBodyPool[solverBodyIdB]; @@ -1302,9 +1302,9 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulseInternal(b { frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; if (rb0) - bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1 * rb0->getInvMass() , frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse); + bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1 * rb0->getInvMass(), frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse); if (rb1) - bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2 * rb1->getInvMass() , -frictionConstraint1.m_angularComponentB, -(btScalar)frictionConstraint1.m_appliedImpulse); + bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2 * rb1->getInvMass(), -frictionConstraint1.m_angularComponentB, -(btScalar)frictionConstraint1.m_appliedImpulse); } else { @@ -1419,13 +1419,13 @@ void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingl if (axis0.length() > 0.001) { btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool, - siData.m_tmpSolverContactRollingFrictionConstraintPool,axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp, + siData.m_tmpSolverContactRollingFrictionConstraintPool, axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); } if (axis1.length() > 0.001) { btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool, - siData.m_tmpSolverContactRollingFrictionConstraintPool,axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, + siData.m_tmpSolverContactRollingFrictionConstraintPool, axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); } } @@ -1456,7 +1456,7 @@ void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingl cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel); applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); - btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) @@ -1494,7 +1494,7 @@ void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingl } else { - btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) @@ -1545,14 +1545,14 @@ void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** } } -void btSequentialImpulseConstraintSolver::convertJointInternal(btAlignedObjectArray& tmpSolverBodyPool, - int& maxOverrideNumSolverIterations, - btSolverConstraint* currentConstraintRow, - btTypedConstraint* constraint, - const btTypedConstraint::btConstraintInfo1& info1, - int solverBodyIdA, - int solverBodyIdB, - const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::convertJointInternal(btAlignedObjectArray& tmpSolverBodyPool, + int& maxOverrideNumSolverIterations, + btSolverConstraint* currentConstraintRow, + btTypedConstraint* constraint, + const btTypedConstraint::btConstraintInfo1& info1, + int solverBodyIdA, + int solverBodyIdB, + const btContactSolverInfo& infoGlobal) { const btRigidBody& rbA = constraint->getRigidBodyA(); const btRigidBody& rbB = constraint->getRigidBodyB(); @@ -1602,7 +1602,7 @@ void btSequentialImpulseConstraintSolver::convertJointInternal(btAlignedObjectAr info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2; info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; info2.rowskip = sizeof(btSolverConstraint) / sizeof(btScalar); //check this - ///the size of btSolverConstraint needs be a multiple of btScalar + ///the size of btSolverConstraint needs be a multiple of btScalar btAssert(info2.rowskip * sizeof(btScalar) == sizeof(btSolverConstraint)); info2.m_constraintError = ¤tConstraintRow->m_rhs; currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; @@ -1747,7 +1747,7 @@ void btSequentialImpulseConstraintSolver::convertJointsInternal(btSISolverSingle int solverBodyIdB = siData.getOrInitSolverBody(rbB, infoGlobal.m_timeStep); convertJointInternal(siData.m_tmpSolverBodyPool, siData.m_maxOverrideNumSolverIterations, - currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal); + currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal); } currentRow += info1.m_numConstraintRows; } @@ -1844,16 +1844,7 @@ void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodi convertBodiesInternal(siData, bodies, numBodies, infoGlobal); } - btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) -{ - solveGroupConvertConstraintPrestep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - solveGroupConvertConstraints(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - solveGroupConvertConstraintPoststep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - return 0.; -} - -btScalar btSequentialImpulseConstraintSolver::solveGroupConvertConstraints(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { m_fixedBodyId = -1; BT_PROFILE("solveGroupCacheFriendlySetup"); @@ -1943,17 +1934,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupConvertConstraints(btCol convertContacts(manifoldPtr, numManifolds, infoGlobal); - - return 0.f; -} - -btScalar btSequentialImpulseConstraintSolver::solveGroupConvertConstraintPrestep(btCollisionObject** /*bodies*/, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** /*constraints*/, int /*numConstraints*/, const btContactSolverInfo& /*infoGlobal*/, btIDebugDraw* /*debugDrawer*/) -{ - return 0; -} - -btScalar btSequentialImpulseConstraintSolver::solveGroupConvertConstraintPoststep(btCollisionObject** /*bodies*/, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** /*constraints*/, int /*numConstraints*/, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/) -{ // btContactSolverInfo info = infoGlobal; int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); @@ -1984,8 +1964,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupConvertConstraintPostste } } - return 0; + return 0.f; } + btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) { BT_PROFILE("solveSingleIteration"); @@ -2013,7 +1994,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSIS for (int j = 0; j < numConstraintPool; ++j) { int tmp = siData.m_orderTmpConstraintPool[j]; - int swapi = btRandInt2a(j + 1,siData.m_seed); + int swapi = btRandInt2a(j + 1, siData.m_seed); siData.m_orderTmpConstraintPool[j] = siData.m_orderTmpConstraintPool[swapi]; siData.m_orderTmpConstraintPool[swapi] = tmp; } @@ -2021,7 +2002,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSIS for (int j = 0; j < numFrictionPool; ++j) { int tmp = siData.m_orderFrictionConstraintPool[j]; - int swapi = btRandInt2a(j + 1,siData.m_seed); + int swapi = btRandInt2a(j + 1, siData.m_seed); siData.m_orderFrictionConstraintPool[j] = siData.m_orderFrictionConstraintPool[swapi]; siData.m_orderFrictionConstraintPool[swapi] = tmp; } @@ -2161,21 +2142,21 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSIS btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/) { btSISolverSingleIterationData siData(m_tmpSolverBodyPool, - m_tmpSolverContactConstraintPool, - m_tmpSolverNonContactConstraintPool, - m_tmpSolverContactFrictionConstraintPool, - m_tmpSolverContactRollingFrictionConstraintPool, - m_orderTmpConstraintPool, - m_orderNonContactConstraintPool, - m_orderFrictionConstraintPool, - m_tmpConstraintSizesPool, - m_resolveSingleConstraintRowGeneric, - m_resolveSingleConstraintRowLowerLimit, - m_resolveSplitPenetrationImpulse, - m_kinematicBodyUniqueIdToSolverBodyTable, - m_btSeed2, - m_fixedBodyId, - m_maxOverrideNumSolverIterations); + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); btScalar leastSquaresResidual = btSequentialImpulseConstraintSolver::solveSingleIterationInternal(siData, iteration, constraints, numConstraints, infoGlobal); @@ -2249,7 +2230,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; for (int iteration = 0; iteration < maxIterations; iteration++) - //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) + //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) { m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); @@ -2321,7 +2302,7 @@ void btSequentialImpulseConstraintSolver::writeBackJointsInternal(btConstraintAr void btSequentialImpulseConstraintSolver::writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { - writeBackBodiesInternal(m_tmpSolverBodyPool,iBegin, iEnd, infoGlobal); + writeBackBodiesInternal(m_tmpSolverBodyPool, iBegin, iEnd, infoGlobal); } void btSequentialImpulseConstraintSolver::writeBackBodiesInternal(btAlignedObjectArray& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { @@ -2357,11 +2338,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInter if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { - writeBackContactsInternal(siData.m_tmpSolverContactConstraintPool,siData.m_tmpSolverContactFrictionConstraintPool,0, siData.m_tmpSolverContactConstraintPool.size(), infoGlobal); + writeBackContactsInternal(siData.m_tmpSolverContactConstraintPool, siData.m_tmpSolverContactFrictionConstraintPool, 0, siData.m_tmpSolverContactConstraintPool.size(), infoGlobal); } writeBackJointsInternal(siData.m_tmpSolverNonContactConstraintPool, 0, siData.m_tmpSolverNonContactConstraintPool.size(), infoGlobal); - writeBackBodiesInternal(siData.m_tmpSolverBodyPool,0, siData.m_tmpSolverBodyPool.size(), infoGlobal); + writeBackBodiesInternal(siData.m_tmpSolverBodyPool, 0, siData.m_tmpSolverBodyPool.size(), infoGlobal); siData.m_tmpSolverContactConstraintPool.resizeNoInitialize(0); siData.m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); @@ -2412,34 +2393,4 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod void btSequentialImpulseConstraintSolver::reset() { m_btSeed2 = 0; -} - -btScalar btSequentialImpulseConstraintSolver::solveGroupConvertBackPrestep(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) -{ - return btScalar(0); -} - -btScalar btSequentialImpulseConstraintSolver::solveGroupConvertBack(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) -{ - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal); - } - - writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal); - writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal); - - return btScalar(0); -} - -btScalar btSequentialImpulseConstraintSolver::solveGroupConvertBackPoststep(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) -{ - m_tmpSolverContactConstraintPool.resizeNoInitialize(0); - m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); - m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); - m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); - - m_tmpSolverBodyPool.resizeNoInitialize(0); - - return btScalar(0); -} +} \ No newline at end of file diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 6eacf043b..aca1d0988 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -52,25 +52,25 @@ struct btSISolverSingleIterationData int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep); static void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep); int getSolverBody(btCollisionObject& body) const; - + btSISolverSingleIterationData(btAlignedObjectArray& tmpSolverBodyPool, - btConstraintArray& tmpSolverContactConstraintPool, - btConstraintArray& tmpSolverNonContactConstraintPool, - btConstraintArray& tmpSolverContactFrictionConstraintPool, - btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, - btAlignedObjectArray& orderTmpConstraintPool, - btAlignedObjectArray& orderNonContactConstraintPool, - btAlignedObjectArray& orderFrictionConstraintPool, - btAlignedObjectArray& tmpConstraintSizesPool, - btSingleConstraintRowSolver& resolveSingleConstraintRowGeneric, - btSingleConstraintRowSolver& resolveSingleConstraintRowLowerLimit, - btSingleConstraintRowSolver& resolveSplitPenetrationImpulse, - btAlignedObjectArray& kinematicBodyUniqueIdToSolverBodyTable, - unsigned long& seed, - int& fixedBodyId, - int& maxOverrideNumSolverIterations - ) + btConstraintArray& tmpSolverContactConstraintPool, + btConstraintArray& tmpSolverNonContactConstraintPool, + btConstraintArray& tmpSolverContactFrictionConstraintPool, + btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, + btAlignedObjectArray& orderTmpConstraintPool, + btAlignedObjectArray& orderNonContactConstraintPool, + btAlignedObjectArray& orderFrictionConstraintPool, + btAlignedObjectArray& tmpConstraintSizesPool, + btSingleConstraintRowSolver& resolveSingleConstraintRowGeneric, + btSingleConstraintRowSolver& resolveSingleConstraintRowLowerLimit, + btSingleConstraintRowSolver& resolveSplitPenetrationImpulse, + btAlignedObjectArray& kinematicBodyUniqueIdToSolverBodyTable, + unsigned long& seed, + int& fixedBodyId, + int& maxOverrideNumSolverIterations + ) :m_tmpSolverBodyPool(tmpSolverBodyPool), m_tmpSolverContactConstraintPool(tmpSolverContactConstraintPool), m_tmpSolverNonContactConstraintPool(tmpSolverNonContactConstraintPool), @@ -126,26 +126,26 @@ protected: btScalar m_leastSquaresResidual; void setupFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, - btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, - const btContactSolverInfo& infoGlobal, - btScalar desiredVelocity = 0., btScalar cfmSlip = 0.); + btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, + const btContactSolverInfo& infoGlobal, + btScalar desiredVelocity = 0., btScalar cfmSlip = 0.); void setupTorsionalFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, - btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, - btScalar desiredVelocity = 0., btScalar cfmSlip = 0.); + btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, + btScalar desiredVelocity = 0., btScalar cfmSlip = 0.); btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.); btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar torsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.f); void setupContactConstraint(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, - const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2); + const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2); static void applyAnisotropicFriction(btCollisionObject * colObj, btVector3 & frictionDirection, int frictionMode); void setFrictionConstraintImpulse(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction unsigned long m_btSeed2; @@ -159,7 +159,7 @@ protected: virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal); void convertJoint(btSolverConstraint * currentConstraintRow, btTypedConstraint * constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal); - + virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint) @@ -186,15 +186,15 @@ protected: } public: - + void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - - + + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); @@ -210,22 +210,22 @@ public: static void convertBodiesInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); static void convertJointsInternal(btSISolverSingleIterationData& siData, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal); static void convertContactInternal(btSISolverSingleIterationData& siData, btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal); - static void setupContactConstraintInternal(btSISolverSingleIterationData& siData, btSolverConstraint& solverConstraint,int solverBodyIdA, int solverBodyIdB,btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,btScalar& relaxation, + static void setupContactConstraintInternal(btSISolverSingleIterationData& siData, btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2); static btScalar restitutionCurveInternal(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold); - static btSolverConstraint& addTorsionalFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.); + static btSolverConstraint& addTorsionalFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.); static void setupTorsionalFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip); static void setupFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip); - static btSolverConstraint& addFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - static void setFrictionConstraintImpulseInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, - + static btSolverConstraint& addFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.); + static void setFrictionConstraintImpulseInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, + btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); - static void convertJointInternal(btAlignedObjectArray& tmpSolverBodyPool, + static void convertJointInternal(btAlignedObjectArray& tmpSolverBodyPool, int& maxOverrideNumSolverIterations, btSolverConstraint* currentConstraintRow, btTypedConstraint* constraint, @@ -242,7 +242,7 @@ public: static void writeBackBodiesInternal(btAlignedObjectArray& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); static void solveGroupCacheFriendlySplitImpulseIterationsInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - + ///clear internal cached data and reset random seed virtual void reset(); @@ -296,14 +296,6 @@ public: static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric(); static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric(); - virtual btScalar solveGroupConvertConstraintPrestep(btCollisionObject** /*bodies*/, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** /*constraints*/, int /*numConstraints*/, const btContactSolverInfo& /*infoGlobal*/, btIDebugDraw* /*debugDrawer*/); - virtual btScalar solveGroupConvertConstraintPoststep(btCollisionObject** /*bodies*/, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** /*constraints*/, int /*numConstraints*/, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/); - virtual btScalar solveGroupConvertConstraints(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - - virtual btScalar solveGroupConvertBackPrestep(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); - virtual btScalar solveGroupConvertBack(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); - virtual btScalar solveGroupConvertBackPoststep(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); - }; -#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H +#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H \ No newline at end of file diff --git a/src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.cpp deleted file mode 100644 index bca5048cb..000000000 --- a/src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.cpp +++ /dev/null @@ -1,760 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2018 Google Inc. http://bulletphysics.org - -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 "btMultiBodyBlockConstraintSolver.h" - -#include - -#include "LinearMath/btQuickprof.h" -#include "btMultiBodyMLCPConstraintSolver.h" -#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" - -static void rigidBodyNotSupported() -{ - printf("Attempts to use rigid body for block solver, which is not supported yet.\n"); - btAssert(false); -} - -btMultiBodyConstraintBlock::btMultiBodyConstraintBlock() - : m_constraintConfigId(-1) -{ - // Do nothing -} - -btMultiBodyConstraintBlock::btMultiBodyConstraintBlock( - btTypedConstraint** m_constraints, - int m_numConstraints, - btAlignedObjectArray* m_solverBodyPool, - btConstraintArray& m_nonContactConstraints, - btConstraintArray& m_normalContactConstraints, - btConstraintArray& m_frictionContactConstraints, - btConstraintArray& m_rollingFrictionContactConstraints, - btMultiBodyConstraint** m_multiBodyConstraints, - int m_numMultiBodyConstraints, - btAlignedObjectArray& m_multiBodyNonContactConstraints, - btAlignedObjectArray& m_multiBodyNormalContactConstraints, - btAlignedObjectArray& m_multiBodyFrictionContactConstraints, - btAlignedObjectArray& m_multiBodyTorsionalFrictionContactConstraints, - btMultiBodyJacobianData* m_data) - : m_constraintConfigId(-1) -{ - // Do nothing -} - -static void copyConstraintDynamicDataToBlock(btAlignedObjectArray& originalConstraints, const btAlignedObjectArray& blockConstraints) -{ - btAssert(originalConstraints.size() == blockConstraints.size()); - for (int i = 0; i < blockConstraints.size(); ++i) - { - btMultiBodySolverConstraint& originalConstraint = *originalConstraints[i]; - const btMultiBodySolverConstraint& blockConstraint = blockConstraints[i]; - - blockConstraint.m_appliedImpulse = originalConstraint.m_appliedImpulse; - blockConstraint.m_appliedPushImpulse = originalConstraint.m_appliedPushImpulse; - } -} - -void debugPrint(const btScalar* data, int size) -{ - for (int i = 0; i < size; ++i) - { - printf("\t%.5f", data[i]); - if (i != size - 1) - printf(","); - } - printf("\n"); -} - -void debugPrintDiff(const btScalar* dataA, const btScalar* dataB, int size, bool ignoreZero = false) -{ - for (int i = 0; i < size; ++i) - { - if (ignoreZero) - { - if (btFabs(dataA[i] - dataB[i]) < 1e-9) - continue; - } - printf("\t%f", dataA[i] - dataB[i]); - if (i != size - 1) - printf(","); - } - printf("\n"); -} - -void btMultiBodyConstraintBlock::copyDynamicDataFromOriginalToBlock() -{ - copyConstraintDynamicDataToBlock(m_originalMultiBodyNormalContactConstraintPtrs, m_internalData.m_multiBodyNormalContactConstraints); - copyConstraintDynamicDataToBlock(m_originalMultiBodyFrictionContactConstraintPtrs, m_internalData.m_multiBodyFrictionContactConstraints); - copyConstraintDynamicDataToBlock(m_originalMultiBodyTorsionalFrictionContactConstraintPtrs, m_internalData.m_multiBodyTorsionalFrictionContactConstraints); - - btAssert(m_multiBodies.size() == m_originalDeltaVelIndices.size()); - btAssert(m_multiBodies.size() == m_deltaVelIndices.size()); - for (int i = 0; i < m_multiBodies.size(); ++i) - { - btMultiBody* multiBody = m_multiBodies[i]; - const int ndof = multiBody->getNumDofs() + 6; - - btMultiBodyJacobianData& originalData = m_internalData.m_data; // TODO(JS): WRONG !! - btAlignedObjectArray& originaDeltaVelocities = originalData.m_deltaVelocities; - - btAlignedObjectArray& blockDeltaVelocities = m_internalData.m_data.m_deltaVelocities; - - const int originalIndex = m_originalDeltaVelIndices[i]; - const int blockIndex = m_deltaVelIndices[i]; - - const btScalar* originalDeltaVelocitiesPtr = &originaDeltaVelocities[originalIndex]; - btScalar* blockDeltaVelocitiesPtr = &blockDeltaVelocities[blockIndex]; - - // printf("[ original --> block ]\n"); - // printf("original: "); - // debugPrint(originalDeltaVelocitiesPtr, ndof); - // printf("block: "); - // debugPrint(blockDeltaVelocitiesPtr, ndof); - // printf("diff: "); - // debugPrintDiff(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof, true); - // printf("\n"); - - memcpy(blockDeltaVelocitiesPtr, originalDeltaVelocitiesPtr, ndof * sizeof(btScalar)); - } -} - -static void copyConstraintDynamicDataFromToOriginal(btAlignedObjectArray& originalConstraints, const btAlignedObjectArray& blockConstraints) -{ - btAssert(originalConstraints.size() == blockConstraints.size()); - for (int i = 0; i < blockConstraints.size(); ++i) - { - btMultiBodySolverConstraint& originalConstraint = *originalConstraints[i]; - const btMultiBodySolverConstraint& blockConstraint = blockConstraints[i]; - - originalConstraint.m_appliedImpulse = blockConstraint.m_appliedImpulse; - originalConstraint.m_appliedPushImpulse = blockConstraint.m_appliedPushImpulse; - } -} - -void btMultiBodyConstraintBlock::copyDynamicDataFromBlockToOriginal() -{ - copyConstraintDynamicDataFromToOriginal(m_originalMultiBodyNormalContactConstraintPtrs, m_internalData.m_multiBodyNormalContactConstraints); - copyConstraintDynamicDataFromToOriginal(m_originalMultiBodyFrictionContactConstraintPtrs, m_internalData.m_multiBodyFrictionContactConstraints); - copyConstraintDynamicDataFromToOriginal(m_originalMultiBodyTorsionalFrictionContactConstraintPtrs, m_internalData.m_multiBodyTorsionalFrictionContactConstraints); - - btAssert(m_multiBodies.size() == m_originalDeltaVelIndices.size()); - btAssert(m_multiBodies.size() == m_deltaVelIndices.size()); - for (int i = 0; i < m_multiBodies.size(); ++i) - { - btMultiBody* multiBody = m_multiBodies[i]; - const int ndof = multiBody->getNumDofs() + 6; - - btMultiBodyJacobianData& originalData = m_internalData.m_data; - btAlignedObjectArray& originaDeltaVelocities = originalData.m_deltaVelocities; - - btAlignedObjectArray& blockDeltaVelocities = m_internalData.m_data.m_deltaVelocities; - - const int originalIndex = m_originalDeltaVelIndices[i]; - const int blockIndex = m_deltaVelIndices[i]; - - btScalar* originalDeltaVelocitiesPtr = &originaDeltaVelocities[originalIndex]; - const btScalar* blockDeltaVelocitiesPtr = &blockDeltaVelocities[blockIndex]; - - // printf("[ block --> original ]\n"); - // printf("original: "); - // debugPrint(originalDeltaVelocitiesPtr, ndof); - // printf("block: "); - // debugPrint(blockDeltaVelocitiesPtr, ndof); - // printf("diff: "); - // debugPrintDiff(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof, true); - // printf("\n"); - - memcpy(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof * sizeof(btScalar)); - } -} - -btSingleBlockSplittingPolicy::btSingleBlockSplittingPolicy(btMultiBodyConstraintSolver* solver) - : m_solver(solver) -{ - // Do nothing -} - -btSingleBlockSplittingPolicy::~btSingleBlockSplittingPolicy() -{ - // Do nothing -} - -void btSingleBlockSplittingPolicy::split(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInput, const btAlignedObjectArray& availableConfigs, btAlignedObjectArray& blocksOutput) -{ - btMultiBodyConstraintBlock newBlock; - // newBlock.m_originalInternalDataBlock = blockInput; - // m_solver->setMultiBodyInternalConstraintData(newBlock.m_originalInternalDataBlock); - newBlock.m_solver = m_solver; - // newBlock.m_constraints = blockInput.m_constraints; - // newBlock.m_numConstraints = blockInput.m_numConstraints; - // newBlock.m_multiBodyConstraints = blockInput.m_multiBodyConstraints; - // newBlock.m_numMultiBodyConstraints = blockInput.m_numMultiBodyConstraints; - - blocksOutput.push_back(newBlock); -} - -btDoubleBlockSplittingPolicy::btDoubleBlockSplittingPolicy(btMultiBodyConstraintSolver* solver) - : m_solver(solver) -{ - // Do nothing -} - -btDoubleBlockSplittingPolicy::~btDoubleBlockSplittingPolicy() -{ - // Do nothing -} - -template -void splitContactConstraints(const ArrayT& input, ArrayT& output1, ArrayT& output2) -{ - const int totalSize = input.size(); - const int halfSize = totalSize / 2; - - output1.resize(halfSize); - output2.resize(totalSize - halfSize); - - for (int i = 0; i < halfSize; ++i) - { - output1[i] = input[i]; - } - - for (int i = halfSize; i < totalSize; ++i) - { - output2[i - halfSize] = input[i]; - } -} - -btMultiBodyConstraintBlock initializeConstraintBlock(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& input) -{ - btMultiBodyConstraintBlock output; - - // MultiBody - - output.m_internalData.m_multiBodyConstraints = input.m_multiBodyConstraints; - output.m_internalData.m_numMultiBodyConstraints = input.m_numMultiBodyConstraints; - //output.m_multiBodyConstraintSet.m_data = input.m_multiBodyConstraintSet.m_data; - - btAssert(output.m_internalData.m_multiBodyNormalContactConstraints.size() == 0); - btAssert(output.m_internalData.m_multiBodyFrictionContactConstraints.size() == 0); - - return output; -} - -static void setupBlockMultiBodyJacobianData( - btMultiBody* multiBody, - btAlignedObjectArray& multiBodySet, - btAlignedObjectArray& blockDeltaVelIndices, - int& blockDeltaVelIndex, - int& blockJacobianIndex, - btMultiBodyJacobianData& blockJacobianData, - btAlignedObjectArray& originalDeltaVelIndices, - const int originalDeltaVelIndex, - const int originalJacobianIndex, - const btMultiBodyJacobianData& originalJacobianData) -{ - const int ndof = multiBody->getNumDofs() + 6; - - btAlignedObjectArray& blockJacobians = blockJacobianData.m_jacobians; - btAlignedObjectArray& blockDeltaVelocities = blockJacobianData.m_deltaVelocities; - btAlignedObjectArray& blockDeltaVelocitiesUnitImpulse = blockJacobianData.m_deltaVelocitiesUnitImpulse; - - const btAlignedObjectArray& originalJacobians = originalJacobianData.m_jacobians; - const btAlignedObjectArray& originalDeltaVelocitiesUnitImpulse = originalJacobianData.m_deltaVelocitiesUnitImpulse; - - int indexInBlock = -1; - for (int i = 0; i < multiBodySet.size(); ++i) - { - if (multiBody == multiBodySet[i]) - { - indexInBlock = i; - break; - } - } - - if (indexInBlock == -1) - { - blockDeltaVelIndex = blockDeltaVelocities.size(); - blockDeltaVelocities.resize(blockDeltaVelocities.size() + ndof); - multiBodySet.push_back(multiBody); - originalDeltaVelIndices.push_back(originalDeltaVelIndex); - blockDeltaVelIndices.push_back(blockDeltaVelIndex); - } - else - { - blockDeltaVelIndex = blockDeltaVelIndices[indexInBlock]; - } - - blockJacobianIndex = blockJacobians.size(); - blockJacobians.resize(blockJacobians.size() + ndof); - blockDeltaVelocitiesUnitImpulse.resize(blockDeltaVelocitiesUnitImpulse.size() + ndof); - btAssert(blockJacobians.size() == blockDeltaVelocitiesUnitImpulse.size()); - - btScalar* blockJacobiansRawPtr = &blockJacobians[blockJacobianIndex]; - const btScalar* originalJacobiansRawPtr = &originalJacobians[originalJacobianIndex]; - memcpy(blockJacobiansRawPtr, originalJacobiansRawPtr, ndof * sizeof(btScalar)); - - btScalar* blockDeltaVelUnitImp = &blockDeltaVelocitiesUnitImpulse[blockJacobianIndex]; - const btScalar* originalDeltaVelUnitImp = &originalDeltaVelocitiesUnitImpulse[originalJacobianIndex]; - memcpy(blockDeltaVelUnitImp, originalDeltaVelUnitImp, ndof * sizeof(btScalar)); - - btAssert(blockJacobians.size() >= blockDeltaVelocities.size()); -} - -static void setupMultiBodyBlockConstraintData( - btMultiBodyConstraintBlock& block, - btAlignedObjectArray& blockMultiBodySet, - btAlignedObjectArray& blockDeltaVelIndices, - btMultiBodySolverConstraint& blockContactConstraint, - btMultiBodyJacobianData& blockJacobianData, - int blockFrictionIndex, - btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, - btAlignedObjectArray& originalDeltaVelIndices, - const btMultiBodySolverConstraint& originalContactConstraint, - const btMultiBodyJacobianData& originalJacobianData) -{ - // Copy all the values. Some values will be updated below if necessary. - blockContactConstraint = originalContactConstraint; - blockContactConstraint.m_frictionIndex = blockFrictionIndex; - - btMultiBody* multiBodyA = blockContactConstraint.m_multiBodyA; - if (multiBodyA) - { - setupBlockMultiBodyJacobianData( - multiBodyA, - blockMultiBodySet, - blockDeltaVelIndices, - blockContactConstraint.m_deltaVelAindex, - blockContactConstraint.m_jacAindex, - blockJacobianData, - originalDeltaVelIndices, - originalContactConstraint.m_deltaVelAindex, - originalContactConstraint.m_jacAindex, - originalJacobianData); - } - else - { - rigidBodyNotSupported(); - } - - btMultiBody* multiBodyB = blockContactConstraint.m_multiBodyB; - if (multiBodyB) - { - setupBlockMultiBodyJacobianData( - multiBodyB, - blockMultiBodySet, - blockDeltaVelIndices, - blockContactConstraint.m_deltaVelBindex, - blockContactConstraint.m_jacBindex, - blockJacobianData, - originalDeltaVelIndices, - originalContactConstraint.m_deltaVelBindex, - originalContactConstraint.m_jacBindex, - originalJacobianData); - } - else - { - rigidBodyNotSupported(); - } -} - -void btDoubleBlockSplittingPolicy::split( - btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, - const btAlignedObjectArray& availableConfigs, - btAlignedObjectArray& subBlocks) -{ - btMultiBodyConstraintBlock constraintBlock1 = initializeConstraintBlock(originalInternalData); - btMultiBodyConstraintBlock constraintBlock2 = initializeConstraintBlock(originalInternalData); - btMultiBodyConstraintBlock constraintBlock3 = initializeConstraintBlock(originalInternalData); - - constraintBlock1.m_solver = m_solver; - constraintBlock2.m_solver = m_solver; - constraintBlock3.m_solver = m_solver; - - btDantzigSolver* mlcp = new btDantzigSolver(); - btMultiBodyMLCPConstraintSolver* sol = new btMultiBodyMLCPConstraintSolver(mlcp); -// constraintBlock2.m_solver = sol; - - const int totalMultiBodyContactConstraintSize = originalInternalData.m_multiBodyNormalContactConstraints.size(); - const int halfMultiBodyContactConstraintSize = totalMultiBodyContactConstraintSize / 2; - - for (int i = 0; i < halfMultiBodyContactConstraintSize; ++i) - { - copyMultiBodyContactConstraint(constraintBlock1, originalInternalData, i); - } - - for (int i = halfMultiBodyContactConstraintSize; i < totalMultiBodyContactConstraintSize; ++i) - { - copyMultiBodyContactConstraint(constraintBlock2, originalInternalData, i); - } - - const int totalMultiBodyNonContactConstraintSize = originalInternalData.m_multiBodyNonContactConstraints.size(); - - for (int i = 0; i < totalMultiBodyNonContactConstraintSize; ++i) - { -// copyMultiBodyNonContactConstraint(constraintBlock3, originalInternalData, i); - } - - -// for (int i = 0; i < totalMultiBodyContactConstraintSize; ++i) -// { -// if (strcmp(originalInternalData.m_multiBodyNormalContactConstraints[i].m_multiBodyA->getBaseName(), "group1") == 0) -// copyMultiBodyContactConstraint(constraintBlock1, originalInternalData, i); -// else -// copyMultiBodyContactConstraint(constraintBlock2, originalInternalData, i); -// } - - subBlocks.push_back(constraintBlock1); - subBlocks.push_back(constraintBlock2); - subBlocks.push_back(constraintBlock3); -} - -btMultiBodyBlockSplittingPolicy::~btMultiBodyBlockSplittingPolicy() -{ - // Do nothing -} - -void btMultiBodyBlockSplittingPolicy::copyMultiBodyNonContactConstraint(btMultiBodyConstraintBlock& block, btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, int originalNonContactConstraintIndex) -{ - btAlignedObjectArray& originalNonContactConstraints = originalInternalData.m_multiBodyNonContactConstraints; - const btMultiBodyJacobianData& originalJacobianData = originalInternalData.m_data; - - btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInternalData = block.m_internalData; - btAlignedObjectArray& blockNonContactConstraints = blockInternalData.m_multiBodyNonContactConstraints; - - btAlignedObjectArray& blockOriginalNonContactConstraintPtrs = block.m_originalMultiBodyNonContactConstraintPtrs; - - btMultiBodyJacobianData& blockJacobianData = block.m_internalData.m_data; - - btAlignedObjectArray& blockMultiBodySet = block.m_multiBodies; - - const int blockFrictionIndex = blockNonContactConstraints.size(); - - btMultiBodySolverConstraint& originalNonContactConstraint = originalNonContactConstraints[originalNonContactConstraintIndex]; - - btMultiBodySolverConstraint& blockNonContactConstraint = blockNonContactConstraints.expandNonInitializing(); - blockOriginalNonContactConstraintPtrs.push_back(&originalNonContactConstraint); - - setupMultiBodyBlockConstraintData( - block, - blockMultiBodySet, - block.m_deltaVelIndices, - blockNonContactConstraint, - blockJacobianData, - blockFrictionIndex, - originalInternalData, - block.m_originalDeltaVelIndices, - originalNonContactConstraint, - originalJacobianData); -} - -void btMultiBodyBlockSplittingPolicy::copyMultiBodyContactConstraint(btMultiBodyConstraintBlock& block, btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, int originalNormalContactConstraintIndex) -{ - btAlignedObjectArray& originalNormalContactConstraints = originalInternalData.m_multiBodyNormalContactConstraints; - btAlignedObjectArray& originalFrictionContactConstraints = originalInternalData.m_multiBodyFrictionContactConstraints; - btAlignedObjectArray& originalTortionalFrictionContactConstraints = originalInternalData.m_multiBodyTorsionalFrictionContactConstraints; - const btMultiBodyJacobianData& originalJacobianData = originalInternalData.m_data; - - btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInternalData = block.m_internalData; - btAlignedObjectArray& blockNormalContactConstraints = blockInternalData.m_multiBodyNormalContactConstraints; - btAlignedObjectArray& blockFrictionContactConstraints = blockInternalData.m_multiBodyFrictionContactConstraints; - btAlignedObjectArray& blockTortionalFrictionContactConstraints = blockInternalData.m_multiBodyTorsionalFrictionContactConstraints; - - btAlignedObjectArray& blockOriginalNormalContactConstraintPtrs = block.m_originalMultiBodyNormalContactConstraintPtrs; - btAlignedObjectArray& blockOriginalFrictionContactConstraintPtrs = block.m_originalMultiBodyFrictionContactConstraintPtrs; - btAlignedObjectArray& blockOriginalTorsionalFrictionContactConstraintPtrs = block.m_originalMultiBodyTorsionalFrictionContactConstraintPtrs; - - btMultiBodyJacobianData& blockJacobianData = block.m_internalData.m_data; - - const int numFrictionPerContact = originalNormalContactConstraints.size() == originalFrictionContactConstraints.size() ? 1 : 2; - - btAlignedObjectArray& blockMultiBodySet = block.m_multiBodies; - - const int blockFrictionIndex = blockNormalContactConstraints.size(); - - //-- 1. Normal contact - - btMultiBodySolverConstraint& originalNormalContactConstraint = originalNormalContactConstraints[originalNormalContactConstraintIndex]; - - btMultiBodySolverConstraint& blockNormalContactConstraint = blockNormalContactConstraints.expandNonInitializing(); - blockOriginalNormalContactConstraintPtrs.push_back(&originalNormalContactConstraint); - - setupMultiBodyBlockConstraintData( - block, - blockMultiBodySet, - block.m_deltaVelIndices, - blockNormalContactConstraint, - blockJacobianData, - blockFrictionIndex, - originalInternalData, - block.m_originalDeltaVelIndices, - originalNormalContactConstraint, - originalJacobianData); - - //-- 2. Friction contacts - - btAssert(originalFrictionContactConstraints.size() != 0); - const int originalFrictionContactConstraintIndex1 = originalNormalContactConstraintIndex * numFrictionPerContact; - btMultiBodySolverConstraint& originalFrictionContactConstraint = originalFrictionContactConstraints[originalFrictionContactConstraintIndex1]; - - blockOriginalFrictionContactConstraintPtrs.push_back(&originalFrictionContactConstraint); - btMultiBodySolverConstraint& blockFrictionContactConstraint1 = blockFrictionContactConstraints.expandNonInitializing(); - setupMultiBodyBlockConstraintData( - block, - blockMultiBodySet, - block.m_deltaVelIndices, - blockFrictionContactConstraint1, - blockJacobianData, - blockFrictionIndex, - originalInternalData, - block.m_originalDeltaVelIndices, - originalFrictionContactConstraint, - originalJacobianData); - - if (numFrictionPerContact == 2) - { - const int originalFrictionContactConstraintIndex2 = originalFrictionContactConstraintIndex1 + 1; - btMultiBodySolverConstraint& originalFrictionContactConstraint = originalFrictionContactConstraints[originalFrictionContactConstraintIndex2]; - - blockOriginalFrictionContactConstraintPtrs.push_back(&originalFrictionContactConstraint); - btMultiBodySolverConstraint& blockFrictionContactConstraint2 = blockFrictionContactConstraints.expandNonInitializing(); - setupMultiBodyBlockConstraintData( - block, - blockMultiBodySet, - block.m_deltaVelIndices, - blockFrictionContactConstraint2, - blockJacobianData, - blockFrictionIndex, - originalInternalData, - block.m_originalDeltaVelIndices, - originalFrictionContactConstraint, - originalJacobianData); - } - - // TODO(JS): Torsional friction contact constraints -} - -btMultiBodyBlockConstraintSolver::btMultiBodyBlockConstraintSolver() -{ - // Do nothing -} - -btMultiBodyBlockConstraintSolver::~btMultiBodyBlockConstraintSolver() -{ - // Do nothing -} - -btScalar btMultiBodyBlockConstraintSolver::solveGroupConvertConstraintPoststep(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) -{ - return btMultiBodyConstraintSolver::solveGroupConvertConstraintPoststep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); -} - -void btMultiBodyBlockConstraintSolver::solveMultiBodyGroup( - btCollisionObject** bodies, - int numBodies, - btPersistentManifold** manifold, - int numManifolds, - btTypedConstraint** constraints, - int numConstraints, - btMultiBodyConstraint** multiBodyConstraints, - int numMultiBodyConstraints, - const btContactSolverInfo& info, - btIDebugDraw* debugDrawer, - btDispatcher* /*dispatcher*/) -{ - m_tmpMultiBodyConstraints = multiBodyConstraints; - m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - // 1. Convert rigid bodies/multibodies, joints, contacts into constraints. - solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - - // 2. Split constraints into constraint blocks - btMultiBodyInternalConstraintData originalInternalDataCopy; - getMultiBodyInternalConstraintData(originalInternalDataCopy); - - btAlignedObjectArray configs; - // TODO(JS): This is just for test - //m_splittingPolicy = new btSingleBlockSplittingPolicy(new btMultiBodyConstraintSolver()); - -// btDantzigSolver* mlcp = new btDantzigSolver(); -// btMultiBodyMLCPConstraintSolver* sol = new btMultiBodyMLCPConstraintSolver(mlcp); -// m_splittingPolicy = new btDoubleBlockSplittingPolicy(sol); - - m_splittingPolicy = new btDoubleBlockSplittingPolicy(new btMultiBodyConstraintSolver()); - - btAssert(m_splittingPolicy); - m_blocks.resize(0); - m_splittingPolicy->split(originalInternalDataCopy, configs, m_blocks); - -// for (int i = 0; i < m_blocks.size(); ++i) -// { -// btMultiBodyConstraintBlock& block = m_blocks[i]; -// btMultiBodyConstraintSolver* solver = block.m_solver; -// btAssert(solver); - -// solver->solveGroupConvertConstraintPrestep(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); -// copyDynamicDataFromOriginalToBlock(block); -//// block.copyDynamicDataFromOriginalToBlock(); -// solver->setMultiBodyInternalConstraintData(block.m_internalData, false); -// solver->solveGroupConvertConstraintPoststep(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); -// } - - // 3. Gauss-Seidel iterations - - const int maxIterations = m_maxOverrideNumSolverIterations > info.m_numIterations ? m_maxOverrideNumSolverIterations : info.m_numIterations; - - m_leastSquaresResidual = 0; - - for (int iteration = 0; iteration < maxIterations; ++iteration) - { - for (int i = 0; i < m_blocks.size(); ++i) - { - // Change the sweep direction every iteration - const int index = iteration & 1 ? m_blocks.size() - 1 - i : i; - - btMultiBodyConstraintBlock& block = m_blocks[index]; - btMultiBodyConstraintSolver* solver = block.m_solver; - btAssert(solver); - - solver->solveGroupConvertConstraintPrestep(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - copyDynamicDataFromOriginalToBlock(block); - solver->setMultiBodyInternalConstraintData(block.m_internalData, false); - solver->solveGroupConvertConstraintPoststep(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - - // TODO(JS): Add split impulse - btScalar newSquaredResidual = solver->solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - m_leastSquaresResidual = btMax(m_leastSquaresResidual, newSquaredResidual); - - solver->solveGroupConvertBackPrestep(bodies, numBodies, info); - solver->solveGroupConvertBack(bodies, numBodies, info); - solver->getMultiBodyInternalConstraintData(block.m_internalData, false); - copyDynamicDataFromBlockToOriginal(block); - solver->solveGroupConvertBackPoststep(bodies, numBodies, info); - } - - if (m_leastSquaresResidual <= info.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) - { -#ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); -#endif - break; - } - } - -// solveGroupConvertBackPrestep(bodies, numBodies, info); -// solveGroupConvertBack(bodies, numBodies, info); -// getMultiBodyInternalConstraintData(block.m_internalData, false); -// copyDynamicDataFromBlockToOriginal(block); -// solveGroupConvertBackPoststep(bodies, numBodies, info); - - solveGroupCacheFriendlyFinish(bodies, numBodies, info); - - m_tmpMultiBodyConstraints = 0; - m_tmpNumMultiBodyConstraints = 0; -} - -void btMultiBodyBlockConstraintSolver::setSplittingPolicy(btMultiBodyBlockSplittingPolicy* policy) -{ - m_splittingPolicy = policy; -} - -void btMultiBodyBlockConstraintSolver::copyDynamicDataFromOriginalToBlock(btMultiBodyConstraintBlock& block) -{ - copyConstraintDynamicDataToBlock(block.m_originalMultiBodyNormalContactConstraintPtrs, block.m_internalData.m_multiBodyNormalContactConstraints); - copyConstraintDynamicDataToBlock(block.m_originalMultiBodyFrictionContactConstraintPtrs, block.m_internalData.m_multiBodyFrictionContactConstraints); - copyConstraintDynamicDataToBlock(block.m_originalMultiBodyTorsionalFrictionContactConstraintPtrs, block.m_internalData.m_multiBodyTorsionalFrictionContactConstraints); - - btAssert(block.m_multiBodies.size() == block.m_originalDeltaVelIndices.size()); - btAssert(block.m_multiBodies.size() == block.m_deltaVelIndices.size()); - for (int i = 0; i < block.m_multiBodies.size(); ++i) - { - btMultiBody* multiBody = block.m_multiBodies[i]; - const int ndof = multiBody->getNumDofs() + 6; - - btMultiBodyJacobianData& originalData = m_data; // TODO(JS): WRONG !! - btAlignedObjectArray& originaDeltaVelocities = originalData.m_deltaVelocities; - - btAlignedObjectArray& blockDeltaVelocities = block.m_internalData.m_data.m_deltaVelocities; - - const int originalIndex = block.m_originalDeltaVelIndices[i]; - const int blockIndex = block.m_deltaVelIndices[i]; - - const btScalar* originalDeltaVelocitiesPtr = &originaDeltaVelocities[originalIndex]; - btScalar* blockDeltaVelocitiesPtr = &blockDeltaVelocities[blockIndex]; - - // printf("[ original --> block ]\n"); - // printf("original: "); - // debugPrint(originalDeltaVelocitiesPtr, ndof); - // printf("block: "); - // debugPrint(blockDeltaVelocitiesPtr, ndof); - // printf("diff: "); - // debugPrintDiff(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof, true); - // printf("\n"); - - memcpy(blockDeltaVelocitiesPtr, originalDeltaVelocitiesPtr, ndof * sizeof(btScalar)); - } -} - -void btMultiBodyBlockConstraintSolver::copyDynamicDataFromBlockToOriginal(btMultiBodyConstraintBlock& block) -{ - copyConstraintDynamicDataFromToOriginal(block.m_originalMultiBodyNormalContactConstraintPtrs, block.m_internalData.m_multiBodyNormalContactConstraints); - copyConstraintDynamicDataFromToOriginal(block.m_originalMultiBodyFrictionContactConstraintPtrs, block.m_internalData.m_multiBodyFrictionContactConstraints); - copyConstraintDynamicDataFromToOriginal(block.m_originalMultiBodyTorsionalFrictionContactConstraintPtrs, block.m_internalData.m_multiBodyTorsionalFrictionContactConstraints); - - btAssert(block.m_multiBodies.size() == block.m_originalDeltaVelIndices.size()); - btAssert(block.m_multiBodies.size() == block.m_deltaVelIndices.size()); - for (int i = 0; i < block.m_multiBodies.size(); ++i) - { - btMultiBody* multiBody = block.m_multiBodies[i]; - const int ndof = multiBody->getNumDofs() + 6; - - btMultiBodyJacobianData& originalData = m_data; - btAlignedObjectArray& originaDeltaVelocities = originalData.m_deltaVelocities; - - btAlignedObjectArray& blockDeltaVelocities = block.m_internalData.m_data.m_deltaVelocities; - - const int originalIndex = block.m_originalDeltaVelIndices[i]; - const int blockIndex = block.m_deltaVelIndices[i]; - - btScalar* originalDeltaVelocitiesPtr = &originaDeltaVelocities[originalIndex]; - const btScalar* blockDeltaVelocitiesPtr = &blockDeltaVelocities[blockIndex]; - - // printf("[ block --> original ]\n"); - // printf("original: "); - // debugPrint(originalDeltaVelocitiesPtr, ndof); - // printf("block: "); - // debugPrint(blockDeltaVelocitiesPtr, ndof); - // printf("diff: "); - // debugPrintDiff(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof, true); - // printf("\n"); - - memcpy(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof * sizeof(btScalar)); - } -} - -int btMultiBodyBlockConstraintSolver::addConfig(btBlockConstraintSolverConfig& config) -{ - m_configs.push_back(config); - return m_configs.size(); -} - -int btMultiBodyBlockConstraintSolver::getNumConfigs() const -{ - return m_configs.size(); -} - -void btMultiBodyBlockConstraintSolver::removeConfig(int configIndex) -{ - m_configs.removeAtIndex(configIndex); -} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h deleted file mode 100644 index 9fecd1fa5..000000000 --- a/src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h +++ /dev/null @@ -1,186 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2018 Google Inc. http://bulletphysics.org - -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 BT_MULTIBODY_BLOCK_CONSTRAINT_SOLVER_H -#define BT_MULTIBODY_BLOCK_CONSTRAINT_SOLVER_H - -#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" - -struct btBlockConstraintSolverConfig -{ - int m_solverType; //SI or MLCP Dantzig - //to be decided: full or subset of - - btContactSolverInfo m_info; -}; - -struct btMultiBodyConstraintBlock -{ - /// \{ \name Multi-body Data - - btAlignedObjectArray m_multiBodies; - btAlignedObjectArray m_originalDeltaVelIndices; - btAlignedObjectArray m_deltaVelIndices; - -// btMultiBodyJacobianData* m_originalDataPtr; - - btAlignedObjectArray m_originalMultiBodyNonContactConstraintPtrs; - btAlignedObjectArray m_originalMultiBodyNormalContactConstraintPtrs; - btAlignedObjectArray m_originalMultiBodyFrictionContactConstraintPtrs; - btAlignedObjectArray m_originalMultiBodyTorsionalFrictionContactConstraintPtrs; - - /// \} - - btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData m_internalData; - - /// Constraint solver - btMultiBodyConstraintSolver* m_solver; - - bool m_ownSolver = false; - // TODO(JS): If this is true, then don't copy all the constraint data, but - // only dynamic data - // TODO(JS): not utilized yet - - /// Index to constraint solver configuration - int m_constraintConfigId; - - /// Default constructor - btMultiBodyConstraintBlock(); - - /// Constructor - btMultiBodyConstraintBlock( - btTypedConstraint** m_constraints, - int m_numConstraints, - btAlignedObjectArray* m_solverBodyPool, - btConstraintArray& m_originalNonContactConstraints, - btConstraintArray& m_originalNormalContactConstraints, - btConstraintArray& m_originalFrictionContactConstraints, - btConstraintArray& m_orginalRollingFrictionContactConstraints, - btMultiBodyConstraint** m_multiBodyConstraints, - int m_numMultiBodyConstraints, - btAlignedObjectArray& m_multiBodyNonContactConstraints, - btAlignedObjectArray& m_multiBodyNormalContactConstraints, - btAlignedObjectArray& m_multiBodyFrictionContactConstraints, - btAlignedObjectArray& m_multiBodyTorsionalFrictionContactConstraints, - btMultiBodyJacobianData* m_data); - - void copyDynamicDataFromOriginalToBlock(); - void copyDynamicDataFromBlockToOriginal(); -}; - -class btMultiBodyBlockSplittingPolicy -{ -public: - /// Destructor - virtual ~btMultiBodyBlockSplittingPolicy(); - - /// Splits a set of constraints into multiple subsets. - /// - /// \param[in] blockInput - /// \param[in] availableConfigs - /// \param[in,out] blocksOutput The splitted blocks. This function adds blocks without clearning the array - /// beforehand. Clearning the array is the caller's responsibility. - virtual void split(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInput, const btAlignedObjectArray& availableConfigs, btAlignedObjectArray& blocksOutput) = 0; - -protected: - void copyMultiBodyNonContactConstraint( - btMultiBodyConstraintBlock& block, - btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, - int originalNonContactConstraintIndex); - - void copyMultiBodyContactConstraint( - btMultiBodyConstraintBlock& block, - btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, - int originalNormalContactConstraintIndex); -}; - -class btSingleBlockSplittingPolicy : public btMultiBodyBlockSplittingPolicy -{ -protected: - btMultiBodyConstraintSolver* m_solver; - -public: - /// Constructor - btSingleBlockSplittingPolicy(btMultiBodyConstraintSolver* solver); - - /// Destructor - virtual ~btSingleBlockSplittingPolicy(); - - // Documentation inherited - virtual void split(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInput, const btAlignedObjectArray& availableConfigs, btAlignedObjectArray& blocksOutput); -}; - -class btDoubleBlockSplittingPolicy : public btMultiBodyBlockSplittingPolicy -{ -protected: - btMultiBodyConstraintSolver* m_solver; - -public: - /// Constructor - btDoubleBlockSplittingPolicy(btMultiBodyConstraintSolver* solver); - - /// Destructor - virtual ~btDoubleBlockSplittingPolicy(); - - // Documentation inherited - virtual void split(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInput, const btAlignedObjectArray& availableConfigs, btAlignedObjectArray& blocksOutput); -}; - -class btMultiBodyBlockConstraintSolver : public btMultiBodyConstraintSolver -{ -protected: - /// Splitting policy. Assumed not a null. - btMultiBodyBlockSplittingPolicy* m_splittingPolicy; - - /// Array of constraint configurations for constraint blocks. - btAlignedObjectArray m_configs; - - /// Array of constraint blocks. - btAlignedObjectArray m_blocks; - -public: - /// Constructor - btMultiBodyBlockConstraintSolver(); - - /// Destructor - virtual ~btMultiBodyBlockConstraintSolver(); - - virtual btScalar solveGroupConvertConstraintPoststep(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - - -protected: - // Documentation inherited. - virtual void solveMultiBodyGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); - - /// Sets the splitting policy. - virtual void setSplittingPolicy(btMultiBodyBlockSplittingPolicy* policy); - - void copyDynamicDataFromOriginalToBlock(btMultiBodyConstraintBlock& block); - void copyDynamicDataFromBlockToOriginal(btMultiBodyConstraintBlock& block); - - /// Adds a constraint block configuration and returns the total number of configurations added to this solver. - virtual int addConfig(btBlockConstraintSolverConfig& config); - - /// Returns the number of configurations added to this solver. - virtual int getNumConfigs() const; - - /// Removes an configuration at \c configIndex - /// - /// \param[in] configIndex The configuration indext in the range of [0, numConfigs). Passing out of the range is an - /// undefined behavior. - virtual void removeConfig(int configIndex); -}; - -#endif // BT_MULTIBODY_BLOCK_CONSTRAINT_SOLVER_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 6300d7688..e97bd71cc 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -23,33 +23,33 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" -btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); - + btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + //solve featherstone non-contact constraints //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); - for (int j=0;jsetPosUpdated(false); - if(constraint.m_multiBodyB) + if (constraint.m_multiBodyB) constraint.m_multiBodyB->setPosUpdated(false); } //solve featherstone normal contact - for (int j0=0;j0setPosUpdated(false); - if(constraint.m_multiBodyB) + if (constraint.m_multiBodyB) constraint.m_multiBodyB->setPosUpdated(false); } //solve featherstone frictional contact - if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)) + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)) { - for (int j1 = 0; j1m_multiBodyTorsionalFrictionContactConstraints.size(); j1++) + for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++) { if (iteration < infoGlobal.m_numIterations) { - int index = j1;//iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1; + int index = j1; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1; btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index]; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; //adjust friction limits here - if (totalImpulse>btScalar(0)) + if (totalImpulse > btScalar(0)) { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); - leastSquaredResidual = btMax(leastSquaredResidual , residual*residual); + leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); if (frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->setPosUpdated(false); @@ -98,29 +98,29 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl { if (iteration < infoGlobal.m_numIterations) { - int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; j1++; - int index2 = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + int index2 = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyFrictionContactConstraints[index2]; btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex); if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex) { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; - frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction*totalImpulse); - frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction*totalImpulse; + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; + frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse); + frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse; btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB); - leastSquaredResidual = btMax(leastSquaredResidual, residual*residual); - + leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); + if (frictionConstraintB.m_multiBodyA) frictionConstraintB.m_multiBodyA->setPosUpdated(false); if (frictionConstraintB.m_multiBodyB) frictionConstraintB.m_multiBodyB->setPosUpdated(false); - + if (frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->setPosUpdated(false); if (frictionConstraint.m_multiBodyB) @@ -131,21 +131,21 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl } else { - for (int j1 = 0; j1m_multiBodyFrictionContactConstraints.size(); j1++) + for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++) { if (iteration < infoGlobal.m_numIterations) { - int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; //adjust friction limits here - if (totalImpulse>btScalar(0)) + if (totalImpulse > btScalar(0)) { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); - leastSquaredResidual = btMax(leastSquaredResidual, residual*residual); + leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); if (frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->setPosUpdated(false); @@ -158,17 +158,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl return leastSquaredResidual; } -btScalar btMultiBodyConstraintSolver::solveSingleIterationNew(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) -{ - return solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); -} - -btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) -{ - return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); -} - -btScalar btMultiBodyConstraintSolver::solveGroupConvertConstraintPrestep(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { m_multiBodyNonContactConstraints.resize(0); m_multiBodyNormalContactConstraints.resize(0); @@ -179,7 +169,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupConvertConstraintPrestep(btColli m_data.m_deltaVelocitiesUnitImpulse.resize(0); m_data.m_deltaVelocities.resize(0); - for (int i = 0; i < numBodies; ++i) + for (int i = 0; i < numBodies; i++) { const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]); if (fcA) @@ -188,26 +178,20 @@ btScalar btMultiBodyConstraintSolver::solveGroupConvertConstraintPrestep(btColli } } - btScalar val = btSequentialImpulseConstraintSolver::solveGroupConvertConstraintPrestep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); return val; } -btScalar btMultiBodyConstraintSolver::solveGroupConvertConstraintPoststep(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) { - return btSequentialImpulseConstraintSolver::solveGroupConvertConstraintPoststep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); -} - -void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) -{ - for (int i = 0; i < ndof; ++i) - m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; + for (int i = 0; i < ndof; ++i) + m_data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse; } btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) { - - btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm; btScalar deltaVelADotn = 0; btScalar deltaVelBDotn = 0; btSolverBody* bodyA = 0; @@ -239,9 +223,8 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); } - - deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom - deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVelADotn * c.m_jacDiagABInv; //m_jacDiagABInv = 1./denom + deltaImpulse -= deltaVelBDotn * c.m_jacDiagABInv; const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; if (sum < c.m_lowerLimit) @@ -258,7 +241,7 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt { c.m_appliedImpulse = sum; } - + if (c.m_multiBodyA) { applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA); @@ -266,12 +249,11 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS } else if (c.m_solverBodyIdA >= 0) { - bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse); - + bodyA->internalApplyImpulse(c.m_contactNormal1 * bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse); } if (c.m_multiBodyB) { @@ -280,54 +262,54 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS } else if (c.m_solverBodyIdB >= 0) { - bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + bodyB->internalApplyImpulse(c.m_contactNormal2 * bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse); } - btScalar deltaVel =deltaImpulse/c.m_jacDiagABInv; + btScalar deltaVel = deltaImpulse / c.m_jacDiagABInv; return deltaVel; } - -btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1,const btMultiBodySolverConstraint& cB) +btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB) { - int ndofA=0; - int ndofB=0; + int ndofA = 0; + int ndofB = 0; btSolverBody* bodyA = 0; btSolverBody* bodyB = 0; btScalar deltaImpulseB = 0.f; btScalar sumB = 0.f; { - deltaImpulseB = cB.m_rhs-btScalar(cB.m_appliedImpulse)*cB.m_cfm; - btScalar deltaVelADotn=0; - btScalar deltaVelBDotn=0; + deltaImpulseB = cB.m_rhs - btScalar(cB.m_appliedImpulse) * cB.m_cfm; + btScalar deltaVelADotn = 0; + btScalar deltaVelBDotn = 0; if (cB.m_multiBodyA) { - ndofA = cB.m_multiBodyA->getNumDofs() + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex+i]; - } else if(cB.m_solverBodyIdA >= 0) + ndofA = cB.m_multiBodyA->getNumDofs() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex + i]; + } + else if (cB.m_solverBodyIdA >= 0) { bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA]; - deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); } if (cB.m_multiBodyB) { - ndofB = cB.m_multiBodyB->getNumDofs() + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex+i]; - } else if(cB.m_solverBodyIdB >= 0) + ndofB = cB.m_multiBodyB->getNumDofs() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex + i]; + } + else if (cB.m_solverBodyIdB >= 0) { bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB]; - deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); } - - deltaImpulseB -= deltaVelADotn*cB.m_jacDiagABInv;//m_jacDiagABInv = 1./denom - deltaImpulseB -= deltaVelBDotn*cB.m_jacDiagABInv; + deltaImpulseB -= deltaVelADotn * cB.m_jacDiagABInv; //m_jacDiagABInv = 1./denom + deltaImpulseB -= deltaVelBDotn * cB.m_jacDiagABInv; sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB; } @@ -336,45 +318,45 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt const btMultiBodySolverConstraint& cA = cA1; { { - deltaImpulseA = cA.m_rhs-btScalar(cA.m_appliedImpulse)*cA.m_cfm; - btScalar deltaVelADotn=0; - btScalar deltaVelBDotn=0; + deltaImpulseA = cA.m_rhs - btScalar(cA.m_appliedImpulse) * cA.m_cfm; + btScalar deltaVelADotn = 0; + btScalar deltaVelBDotn = 0; if (cA.m_multiBodyA) { - ndofA = cA.m_multiBodyA->getNumDofs() + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex+i]; - } else if(cA.m_solverBodyIdA >= 0) + ndofA = cA.m_multiBodyA->getNumDofs() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex + i]; + } + else if (cA.m_solverBodyIdA >= 0) { bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA]; - deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); } if (cA.m_multiBodyB) { - ndofB = cA.m_multiBodyB->getNumDofs() + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex+i]; - } else if(cA.m_solverBodyIdB >= 0) + ndofB = cA.m_multiBodyB->getNumDofs() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex + i]; + } + else if (cA.m_solverBodyIdB >= 0) { bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB]; - deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); } - - deltaImpulseA -= deltaVelADotn*cA.m_jacDiagABInv;//m_jacDiagABInv = 1./denom - deltaImpulseA -= deltaVelBDotn*cA.m_jacDiagABInv; + deltaImpulseA -= deltaVelADotn * cA.m_jacDiagABInv; //m_jacDiagABInv = 1./denom + deltaImpulseA -= deltaVelBDotn * cA.m_jacDiagABInv; sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA; } } - if (sumA*sumA+sumB*sumB>=cA.m_lowerLimit*cB.m_lowerLimit) + if (sumA * sumA + sumB * sumB >= cA.m_lowerLimit * cB.m_lowerLimit) { - btScalar angle = btAtan2(sumA,sumB); - btScalar sumAclipped = btFabs(cA.m_lowerLimit*btSin(angle)); - btScalar sumBclipped = btFabs(cB.m_lowerLimit*btCos(angle)); + btScalar angle = btAtan2(sumA, sumB); + btScalar sumAclipped = btFabs(cA.m_lowerLimit * btSin(angle)); + btScalar sumBclipped = btFabs(cB.m_lowerLimit * btCos(angle)); - if (sumA < -sumAclipped) { deltaImpulseA = -sumAclipped - cA.m_appliedImpulse; @@ -408,78 +390,77 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt //cA.m_appliedImpulse = sumAclipped; //deltaImpulseB = sumBclipped-cB.m_appliedImpulse; //cB.m_appliedImpulse = sumBclipped; - } + } else { cA.m_appliedImpulse = sumA; cB.m_appliedImpulse = sumB; } - + if (cA.m_multiBodyA) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA,cA.m_deltaVelAindex,ndofA); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA, cA.m_deltaVelAindex, ndofA); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } else if(cA.m_solverBodyIdA >= 0) + cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else if (cA.m_solverBodyIdA >= 0) { - bodyA->internalApplyImpulse(cA.m_contactNormal1*bodyA->internalGetInvMass(),cA.m_angularComponentA,deltaImpulseA); - + bodyA->internalApplyImpulse(cA.m_contactNormal1 * bodyA->internalGetInvMass(), cA.m_angularComponentA, deltaImpulseA); } if (cA.m_multiBodyB) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA,cA.m_deltaVelBindex,ndofB); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA, cA.m_deltaVelBindex, ndofB); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } else if(cA.m_solverBodyIdB >= 0) + cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else if (cA.m_solverBodyIdB >= 0) { - bodyB->internalApplyImpulse(cA.m_contactNormal2*bodyB->internalGetInvMass(),cA.m_angularComponentB,deltaImpulseA); + bodyB->internalApplyImpulse(cA.m_contactNormal2 * bodyB->internalGetInvMass(), cA.m_angularComponentB, deltaImpulseA); } if (cB.m_multiBodyA) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB,cB.m_deltaVelAindex,ndofA); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB, cB.m_deltaVelAindex, ndofA); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } else if(cB.m_solverBodyIdA >= 0) + cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else if (cB.m_solverBodyIdA >= 0) { - bodyA->internalApplyImpulse(cB.m_contactNormal1*bodyA->internalGetInvMass(),cB.m_angularComponentA,deltaImpulseB); + bodyA->internalApplyImpulse(cB.m_contactNormal1 * bodyA->internalGetInvMass(), cB.m_angularComponentA, deltaImpulseB); } if (cB.m_multiBodyB) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB,cB.m_deltaVelBindex,ndofB); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB, cB.m_deltaVelBindex, ndofB); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } else if(cB.m_solverBodyIdB >= 0) + cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else if (cB.m_solverBodyIdB >= 0) { - bodyB->internalApplyImpulse(cB.m_contactNormal2*bodyB->internalGetInvMass(),cB.m_angularComponentB,deltaImpulseB); + bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB); } - btScalar deltaVel =deltaImpulseA/cA.m_jacDiagABInv+deltaImpulseB/cB.m_jacDiagABInv; - return deltaVel; + btScalar deltaVel = deltaImpulseA / cA.m_jacDiagABInv + deltaImpulseB / cB.m_jacDiagABInv; + return deltaVel; } - - - -void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { - BT_PROFILE("setupMultiBodyContactConstraint"); btVector3 rel_pos1; btVector3 rel_pos2; @@ -497,44 +478,46 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); if (bodyB) rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); relaxation = infoGlobal.m_sor; - - btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; - - //cfm = 1 / ( dt * kp + kd ) - //erp = dt * kp / ( dt * kp + kd ) - - btScalar cfm; + + btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep; + + //cfm = 1 / ( dt * kp + kd ) + //erp = dt * kp / ( dt * kp + kd ) + + btScalar cfm; btScalar erp; if (isFriction) { cfm = infoGlobal.m_frictionCFM; erp = infoGlobal.m_frictionERP; - } else + } + else { cfm = infoGlobal.m_globalCfm; erp = infoGlobal.m_erp2; - if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)) + if ((cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP)) { - if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) - cfm = cp.m_contactCFM; - if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP) - erp = cp.m_contactERP; - } else + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) + cfm = cp.m_contactCFM; + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP) + erp = cp.m_contactERP; + } + else { if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING) { - btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 ); + btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1); if (denom < SIMD_EPSILON) { denom = SIMD_EPSILON; } - cfm = btScalar(1) / denom; + cfm = btScalar(1) / denom; erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; } } @@ -544,188 +527,185 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (multiBodyA) { - if (solverConstraint.m_linkA<0) + if (solverConstraint.m_linkA < 0) { rel_pos1 = pos1 - multiBodyA->getBasePos(); - } else + } + else { rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); } - const int ndofA = multiBodyA->getNumDofs() + 6; + const int ndofA = multiBodyA->getNumDofs() + 6; solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - if (solverConstraint.m_deltaVelAindex <0) + if (solverConstraint.m_deltaVelAindex < 0) { solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); - } else + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA); + } + else { - btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA); } solverConstraint.m_jacAindex = m_data.m_jacobians.size(); - m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA); btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; + btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex]; multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); + multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v); btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = contactNormal; - } else + } + else { btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = contactNormal; - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0); } - - if (multiBodyB) { - if (solverConstraint.m_linkB<0) + if (solverConstraint.m_linkB < 0) { rel_pos2 = pos2 - multiBodyB->getBasePos(); - } else + } + else { rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); } - const int ndofB = multiBodyB->getNumDofs() + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) + if (solverConstraint.m_deltaVelBindex < 0) { solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB); } solverConstraint.m_jacBindex = m_data.m_jacobians.size(); - m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB); btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); - - btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); + multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v); + + btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; solverConstraint.m_contactNormal2 = -contactNormal; - - } else + } + else { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); + btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; solverConstraint.m_contactNormal2 = -contactNormal; - - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0); } { - btVector3 vec; btScalar denom0 = 0.f; btScalar denom1 = 0.f; btScalar* jacB = 0; btScalar* jacA = 0; - btScalar* lambdaA =0; - btScalar* lambdaB =0; - int ndofA = 0; + btScalar* lambdaA = 0; + btScalar* lambdaB = 0; + int ndofA = 0; if (multiBodyA) { - ndofA = multiBodyA->getNumDofs() + 6; + ndofA = multiBodyA->getNumDofs() + 6; jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA; ++i) { - btScalar j = jacA[i] ; - btScalar l =lambdaA[i]; - denom0 += j*l; + btScalar j = jacA[i]; + btScalar l = lambdaA[i]; + denom0 += j * l; } - } else + } + else { if (rb0) { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); denom0 = rb0->getInvMass() + contactNormal.dot(vec); } } if (multiBodyB) { - const int ndofB = multiBodyB->getNumDofs() + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB; ++i) { - btScalar j = jacB[i] ; - btScalar l =lambdaB[i]; - denom1 += j*l; + btScalar j = jacB[i]; + btScalar l = lambdaB[i]; + denom1 += j * l; } - - } else + } + else { if (rb1) { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2); denom1 = rb1->getInvMass() + contactNormal.dot(vec); } } - - - btScalar d = denom0+denom1+cfm; - if (d>SIMD_EPSILON) - { - solverConstraint.m_jacDiagABInv = relaxation/(d); - } else - { + btScalar d = denom0 + denom1 + cfm; + if (d > SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation / (d); + } + else + { //disable the constraint row to handle singularity/redundant constraint - solverConstraint.m_jacDiagABInv = 0.f; - } - + solverConstraint.m_jacDiagABInv = 0.f; + } } - //compute rhs and remaining solverConstraint fields - - btScalar restitution = 0.f; - btScalar distance = 0; - if (!isFriction) - { - distance = cp.getDistance()+infoGlobal.m_linearSlop; - } else - { - if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) - { - distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal); - } - } - - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; + btScalar distance = 0; + if (!isFriction) { + distance = cp.getDistance() + infoGlobal.m_linearSlop; + } + else + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) + { + distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal); + } + } - btVector3 vel1,vel2; + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + btVector3 vel1, vel2; if (multiBodyA) { - ndofA = multiBodyA->getNumDofs() + 6; + ndofA = multiBodyA->getNumDofs() + 6; btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) + for (int i = 0; i < ndofA; ++i) rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } else + } + else { if (rb0) { @@ -737,12 +717,12 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } if (multiBodyB) { - ndofB = multiBodyB->getNumDofs() + 6; + ndofB = multiBodyB->getNumDofs() + 6; btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) + for (int i = 0; i < ndofB; ++i) rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } else + } + else { if (rb1) { @@ -755,9 +735,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol solverConstraint.m_friction = cp.m_combinedFriction; - if(!isFriction) + if (!isFriction) { - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); if (restitution <= btScalar(0.)) { restitution = 0.f; @@ -765,10 +745,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } } - ///warm starting (or zero if disabled) //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) - if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + if (0) //infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; @@ -778,27 +757,30 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { btScalar impulse = solverConstraint.m_appliedImpulse; btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse); - - applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else + multiBodyA->applyDeltaVeeMultiDof(deltaV, impulse); + + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA); + } + else { if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); } if (multiBodyB) { btScalar impulse = solverConstraint.m_appliedImpulse; btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse); - applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else + multiBodyB->applyDeltaVeeMultiDof(deltaV, impulse); + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB); + } + else { if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse); } } - } else + } + else { solverConstraint.m_appliedImpulse = 0.f; } @@ -806,38 +788,37 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol solverConstraint.m_appliedPushImpulse = 0.f; { - btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction + btScalar velocityError = restitution - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction if (isFriction) { - positionalError = -distance * erp/infoGlobal.m_timeStep; - } else + positionalError = -distance * erp / infoGlobal.m_timeStep; + } + else { - if (distance>0) + if (distance > 0) { positionalError = 0; velocityError -= distance / infoGlobal.m_timeStep; - - } else + } + else { - positionalError = -distance * erp/infoGlobal.m_timeStep; + positionalError = -distance * erp / infoGlobal.m_timeStep; } } - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; - if(!isFriction) + if (!isFriction) { - // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; - } - /*else + /*else { //split position and velocity into rhs and m_rhsPenetration solverConstraint.m_rhs = velocityImpulse; @@ -849,305 +830,288 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } else { - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; } - solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv; - - - + solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv; } - } void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& constraintNormal, - btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) + const btVector3& constraintNormal, + btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { - - BT_PROFILE("setupMultiBodyRollingFrictionConstraint"); - btVector3 rel_pos1; - btVector3 rel_pos2; - - btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; - btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - - btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; - btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; - - btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; - btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; - - if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - - relaxation = infoGlobal.m_sor; - - // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; - - - if (multiBodyA) - { - if (solverConstraint.m_linkA<0) - { - rel_pos1 = pos1 - multiBodyA->getBasePos(); - } else - { - rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); - } - const int ndofA = multiBodyA->getNumDofs() + 6; - - solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (solverConstraint.m_deltaVelAindex <0) - { - solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); - } else - { - btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); - } - - solverConstraint.m_jacAindex = m_data.m_jacobians.size(); - m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); - btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - - btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; - multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0,0,0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); - - btVector3 torqueAxis0 = -constraintNormal; - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = btVector3(0,0,0); - } else - { - btVector3 torqueAxis0 = -constraintNormal; - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = btVector3(0,0,0); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - } - - - - if (multiBodyB) - { - if (solverConstraint.m_linkB<0) - { - rel_pos2 = pos2 - multiBodyB->getBasePos(); - } else - { - rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); - } - - const int ndofB = multiBodyB->getNumDofs() + 6; - - solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) - { - solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); - } - - solverConstraint.m_jacBindex = m_data.m_jacobians.size(); - - m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); - btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - - multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0,0,0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); - - btVector3 torqueAxis1 = constraintNormal; - solverConstraint.m_relpos2CrossNormal = torqueAxis1; - solverConstraint.m_contactNormal2 = -btVector3(0,0,0); - - } else - { - btVector3 torqueAxis1 = constraintNormal; - solverConstraint.m_relpos2CrossNormal = torqueAxis1; - solverConstraint.m_contactNormal2 = -btVector3(0,0,0); - - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - } - - { - - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* lambdaA =0; - btScalar* lambdaB =0; - int ndofA = 0; - if (multiBodyA) - { - ndofA = multiBodyA->getNumDofs() + 6; - jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; - lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i] ; - btScalar l =lambdaA[i]; - denom0 += j*l; - } - } else - { - if (rb0) - { - btVector3 iMJaA = rb0?rb0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0); + BT_PROFILE("setupMultiBodyRollingFrictionConstraint"); + btVector3 rel_pos1; + btVector3 rel_pos2; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; + btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = infoGlobal.m_sor; + + // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; + + if (multiBodyA) + { + if (solverConstraint.m_linkA < 0) + { + rel_pos1 = pos1 - multiBodyA->getBasePos(); + } + else + { + rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); + } + const int ndofA = multiBodyA->getNumDofs() + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex < 0) + { + solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA); + } + else + { + btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA); + } + + solverConstraint.m_jacAindex = m_data.m_jacobians.size(); + m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0, 0, 0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v); + + btVector3 torqueAxis0 = -constraintNormal; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = btVector3(0, 0, 0); + } + else + { + btVector3 torqueAxis0 = -constraintNormal; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = btVector3(0, 0, 0); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0); + } + + if (multiBodyB) + { + if (solverConstraint.m_linkB < 0) + { + rel_pos2 = pos2 - multiBodyB->getBasePos(); + } + else + { + rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); + } + + const int ndofB = multiBodyB->getNumDofs() + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex < 0) + { + solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB); + } + + solverConstraint.m_jacBindex = m_data.m_jacobians.size(); + + m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0, 0, 0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v); + + btVector3 torqueAxis1 = constraintNormal; + solverConstraint.m_relpos2CrossNormal = torqueAxis1; + solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0); + } + else + { + btVector3 torqueAxis1 = constraintNormal; + solverConstraint.m_relpos2CrossNormal = torqueAxis1; + solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0); + + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0); + } + + { + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA = 0; + btScalar* lambdaB = 0; + int ndofA = 0; + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i]; + btScalar l = lambdaA[i]; + denom0 += j * l; + } + } + else + { + if (rb0) + { + btVector3 iMJaA = rb0 ? rb0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0); denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal); - } - } - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumDofs() + 6; - jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; - lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i] ; - btScalar l =lambdaB[i]; - denom1 += j*l; - } - - } else - { - if (rb1) - { - btVector3 iMJaB = rb1?rb1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i]; + btScalar l = lambdaB[i]; + denom1 += j * l; + } + } + else + { + if (rb1) + { + btVector3 iMJaB = rb1 ? rb1->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0); denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - } - } - - - - btScalar d = denom0+denom1+infoGlobal.m_globalCfm; - if (d>SIMD_EPSILON) - { - solverConstraint.m_jacDiagABInv = relaxation/(d); - } else - { - //disable the constraint row to handle singularity/redundant constraint - solverConstraint.m_jacDiagABInv = 0.f; - } - - } - - - //compute rhs and remaining solverConstraint fields - - - - btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : cp.getDistance(); - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = multiBodyA->getNumDofs() + 6; - btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } else + } + } + + btScalar d = denom0 + denom1 + infoGlobal.m_globalCfm; + if (d > SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation / (d); + } + else + { + //disable the constraint row to handle singularity/redundant constraint + solverConstraint.m_jacDiagABInv = 0.f; + } + } + + //compute rhs and remaining solverConstraint fields + + btScalar restitution = 0.f; + btScalar penetration = isFriction ? 0 : cp.getDistance(); + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + btVector3 vel1, vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } + else { if (rb0) { btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; rel_vel += solverConstraint.m_contactNormal1.dot(rb0 ? solverBodyA->m_linearVelocity + solverBodyA->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? solverBodyA->m_angularVelocity : btVector3(0, 0, 0)); } - } - if (multiBodyB) - { - ndofB = multiBodyB->getNumDofs() + 6; - btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } else - { + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumDofs() + 6; + btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + } + else + { if (rb1) { btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; rel_vel += solverConstraint.m_contactNormal2.dot(rb1 ? solverBodyB->m_linearVelocity + solverBodyB->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? solverBodyB->m_angularVelocity : btVector3(0, 0, 0)); - } - } + } + } - solverConstraint.m_friction =combinedTorsionalFriction; - - if(!isFriction) - { - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - } - } - } - - - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - btScalar velocityError = 0 - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction - - - - btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv; - - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - solverConstraint.m_lowerLimit = -solverConstraint.m_friction; - solverConstraint.m_upperLimit = solverConstraint.m_friction; - - solverConstraint.m_cfm = infoGlobal.m_globalCfm*solverConstraint.m_jacDiagABInv; - - - - } - + solverConstraint.m_friction = combinedTorsionalFriction; + + if (!isFriction) + { + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + } + } + } + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + btScalar velocityError = 0 - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction + + btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; + + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + solverConstraint.m_lowerLimit = -solverConstraint.m_friction; + solverConstraint.m_upperLimit = solverConstraint.m_friction; + + solverConstraint.m_cfm = infoGlobal.m_globalCfm * solverConstraint.m_jacDiagABInv; + } } -btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { BT_PROFILE("addMultiBodyFrictionConstraint"); btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; - + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; + solverConstraint.m_frictionIndex = frictionIndex; bool isFriction = true; const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - - btMultiBody* mbA = fcA? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB? fcB->m_multiBody : 0; - int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; + + int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -1161,95 +1125,92 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo solverConstraint.m_originalContactPoint = &cp; - setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); + setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); return solverConstraint; } -btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { - BT_PROFILE("addMultiBodyRollingFrictionConstraint"); + BT_PROFILE("addMultiBodyRollingFrictionConstraint"); - bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)); + bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)); - btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing(); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; - - solverConstraint.m_frictionIndex = frictionIndex; - bool isFriction = true; - - const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); - const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - - btMultiBody* mbA = fcA? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB? fcB->m_multiBody : 0; - - int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); - - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_multiBodyA = mbA; - if (mbA) - solverConstraint.m_linkA = fcA->m_link; - - solverConstraint.m_multiBodyB = mbB; - if (mbB) - solverConstraint.m_linkB = fcB->m_link; - - solverConstraint.m_originalContactPoint = &cp; - - setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction,infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); - return solverConstraint; + btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction ? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing(); + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; + + solverConstraint.m_frictionIndex = frictionIndex; + bool isFriction = true; + + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; + + int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); + return solverConstraint; } -void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) +void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) { const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - - btMultiBody* mbA = fcA? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB? fcB->m_multiBody : 0; - btCollisionObject* colObj0=0,*colObj1=0; + btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; + + btCollisionObject *colObj0 = 0, *colObj1 = 0; colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); - int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); - -// btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; -// btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; + int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); + // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; + // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; ///avoid collision response between two static objects -// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) + // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) // return; - //only a single rollingFriction per manifold - int rollingFriction=1; - - for (int j=0;jgetNumContacts();j++) - { + //only a single rollingFriction per manifold + int rollingFriction = 1; + for (int j = 0; j < manifold->getNumContacts(); j++) + { btManifoldPoint& cp = manifold->getContactPoint(j); if (cp.getDistance() <= manifold->getContactProcessingThreshold()) { - btScalar relaxation; int frictionIndex = m_multiBodyNormalContactConstraints.size(); btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); - // btRigidBody* rb0 = btRigidBody::upcast(colObj0); - // btRigidBody* rb1 = btRigidBody::upcast(colObj1); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; + // btRigidBody* rb0 = btRigidBody::upcast(colObj0); + // btRigidBody* rb1 = btRigidBody::upcast(colObj1); + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_multiBodyA = mbA; @@ -1263,10 +1224,10 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* solverConstraint.m_originalContactPoint = &cp; bool isFriction = false; - setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction); + setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp, infoGlobal, relaxation, isFriction); -// const btVector3& pos1 = cp.getPositionWorldOnA(); -// const btVector3& pos2 = cp.getPositionWorldOnB(); + // const btVector3& pos1 = cp.getPositionWorldOnA(); + // const btVector3& pos2 = cp.getPositionWorldOnB(); /////setup the friction constraints #define ENABLE_FRICTION @@ -1277,46 +1238,45 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* ///By default, each contact has only a single friction direction that is recomputed automatically every frame ///based on the relative linear velocity. ///If the relative velocity is zero, it will automatically compute a friction direction. - + ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. /// ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. /// - ///The user can manually override the friction directions for certain contacts using a contact callback, + ///The user can manually override the friction directions for certain contacts using a contact callback, ///and set the cp.m_lateralFrictionInitialized to true ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect /// - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2); cp.m_lateralFrictionDir1.normalize(); cp.m_lateralFrictionDir2.normalize(); - if (rollingFriction > 0 ) - { - if (cp.m_combinedSpinningFriction>0) - { - addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal); - } - if (cp.m_combinedRollingFriction>0) - { + if (rollingFriction > 0) + { + if (cp.m_combinedSpinningFriction > 0) + { + addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal); + } + if (cp.m_combinedRollingFriction > 0) + { + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - - if (cp.m_lateralFrictionDir1.length()>0.001) - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); + if (cp.m_lateralFrictionDir1.length() > 0.001) + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); - if (cp.m_lateralFrictionDir2.length()>0.001) - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); - } - rollingFriction--; - } - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) - {/* + if (cp.m_lateralFrictionDir2.length() > 0.001) + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); + } + rollingFriction--; + } + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) + { /* cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) @@ -1339,155 +1299,77 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* } else */ { - - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); } if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) { - cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; + cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; } } - - } else + } + else { - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_frictionCFM); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_frictionCFM); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); //todo: solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; } - - -#endif //ENABLE_FRICTION +#endif //ENABLE_FRICTION } } } -void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) +void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) { //btPersistentManifold* manifold = 0; - for (int i=0;igetBody0()); const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); if (!fcA && !fcB) { //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case - convertContact(manifold,infoGlobal); - } else + convertContact(manifold, infoGlobal); + } + else { - convertMultiBodyContact(manifold,infoGlobal); + convertMultiBodyContact(manifold, infoGlobal); } } //also convert the multibody constraints, if any - - for (int i=0;icreateConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal); - } -} - -static void copyConstraintsFromProxy(btAlignedObjectArray& constraints, const btAlignedObjectArray& constraintsProxy, bool onlyDynamicData) -{ - if (onlyDynamicData) - { - btAssert(constraints.size() == constraintsProxy.size()); - for (int i = 0; i < constraintsProxy.size(); ++i) - { - btMultiBodySolverConstraint& destConstraint = constraints[i]; - const btMultiBodySolverConstraint& srcConstraint = constraintsProxy[i]; - - destConstraint.m_appliedImpulse = srcConstraint.m_appliedImpulse; - destConstraint.m_appliedPushImpulse = srcConstraint.m_appliedPushImpulse; - } - return; - } - - constraints.resize(constraintsProxy.size()); - for (int i = 0; i < constraintsProxy.size(); ++i) - { - constraints[i] = constraintsProxy[i]; + c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal); } } -static void copyConstraintsToProxy(btAlignedObjectArray& constraintsProxy, btAlignedObjectArray& constraints, bool onlyDynamicData) -{ - if (onlyDynamicData) - { - btAssert(constraintsProxy.size() == constraints.size()); - for (int i = 0; i < constraints.size(); ++i) - { - btMultiBodySolverConstraint& destConstraint = constraintsProxy[i]; - const btMultiBodySolverConstraint& srcConstraint = constraints[i]; - - destConstraint.m_appliedImpulse = srcConstraint.m_appliedImpulse; - destConstraint.m_appliedPushImpulse = srcConstraint.m_appliedPushImpulse; - } - return; - } - - constraintsProxy.resize(constraints.size()); - for (int i = 0; i < constraints.size(); ++i) - { - constraintsProxy[i] = constraints[i]; - } -} - -void btMultiBodyConstraintSolver::setMultiBodyInternalConstraintData(const btMultiBodyInternalConstraintData& data, bool onlyDynamicData) -{ - copyConstraintsFromProxy(m_multiBodyNonContactConstraints, data.m_multiBodyNonContactConstraints, onlyDynamicData); - copyConstraintsFromProxy(m_multiBodyNormalContactConstraints, data.m_multiBodyNormalContactConstraints, onlyDynamicData); - copyConstraintsFromProxy(m_multiBodyFrictionContactConstraints, data.m_multiBodyFrictionContactConstraints, onlyDynamicData); - copyConstraintsFromProxy(m_multiBodyTorsionalFrictionContactConstraints, data.m_multiBodyTorsionalFrictionContactConstraints, onlyDynamicData); - - if (onlyDynamicData) - m_data.m_deltaVelocities = data.m_data.m_deltaVelocities; - else - m_data = data.m_data; -} - -void btMultiBodyConstraintSolver::getMultiBodyInternalConstraintData(btMultiBodyInternalConstraintData& data, bool onlyDynamicData) -{ - copyConstraintsToProxy(data.m_multiBodyNonContactConstraints, m_multiBodyNonContactConstraints, onlyDynamicData); - copyConstraintsToProxy(data.m_multiBodyNormalContactConstraints, m_multiBodyNormalContactConstraints, onlyDynamicData); - copyConstraintsToProxy(data.m_multiBodyFrictionContactConstraints, m_multiBodyFrictionContactConstraints, onlyDynamicData); - copyConstraintsToProxy(data.m_multiBodyTorsionalFrictionContactConstraints, m_multiBodyTorsionalFrictionContactConstraints, onlyDynamicData); - - if (onlyDynamicData) - data.m_data.m_deltaVelocities = m_data.m_deltaVelocities; - else - data.m_data = m_data; -} - -btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) +btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) { //printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints); - return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); + return btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher); } #if 0 @@ -1511,56 +1393,54 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS } #endif - void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime) { -#if 1 - +#if 1 + //bod->addBaseForce(m_gravity * bod->getBaseMass()); //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); if (c.m_orgConstraint) { - c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse); + c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex, c.m_appliedImpulse); } - if (c.m_multiBodyA) { - c.m_multiBodyA->setCompanionId(-1); - btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime); - btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime); - if (c.m_linkA<0) + btVector3 force = c.m_contactNormal1 * (c.m_appliedImpulse / deltaTime); + btVector3 torque = c.m_relpos1CrossNormal * (c.m_appliedImpulse / deltaTime); + if (c.m_linkA < 0) { c.m_multiBodyA->addBaseConstraintForce(force); c.m_multiBodyA->addBaseConstraintTorque(torque); - } else + } + else { - c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force); - //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); - c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque); + c.m_multiBodyA->addLinkConstraintForce(c.m_linkA, force); + //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); + c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA, torque); } } - + if (c.m_multiBodyB) { { c.m_multiBodyB->setCompanionId(-1); - btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime); - btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime); - if (c.m_linkB<0) + btVector3 force = c.m_contactNormal2 * (c.m_appliedImpulse / deltaTime); + btVector3 torque = c.m_relpos2CrossNormal * (c.m_appliedImpulse / deltaTime); + if (c.m_linkB < 0) { c.m_multiBodyB->addBaseConstraintForce(force); c.m_multiBodyB->addBaseConstraintTorque(torque); - } else + } + else { { - c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force); + c.m_multiBodyB->addLinkConstraintForce(c.m_linkB, force); //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); - c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque); + c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB, torque); } - } } } @@ -1570,59 +1450,53 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv if (c.m_multiBodyA) { - c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); + c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], c.m_appliedImpulse); } - + if (c.m_multiBodyB) { - c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); + c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], c.m_appliedImpulse); } #endif - - - } -btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish"); int numPoolConstraints = m_multiBodyNormalContactConstraints.size(); - - //write back the delta v to the multi bodies, either as applied impulse (direct velocity change) + //write back the delta v to the multi bodies, either as applied impulse (direct velocity change) //or as applied force, so we can measure the joint reaction forces easier - for (int i=0;im_appliedImpulse = solverConstraint.m_appliedImpulse; pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse; - + //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { @@ -1635,23 +1509,119 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO //do a callback here? } +#if 0 + //multibody joint feedback + { + BT_PROFILE("multi body joint feedback"); + for (int j=0;jisMultiDof()) + { + applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); + } + if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof()) + { + applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep)); + } +#if 0 + if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof()) + { + applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex], + m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex, + m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA, + m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); + + } + if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof()) + { + applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex], + m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex, + m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB, + m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep)); + } + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof()) + { + applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1], + m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex, + m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA, + m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); + } + + if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof()) + { + applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1], + m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex, + m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB, + m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep)); + } + } +#endif + } + + for (int i=0;iisMultiDof()) + { + applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); + } + if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof()) + { + applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); + } + } + } + + numPoolConstraints = m_multiBodyNonContactConstraints.size(); + +#if 0 + //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint + for (int i=0;igetJointFeedback(); + if (fb) + { + fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; + fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ + + } + + constr->internalSetAppliedImpulse(c.m_appliedImpulse); + if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) + { + constr->setEnabled(false); + } + + } +#endif +#endif return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); } - -void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) +void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) { //printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints); //printf("solveMultiBodyGroup start\n"); m_tmpMultiBodyConstraints = multiBodyConstraints; m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); + + btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher); m_tmpMultiBodyConstraints = 0; m_tmpNumMultiBodyConstraints = 0; - - } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 057ededbe..f39f2879f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -28,111 +28,68 @@ class btMultiBody; ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver { - protected: + btMultiBodyConstraintArray m_multiBodyNonContactConstraints; - btMultiBodyConstraintArray m_multiBodyNonContactConstraints; + btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; + btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; + btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints; - btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; - btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; - btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints; + btMultiBodyJacobianData m_data; - btMultiBodyJacobianData m_data; - //temp storage for multi body constraints for a specific island/group called by 'solveGroup' - btMultiBodyConstraint** m_tmpMultiBodyConstraints; - int m_tmpNumMultiBodyConstraints; + btMultiBodyConstraint** m_tmpMultiBodyConstraints; + int m_tmpNumMultiBodyConstraints; btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); - + //solve 2 friction directions and clamp against the implicit friction cone btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB); - - void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); - - btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); + void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); - btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); + btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); - void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, - btScalar* jacA,btScalar* jacB, - btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, - const btContactSolverInfo& infoGlobal); + btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); - void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - //either rolling or spinning friction - void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); + void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint & constraintRow, + btScalar * jacA, btScalar * jacB, + btScalar penetration, btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, + const btContactSolverInfo& infoGlobal); - void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); -// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + + //either rolling or spinning friction + void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint & solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + + void convertMultiBodyContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal); + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + // virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof); + void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime); - virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof); - void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime); public: - - struct btMultiBodyInternalConstraintData - { - /// Multibody (joint) constraints. This is shared by all the blocks. - btMultiBodyConstraint** m_multiBodyConstraints; - - /// Number of multibody (joint) constraints. This is shared by all the - /// blocks. - int m_numMultiBodyConstraints; - - /// Array of multibody non-contact constraints - btAlignedObjectArray m_multiBodyNonContactConstraints; - - /// Array of multibody normal contact constraints - btAlignedObjectArray m_multiBodyNormalContactConstraints; - - /// Array of multibody friction contact constraints - btAlignedObjectArray m_multiBodyFrictionContactConstraints; - - /// Array of multibody rolling friction contact constraints - btAlignedObjectArray m_multiBodyTorsionalFrictionContactConstraints; - - /// Pointer to the block constraint solver's multi body Jacobian data, which - /// is shared by all the constraint blocks. - btMultiBodyJacobianData m_data; - }; - BT_DECLARE_ALIGNED_ALLOCATOR(); - /// Copies internal constraint data from \p proxy - virtual void setMultiBodyInternalConstraintData(const btMultiBodyInternalConstraintData& proxy, bool onlyDynamicData = false); - - /// Copies internal constraint data to \p proxy - virtual void getMultiBodyInternalConstraintData(btMultiBodyInternalConstraintData& data, bool onlyDynamicData = false); - ///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints) - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); - virtual btScalar solveGroupConvertConstraintPrestep(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - virtual btScalar solveGroupConvertConstraintPoststep(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - virtual btScalar solveSingleIterationNew(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); - - virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); + virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); + + virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); }; - - - - -#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H - +#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H From 893b46ce1429a8e673099312bf3d336a46333c30 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 28 Feb 2019 17:13:51 -0800 Subject: [PATCH 5/6] apply mouse_move_multiplier and mouse_wheel_multiplier --- examples/CommonInterfaces/CommonGraphicsAppInterface.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/CommonInterfaces/CommonGraphicsAppInterface.h b/examples/CommonInterfaces/CommonGraphicsAppInterface.h index cd821da1f..cde91831f 100644 --- a/examples/CommonInterfaces/CommonGraphicsAppInterface.h +++ b/examples/CommonInterfaces/CommonGraphicsAppInterface.h @@ -204,17 +204,17 @@ struct CommonGraphicsApp if (m_middleMouseButton) { - cameraTargetPosition += cameraUp * yDelta * 0.01; + cameraTargetPosition += cameraUp * yDelta *m_mouseMoveMultiplier* 0.01; b3Vector3 fwd = cameraTargetPosition - cameraPosition; b3Vector3 side = cameraUp.cross(fwd); side.normalize(); - cameraTargetPosition += side * xDelta * 0.01; + cameraTargetPosition += side * xDelta *m_mouseMoveMultiplier* 0.01; } if (m_rightMouseButton) { - cameraDistance -= xDelta * 0.01f; - cameraDistance -= yDelta * 0.01f; + cameraDistance -= xDelta * m_mouseMoveMultiplier*0.01f; + cameraDistance -= yDelta * m_mouseMoveMultiplier*0.01f; if (cameraDistance < 1) cameraDistance = 1; if (cameraDistance > 1000) @@ -251,7 +251,7 @@ struct CommonGraphicsApp float cameraDistance = camera->getCameraDistance(); if (deltay < 0 || cameraDistance > 1) { - cameraDistance -= deltay * 0.01f; + cameraDistance -= deltay*m_wheelMultiplier * 0.01f; if (cameraDistance < 1) cameraDistance = 1; camera->setCameraDistance(cameraDistance); From 8b5a238b2f3feda5cc01be168cf0bd6bc4d81b81 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Wed, 6 Mar 2019 13:47:49 -0800 Subject: [PATCH 6/6] Add boxes joint by fixed joint example\n Onlyp2p joint works --- examples/BulletRobotics/FixJointBoxes.cpp | 117 +++++++++++++++++++++ examples/BulletRobotics/FixJointBoxes.h | 21 ++++ examples/ExampleBrowser/CMakeLists.txt | 1 + examples/ExampleBrowser/ExampleEntries.cpp | 5 + 4 files changed, 144 insertions(+) create mode 100644 examples/BulletRobotics/FixJointBoxes.cpp create mode 100644 examples/BulletRobotics/FixJointBoxes.h diff --git a/examples/BulletRobotics/FixJointBoxes.cpp b/examples/BulletRobotics/FixJointBoxes.cpp new file mode 100644 index 000000000..875a76b1d --- /dev/null +++ b/examples/BulletRobotics/FixJointBoxes.cpp @@ -0,0 +1,117 @@ + +#include "FixJointBoxes.h" + +#include "../CommonInterfaces/CommonGraphicsAppInterface.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "../CommonInterfaces/CommonRenderInterface.h" +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemory/PhysicsServerSharedMemory.h" +#include "../SharedMemory/PhysicsClientC_API.h" +#include "../SharedMemory/SharedMemoryPublic.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include +#include +#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" + +class FixJointBoxes : public CommonExampleInterface +{ + GUIHelperInterface* m_guiHelper; + b3RobotSimulatorClientAPI m_robotSim; + int m_options; + +public: + FixJointBoxes(GUIHelperInterface* helper, int options) + : m_guiHelper(helper), + m_options(options) + { + } + + virtual ~FixJointBoxes() + { + } + + virtual void physicsDebugDraw(int debugDrawMode) + { + m_robotSim.debugDraw(debugDrawMode); + } + virtual void initPhysics() + { + int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; + m_robotSim.setGuiHelper(m_guiHelper); + bool connected = m_robotSim.connect(mode); + + b3Printf("robotSim connected = %d", connected); + + m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0); + + b3RobotSimulatorLoadUrdfFileArgs args; + + size_t numCubes = 10; + std::vector cubeIds(numCubes, 0); + for (int i = 0; i < numCubes; i++) + { + args.m_forceOverrideFixedBase = (i == 0); + args.m_startPosition.setValue(0, i * 0.05, 1); + cubeIds[i] = m_robotSim.loadURDF("cube_small.urdf", args); + + b3JointInfo jointInfo; + + jointInfo.m_parentFrame[1] = -0.025; + jointInfo.m_childFrame[1] = 0.025; + jointInfo.m_jointType = eFixedType; + // jointInfo.m_jointType = ePoint2PointType; + // jointInfo.m_jointType = ePrismaticType; + + if (i > 0) + { + m_robotSim.createConstraint(cubeIds[i], -1, cubeIds[i - 1], -1, &jointInfo); + } + + m_robotSim.loadURDF("plane.urdf"); + } + } + + virtual void exitPhysics() + { + m_robotSim.disconnect(); + } + virtual void stepSimulation(float deltaTime) + { + m_robotSim.stepSimulation(); + } + virtual void renderScene() + { + m_robotSim.renderScene(); + } + + virtual bool mouseMoveCallback(float x, float y) + { + return m_robotSim.mouseMoveCallback(x, y); + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return m_robotSim.mouseButtonCallback(button, state, x, y); + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + virtual void resetCamera() + { + // float dist = 1; + // float pitch = -20; + // float yaw = -30; + // float targetPos[3] = {0, 0.2, 0.5}; + + // m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } +}; + +class CommonExampleInterface* FixJointBoxesCreateFunc(struct CommonExampleOptions& options) +{ + return new FixJointBoxes(options.m_guiHelper, options.m_option); +} diff --git a/examples/BulletRobotics/FixJointBoxes.h b/examples/BulletRobotics/FixJointBoxes.h new file mode 100644 index 000000000..c8a305dfc --- /dev/null +++ b/examples/BulletRobotics/FixJointBoxes.h @@ -0,0 +1,21 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2016 Google Inc. http://bulletphysics.org + +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 FIX_JOINT_BOXES_H +#define FIX_JOINT_BOXES_H + +class CommonExampleInterface* FixJointBoxesCreateFunc(struct CommonExampleOptions& options); + +#endif //FIX_JOINT_BOXES_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index ef0789e42..6b8242ef4 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -144,6 +144,7 @@ SET(ExtendedTutorialsSources ) SET(BulletExampleBrowser_SRCS + ../BulletRobotics/FixJointBoxes.cpp ../TinyRenderer/geometry.cpp ../TinyRenderer/model.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 8f1f7d37a..d3e3a3c9d 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -2,6 +2,7 @@ #include "LinearMath/btAlignedObjectArray.h" #include "EmptyExample.h" +#include "../BulletRobotics/FixJointBoxes.h" #include "../RenderingExamples/RenderInstancingDemo.h" #include "../RenderingExamples/CoordinateSystemDemo.h" #include "../RenderingExamples/RaytracerSetup.h" @@ -128,6 +129,10 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", RigidBodySoftContactCreateFunc), + + ExampleEntry(0, "Bullet Robotics"), + ExampleEntry(1, "FixJoint Boxes", "FixJoint Boxes", FixJointBoxesCreateFunc), + ExampleEntry(0, "MultiBody"), ExampleEntry(1, "MultiDof", "Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc), ExampleEntry(1, "TestJointTorque", "Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc),