From 0e1a77047cfaa9bf94f7318686ba4179f40c8c1b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 3 May 2014 02:50:09 -0700 Subject: [PATCH] fix Linux build --- Demos3/AllBullet2Demos/CMakeLists.txt | 7 +- Demos3/SimpleOpenGL3/CMakeLists.txt | 2 +- .../btSequentialImpulseConstraintSolver.cpp | 295 ++++++++++-------- .../btSequentialImpulseConstraintSolver.h | 11 +- 4 files changed, 179 insertions(+), 136 deletions(-) diff --git a/Demos3/AllBullet2Demos/CMakeLists.txt b/Demos3/AllBullet2Demos/CMakeLists.txt index 04e995302..0d16efb3d 100644 --- a/Demos3/AllBullet2Demos/CMakeLists.txt +++ b/Demos3/AllBullet2Demos/CMakeLists.txt @@ -34,6 +34,10 @@ SET(App_AllBullet2Demos_SRCS ${BULLET_PHYSICS_SOURCE_DIR}/build3/bullet.rc ) +LINK_LIBRARIES( + Bullet3Common BulletSoftBody BulletDynamics BulletCollision LinearMath OpenGLWindow gwen ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} +) + IF (WIN32) SET(App_AllBullet2Demos_SRCS ${App_AllBullet2Demos_SRCS} ${App_AllBullet2Demos_Common_SRCS}) INCLUDE_DIRECTORIES( @@ -52,9 +56,6 @@ ENDIF(WIN32) -LINK_LIBRARIES( - Bullet3Common BulletSoftBody BulletDynamics BulletCollision LinearMath OpenGLWindow gwen ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} -) ADD_EXECUTABLE(App_AllBullet2Demos ${App_AllBullet2Demos_SRCS} diff --git a/Demos3/SimpleOpenGL3/CMakeLists.txt b/Demos3/SimpleOpenGL3/CMakeLists.txt index 7cae8b701..14f604717 100644 --- a/Demos3/SimpleOpenGL3/CMakeLists.txt +++ b/Demos3/SimpleOpenGL3/CMakeLists.txt @@ -12,7 +12,7 @@ SET(AppSimpleOpenGL3_SRCS ) LINK_LIBRARIES( - gwen OpenGLWindow Bullet3Common ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} + gwen OpenGLWindow Bullet3Common ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} ) IF (WIN32) diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index ddc65f79b..ee5000e22 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +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. @@ -22,6 +22,8 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btCpuFeatureUtility.h" + //#include "btJacobianEntry.h" #include "LinearMath/btMinMax.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" @@ -38,11 +40,70 @@ int gNumSplitImpulseRecoveries = 0; #include "BulletDynamics/Dynamics/btRigidBody.h" +///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver +///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check. +static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +{ + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + + // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else if (sum > c.m_upperLimit) + { + deltaImpulse = c.m_upperLimit - c.m_appliedImpulse; + c.m_appliedImpulse = c.m_upperLimit; + } + else + { + c.m_appliedImpulse = sum; + } + + body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + + return deltaImpulse; +} + + +static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +{ + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else + { + c.m_appliedImpulse = sum; + } + body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + + return deltaImpulse; +} + + #ifdef USE_SIMD #include -//#include //is it safe to include this header? #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) @@ -51,12 +112,12 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) ); } -#if defined (BT_USE_SSE4) +#if defined (BT_ALLOW_SSE4) #include #define USE_FMA 1 #define USE_FMA3_INSTEAD_FMA4 1 -#define USE_SSE4_DOT 0 +#define USE_SSE4_DOT 1 #define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f) #define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f)) @@ -147,6 +208,7 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& } + static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) { __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); @@ -217,34 +279,7 @@ btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGene // Project Gauss Seidel or the equivalent Sequential Impulse btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); - -// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else if (sum > c.m_upperLimit) - { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_upperLimit; - } - else - { - c.m_appliedImpulse = sum; - } - - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); - - return deltaImpulse; + return gResolveSingleConstraintRowGeneric_scalar_reference(body1, body2, c); } btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) @@ -259,26 +294,7 @@ btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowe btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); - - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else - { - c.m_appliedImpulse = sum; - } - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); - - return deltaImpulse; + return gResolveSingleConstraintRowLowerLimit_scalar_reference(body1,body2,c); } @@ -349,34 +365,61 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() :m_btSeed2(0), - m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_sse2), - m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_sse2) + m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference), + m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference) { +#ifdef USE_SIMD + m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2; + m_resolveSingleConstraintRowLowerLimit=gResolveSingleConstraintRowLowerLimit_sse2; +#endif //USE_SIMD + +#ifdef BT_ALLOW_SSE4 + int cpuFeatures = btCpuFeatureUtility::getCpuFeatures(); + if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1)) + { + m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3; + m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3; + } +#endif//BT_ALLOW_SSE4 + } btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() { } + btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric() + { + return gResolveSingleConstraintRowGeneric_scalar_reference; + } + btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit() + { + return gResolveSingleConstraintRowLowerLimit_scalar_reference; + } + + +#ifdef USE_SIMD btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric() { return gResolveSingleConstraintRowGeneric_sse2; } - btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric() - { - return gResolveSingleConstraintRowGeneric_sse4_1_fma3; - } btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit() { return gResolveSingleConstraintRowLowerLimit_sse2; } +#ifdef BT_ALLOW_SSE4 + btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric() + { + return gResolveSingleConstraintRowGeneric_sse4_1_fma3; + } btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit() { return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3; } - +#endif //BT_ALLOW_SSE4 +#endif //USE_SIMD unsigned long btSequentialImpulseConstraintSolver::btRand2() { @@ -437,7 +480,7 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod 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(); @@ -469,7 +512,7 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode) { - + if (colObj && colObj->hasAnisotropicFriction(frictionMode)) { @@ -490,7 +533,7 @@ 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, btScalar desiredVelocity, btScalar cfmSlip) { - + btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; @@ -551,12 +594,12 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr } { - + btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); rel_vel = vel1Dotn+vel2Dotn; @@ -570,7 +613,7 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; - + } } @@ -578,7 +621,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c { btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; - setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, + setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); return solverConstraint; } @@ -586,7 +629,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) { @@ -632,12 +675,12 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv } { - + btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); rel_vel = vel1Dotn+vel2Dotn; @@ -650,7 +693,7 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; - + } } @@ -665,7 +708,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConst { btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; - setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, + setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); return solverConstraint; } @@ -693,7 +736,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body.setCompanionId(solverBodyIdA); } else { - + if (m_fixedBodyId<0) { m_fixedBodyId = m_tmpSolverBodyPool.size(); @@ -711,13 +754,13 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& #include -void btSequentialImpulseConstraintSolver::setupContactConstraint(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) { - + const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos2 = cp.getPositionWorldOnB(); @@ -727,23 +770,23 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra btRigidBody* rb0 = bodyA->m_originalBody; btRigidBody* rb1 = bodyB->m_originalBody; -// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); +// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); relaxation = 1.f; btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); { #ifdef COMPUTE_IMPULSE_DENOM btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); -#else +#else btVector3 vec; btScalar denom0 = 0.f; btScalar denom1 = 0.f; @@ -757,7 +800,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); } -#endif //COMPUTE_IMPULSE_DENOM +#endif //COMPUTE_IMPULSE_DENOM btScalar denom = relaxation/(denom0+denom1); solverConstraint.m_jacDiagABInv = denom; @@ -795,11 +838,11 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra btVector3 vel = vel1 - vel2; btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); - + solverConstraint.m_friction = cp.m_combinedFriction; - + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); if (restitution <= btScalar(0.)) { @@ -829,17 +872,17 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0); btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0); btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0); - - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA) + + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB) + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB); btScalar rel_vel = vel1Dotn+vel2Dotn; btScalar positionalError = 0.f; btScalar velocityError = restitution - rel_vel;// * damping; - + btScalar erp = infoGlobal.m_erp2; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) @@ -884,7 +927,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra -void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, +void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) { @@ -963,7 +1006,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m btVector3 rel_pos1; btVector3 rel_pos2; btScalar relaxation; - + int frictionIndex = m_tmpSolverContactConstraintPool.size(); btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); @@ -977,7 +1020,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos2 = cp.getPositionWorldOnB(); - rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); + rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); @@ -985,13 +1028,13 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1); solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 ); - + btVector3 vel = vel1 - vel2; btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2); - + // const btVector3& pos1 = cp.getPositionWorldOnA(); // const btVector3& pos2 = cp.getPositionWorldOnB(); @@ -1032,21 +1075,21 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); if (axis1.length()>0.001) addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - + } } ///Bullet has several options to set the friction directions - ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///By default, each contact has only a single friction direction that is recomputed automatically very frame ///based on the relative linear velocity. ///If the relative velocity it 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 @@ -1102,9 +1145,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m } setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); - - + + } } @@ -1144,7 +1187,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol bool found=false; for (int b=0;bgetRigidBodyA()==bodies[b]) { found = true; @@ -1176,7 +1219,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol bool found=false; for (int b=0;bgetBody0()==bodies[b]) { found = true; @@ -1200,8 +1243,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } #endif //BT_ADDITIONAL_DEBUG - - + + for (int i = 0; i < numBodies; i++) { bodies[i]->setCompanionId(-1); @@ -1232,7 +1275,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } } - + if (1) { int j; @@ -1252,7 +1295,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol int totalNumRows = 0; int i; - + m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); //calculate the total number of contraint rows for (i=0;im_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0); @@ -1405,11 +1448,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0); btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0); - - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) + + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA); - - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) + + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB); rel_vel = vel1Dotn+vel2Dotn; @@ -1475,7 +1518,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - + if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { if (1) // uncomment this for a bit less random ((iteration & 7) == 0) @@ -1488,7 +1531,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration m_orderNonContactConstraintPool[swapi] = tmp; } - //contact/friction constraints are not solved more than + //contact/friction constraints are not solved more than if (iteration< infoGlobal.m_numIterations) { for (int j=0; jbtScalar(0)) { solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); @@ -1592,8 +1635,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); } - - + + ///solve all friction constraints, using SIMD, if available @@ -1612,7 +1655,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration } } - + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); for (j=0;j= 0;iteration--) - { + { solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); } - + } return 0.f; } @@ -1802,7 +1845,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ - + } constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); @@ -1823,7 +1866,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); else m_tmpSolverBodyPool[i].writebackVelocity(); - + m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity( m_tmpSolverBodyPool[i].m_linearVelocity+ m_tmpSolverBodyPool[i].m_externalForceImpulse); @@ -1856,13 +1899,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod BT_PROFILE("solveGroup"); //you need to provide at least some bodies - + solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer); solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer); solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); - + return 0.f; } diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 001708a21..a60291809 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -158,16 +158,15 @@ public: m_resolveSingleConstraintRowLowerLimit = rowSolver; } - - // btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit(); + ///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(); + + ///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(); - - - - };