diff options
Diffstat (limited to 'thirdparty/bullet')
22 files changed, 318 insertions, 70 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp index c79623bd57..6873a95d90 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp @@ -80,6 +80,7 @@ struct ClipVertex btVector3 v; int id; //b2ContactID id; + //b2ContactID id; }; #define b2Dot(a, b) (a).dot(b) diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h index e085c40892..dbe82fd61f 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -24,6 +24,7 @@ subject to the following restrictions: #define WANTS_DEACTIVATION 3 #define DISABLE_DEACTIVATION 4 #define DISABLE_SIMULATION 5 +#define FIXED_BASE_MULTI_BODY 6 struct btBroadphaseProxy; class btCollisionShape; @@ -304,7 +305,7 @@ public: SIMD_FORCE_INLINE bool isActive() const { - return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); + return ((getActivationState() != FIXED_BASE_MULTI_BODY) && (getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); } void setRestitution(btScalar rest) diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 71184f36ac..f74dcabc54 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -1037,7 +1037,7 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback m_castShape(castShape) { btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin() - m_convexFromTrans.getOrigin()); - btVector3 rayDir = unnormalizedRayDir.normalized(); + btVector3 rayDir = unnormalizedRayDir.fuzzyZero() ? btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0)) : unnormalizedRayDir.normalized(); ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; @@ -1294,9 +1294,7 @@ public: btVector3 normalColor(1, 1, 0); m_debugDrawer->drawLine(center, center + normal, normalColor); } - m_debugDrawer->drawLine(wv0, wv1, m_color); - m_debugDrawer->drawLine(wv1, wv2, m_color); - m_debugDrawer->drawLine(wv2, wv0, m_color); + m_debugDrawer->drawTriangle(wv0, wv1, wv2, m_color, 1.0); } }; diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index cab6980b65..01bf7f67f5 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -18,12 +18,57 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" btHeightfieldTerrainShape::btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, + const float* heightfieldData, btScalar minHeight, btScalar maxHeight, + int upAxis, bool flipQuadEdges) + : m_userValue3(0), m_triangleInfoMap(0) +{ + initialize(heightStickWidth, heightStickLength, heightfieldData, + /*heightScale=*/1, minHeight, maxHeight, upAxis, PHY_FLOAT, + flipQuadEdges); +} + +btHeightfieldTerrainShape::btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, const double* heightfieldData, + btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges) + : m_userValue3(0), m_triangleInfoMap(0) +{ + initialize(heightStickWidth, heightStickLength, heightfieldData, + /*heightScale=*/1, minHeight, maxHeight, upAxis, PHY_DOUBLE, + flipQuadEdges); +} + +btHeightfieldTerrainShape::btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, const short* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges) + : m_userValue3(0), m_triangleInfoMap(0) +{ + initialize(heightStickWidth, heightStickLength, heightfieldData, + heightScale, minHeight, maxHeight, upAxis, PHY_SHORT, + flipQuadEdges); +} + +btHeightfieldTerrainShape::btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, const unsigned char* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges) + : m_userValue3(0), m_triangleInfoMap(0) +{ + initialize(heightStickWidth, heightStickLength, heightfieldData, + heightScale, minHeight, maxHeight, upAxis, PHY_UCHAR, + flipQuadEdges); +} + +btHeightfieldTerrainShape::btHeightfieldTerrainShape( int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, PHY_ScalarType hdt, bool flipQuadEdges) :m_userValue3(0), m_triangleInfoMap(0) { + // legacy constructor: Assumes PHY_FLOAT means btScalar. +#ifdef BT_USE_DOUBLE_PRECISION + if (hdt == PHY_FLOAT) hdt = PHY_DOUBLE; +#endif initialize(heightStickWidth, heightStickLength, heightfieldData, heightScale, minHeight, maxHeight, upAxis, hdt, flipQuadEdges); @@ -33,9 +78,12 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int h : m_userValue3(0), m_triangleInfoMap(0) { - // legacy constructor: support only float or unsigned char, - // and min height is zero + // legacy constructor: support only btScalar or unsigned char data, + // and min height is zero. PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR; +#ifdef BT_USE_DOUBLE_PRECISION + if (hdt == PHY_FLOAT) hdt = PHY_DOUBLE; +#endif btScalar minHeight = 0.0f; // previously, height = uchar * maxHeight / 65535. @@ -59,7 +107,7 @@ void btHeightfieldTerrainShape::initialize( // btAssert(heightScale) -- do we care? Trust caller here btAssert(minHeight <= maxHeight); // && "bad min/max height"); btAssert(upAxis >= 0 && upAxis < 3); // && "bad upAxis--should be in range [0,2]"); - btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT); // && "Bad height data type enum"); + btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_DOUBLE || hdt != PHY_SHORT); // && "Bad height data type enum"); // initialize member variables m_shapeType = TERRAIN_SHAPE_PROXYTYPE; @@ -152,6 +200,12 @@ btHeightfieldTerrainShape::getRawHeightFieldValue(int x, int y) const break; } + case PHY_DOUBLE: + { + val = m_heightfieldDataDouble[(y * m_heightStickWidth) + x]; + break; + } + case PHY_UCHAR: { unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y * m_heightStickWidth) + x]; @@ -232,6 +286,30 @@ getQuantized( return (int)(x + 0.5); } +// Equivalent to std::minmax({a, b, c}). +// Performs at most 3 comparisons. +static btHeightfieldTerrainShape::Range minmaxRange(btScalar a, btScalar b, btScalar c) +{ + if (a > b) + { + if (b > c) + return btHeightfieldTerrainShape::Range(c, a); + else if (a > c) + return btHeightfieldTerrainShape::Range(b, a); + else + return btHeightfieldTerrainShape::Range(b, c); + } + else + { + if (a > c) + return btHeightfieldTerrainShape::Range(c, b); + else if (b > c) + return btHeightfieldTerrainShape::Range(a, b); + else + return btHeightfieldTerrainShape::Range(a, c); + } +} + /// given input vector, return quantized version /** This routine is basically determining the gridpoint indices for a given @@ -334,7 +412,8 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback } // TODO If m_vboundsGrid is available, use it to determine if we really need to process this area - + + const Range aabbUpRange(aabbMin[m_upAxis], aabbMax[m_upAxis]); for (int j = startJ; j < endJ; j++) { for (int x = startX; x < endX; x++) @@ -349,29 +428,51 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j + x) & 1)) || (m_useZigzagSubdivision && !(j & 1))) { - //first triangle getVertex(x, j, vertices[indices[0]]); getVertex(x, j + 1, vertices[indices[1]]); getVertex(x + 1, j + 1, vertices[indices[2]]); - callback->processTriangle(vertices, 2 * x, j); - //second triangle - // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman - getVertex(x + 1, j + 1, vertices[indices[1]]); + + // Skip triangle processing if the triangle is out-of-AABB. + Range upRange = minmaxRange(vertices[0][m_upAxis], vertices[1][m_upAxis], vertices[2][m_upAxis]); + + if (upRange.overlaps(aabbUpRange)) + callback->processTriangle(vertices, 2 * x, j); + + // already set: getVertex(x, j, vertices[indices[0]]) + + // equivalent to: getVertex(x + 1, j + 1, vertices[indices[1]]); + vertices[indices[1]] = vertices[indices[2]]; + getVertex(x + 1, j, vertices[indices[2]]); - callback->processTriangle(vertices, 2 * x+1, j); + upRange.min = btMin(upRange.min, vertices[indices[2]][m_upAxis]); + upRange.max = btMax(upRange.max, vertices[indices[2]][m_upAxis]); + + if (upRange.overlaps(aabbUpRange)) + callback->processTriangle(vertices, 2 * x + 1, j); } else { - //first triangle getVertex(x, j, vertices[indices[0]]); getVertex(x, j + 1, vertices[indices[1]]); getVertex(x + 1, j, vertices[indices[2]]); - callback->processTriangle(vertices, 2 * x, j); - //second triangle - getVertex(x + 1, j, vertices[indices[0]]); - //getVertex(x,j+1,vertices[1]); + + // Skip triangle processing if the triangle is out-of-AABB. + Range upRange = minmaxRange(vertices[0][m_upAxis], vertices[1][m_upAxis], vertices[2][m_upAxis]); + + if (upRange.overlaps(aabbUpRange)) + callback->processTriangle(vertices, 2 * x, j); + + // already set: getVertex(x, j + 1, vertices[indices[1]]); + + // equivalent to: getVertex(x + 1, j, vertices[indices[0]]); + vertices[indices[0]] = vertices[indices[2]]; + getVertex(x + 1, j + 1, vertices[indices[2]]); - callback->processTriangle(vertices, 2 * x+1, j); + upRange.min = btMin(upRange.min, vertices[indices[2]][m_upAxis]); + upRange.max = btMax(upRange.max, vertices[indices[2]][m_upAxis]); + + if (upRange.overlaps(aabbUpRange)) + callback->processTriangle(vertices, 2 * x + 1, j); } } } @@ -846,4 +947,4 @@ void btHeightfieldTerrainShape::buildAccelerator(int chunkSize) void btHeightfieldTerrainShape::clearAccelerator() { m_vboundsGrid.clear(); -}
\ No newline at end of file +} diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 2cf3c00721..7e251fa71e 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -50,17 +50,15 @@ subject to the following restrictions: The heightfield heights are determined from the data type used for the heightfieldData array. - - PHY_UCHAR: height at a point is the uchar value at the + - unsigned char: height at a point is the uchar value at the grid point, multipled by heightScale. uchar isn't recommended because of its inability to deal with negative values, and low resolution (8-bit). - - PHY_SHORT: height at a point is the short int value at that grid + - short: height at a point is the short int value at that grid point, multipled by heightScale. - - PHY_FLOAT: height at a point is the float value at that grid - point. heightScale is ignored when using the float heightfield - data type. + - float or dobule: height at a point is the value at that grid point. Whatever the caller specifies as minHeight and maxHeight will be honored. The class will not inspect the heightfield to discover the actual minimum @@ -75,6 +73,14 @@ btHeightfieldTerrainShape : public btConcaveShape public: struct Range { + Range() {} + Range(btScalar min, btScalar max) : min(min), max(max) {} + + bool overlaps(const Range& other) const + { + return !(min > other.max || max < other.min); + } + btScalar min; btScalar max; }; @@ -95,7 +101,8 @@ protected: union { const unsigned char* m_heightfieldDataUnsignedChar; const short* m_heightfieldDataShort; - const btScalar* m_heightfieldDataFloat; + const float* m_heightfieldDataFloat; + const double* m_heightfieldDataDouble; const void* m_heightfieldDataUnknown; }; @@ -135,11 +142,33 @@ protected: public: BT_DECLARE_ALIGNED_ALLOCATOR(); - /// preferred constructor + /// preferred constructors + btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, + const float* heightfieldData, btScalar minHeight, btScalar maxHeight, + int upAxis, bool flipQuadEdges); + btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, + const double* heightfieldData, btScalar minHeight, btScalar maxHeight, + int upAxis, bool flipQuadEdges); + btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, + const short* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight, + int upAxis, bool flipQuadEdges); + btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, + const unsigned char* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight, + int upAxis, bool flipQuadEdges); + + /// legacy constructor /** This constructor supports a range of heightfield data types, and allows for a non-zero minimum height value. heightScale is needed for any integer-based heightfield data types. + + This legacy constructor considers `PHY_FLOAT` to mean `btScalar`. + With `BT_USE_DOUBLE_PRECISION`, it will expect `heightfieldData` + to be double-precision. */ btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar heightScale, @@ -150,7 +179,7 @@ public: /// legacy constructor /** The legacy constructor assumes the heightfield has a minimum height - of zero. Only unsigned char or floats are supported. For legacy + of zero. Only unsigned char or btScalar data are supported. For legacy compatibility reasons, heightScale is calculated as maxHeight / 65535 (and is only used when useFloatData = false). */ @@ -218,4 +247,4 @@ public: } }; -#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H
\ No newline at end of file +#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp index 7cb92fa3b4..d7588aedc8 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp @@ -33,8 +33,8 @@ namespace { -const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) -const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds +const btScalar INITIAL_SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) +const btScalar INITIAL_SLEEP_TIMEOUT = btScalar(2); // in seconds } // namespace void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame @@ -110,6 +110,9 @@ btMultiBody::btMultiBody(int n_links, m_canSleep(canSleep), m_canWakeup(true), m_sleepTimer(0), + m_sleepEpsilon(INITIAL_SLEEP_EPSILON), + m_sleepTimeout(INITIAL_SLEEP_TIMEOUT), + m_userObjectPointer(0), m_userIndex2(-1), m_userIndex(-1), @@ -1411,7 +1414,7 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV } } -void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const +void btMultiBody::mulMatrix(const btScalar *pA, const btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const { for (int row = 0; row < rowsA; row++) { @@ -2104,10 +2107,10 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) motion += m_realBuf[i] * m_realBuf[i]; } - if (motion < SLEEP_EPSILON) + if (motion < m_sleepEpsilon) { m_sleepTimer += timestep; - if (m_sleepTimer > SLEEP_TIMEOUT) + if (m_sleepTimer > m_sleepTimeout) { goToSleep(); } diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h index 5a3efc9414..345970d261 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h @@ -545,7 +545,10 @@ public: { m_canWakeup = canWakeup; } - bool isAwake() const { return m_awake; } + bool isAwake() const + { + return m_awake; + } void wakeUp(); void goToSleep(); void checkMotionAndSleepIfRequired(btScalar timestep); @@ -726,6 +729,17 @@ public: bool isLinkAndAllAncestorsKinematic(const int i) const; + void setSleepThreshold(btScalar sleepThreshold) + { + m_sleepEpsilon = sleepThreshold; + } + + void setSleepTimeout(btScalar sleepTimeout) + { + this->m_sleepTimeout = sleepTimeout; + } + + private: btMultiBody(const btMultiBody &); // not implemented void operator=(const btMultiBody &); // not implemented @@ -745,7 +759,7 @@ private: } } - void mulMatrix(btScalar * pA, btScalar * pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; + void mulMatrix(const btScalar *pA, const btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; private: btMultiBodyLinkCollider *m_baseCollider; //can be NULL @@ -801,6 +815,8 @@ private: bool m_canSleep; bool m_canWakeup; btScalar m_sleepTimer; + btScalar m_sleepEpsilon; + btScalar m_sleepTimeout; void *m_userObjectPointer; int m_userIndex2; diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 1ba5861145..00d5fd5609 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -61,7 +61,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra btScalar lowerLimit, btScalar upperLimit, bool angConstraint, btScalar relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip, + btScalar damping) { solverConstraint.m_multiBodyA = m_bodyA; solverConstraint.m_multiBodyB = m_bodyB; @@ -348,7 +349,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra { btScalar positionalError = 0.f; - btScalar velocityError = desiredVelocity - rel_vel; // * damping; + btScalar velocityError = (desiredVelocity - rel_vel) * damping; btScalar erp = infoGlobal.m_erp2; diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 4a6007ee3e..1aaa07b69e 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -94,7 +94,7 @@ protected: bool angConstraint = false, btScalar relaxation = 1.f, - bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0, btScalar damping = 1.0); public: BT_DECLARE_ALIGNED_ALLOCATOR(); diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index fef95f0c4e..e7af332eb3 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -137,7 +137,14 @@ void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) btMultiBodyLinkCollider* col = body->getBaseCollider(); if (col && col->getActivationState() == ACTIVE_TAG) { - col->setActivationState(WANTS_DEACTIVATION); + if (body->hasFixedBase()) + { + col->setActivationState(FIXED_BASE_MULTI_BODY); + } else + { + col->setActivationState(WANTS_DEACTIVATION); + } + col->setDeactivationTime(0.f); } for (int b = 0; b < body->getNumLinks(); b++) diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 4372489fa1..fec9b03213 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -43,6 +43,7 @@ void btMultiBodyJointMotor::finalizeMultiDof() unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); // row 0: the lower bound + // row 0: the lower bound jacobianA(0)[offset] = 1; m_numDofsFinalized = m_jacSizeBoth; diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp index 5c20d2a0d4..00a7ef3579 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp @@ -26,10 +26,13 @@ btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR), m_desiredVelocity(0, 0, 0), m_desiredPosition(0,0,0,1), - m_kd(1.), - m_kp(0.2), + m_use_multi_dof_params(false), + m_kd(1., 1., 1.), + m_kp(0.2, 0.2, 0.2), m_erp(1), - m_rhsClamp(SIMD_INFINITY) + m_rhsClamp(SIMD_INFINITY), + m_maxAppliedImpulseMultiDof(maxMotorImpulse, maxMotorImpulse, maxMotorImpulse), + m_damping(1.0, 1.0, 1.0) { m_maxAppliedImpulse = maxMotorImpulse; @@ -45,6 +48,7 @@ void btMultiBodySphericalJointMotor::finalizeMultiDof() unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); // row 0: the lower bound + // row 0: the lower bound jacobianA(0)[offset] = 1; m_numDofsFinalized = m_jacSizeBoth; @@ -138,7 +142,8 @@ btQuaternion relRot = currentQuat.inverse() * desiredQuat; btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; btScalar desiredVelocity = this->m_desiredVelocity[row]; - btScalar velocityError = desiredVelocity - currentVelocity; + double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0]; + btScalar velocityError = (desiredVelocity - currentVelocity) * kd; btMatrix3x3 frameAworld; frameAworld.setIdentity(); @@ -151,12 +156,16 @@ btQuaternion relRot = currentQuat.inverse() * desiredQuat; case btMultibodyLink::eSpherical: { btVector3 constraintNormalAng = frameAworld.getColumn(row % 3); - posError = m_kp*angleDiff[row % 3]; + double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0]; + posError = kp*angleDiff[row % 3]; + double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse; fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, btVector3(0,0,0), dummy, dummy, posError, infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse, true); + -max_applied_impulse, max_applied_impulse, true, + 1.0, false, 0, 0, + m_damping[row % 3]); constraintRow.m_orgConstraint = this; constraintRow.m_orgDofIndex = row; break; diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h index 621beab5a4..bdeccc2e24 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h @@ -26,10 +26,13 @@ class btMultiBodySphericalJointMotor : public btMultiBodyConstraint protected: btVector3 m_desiredVelocity; btQuaternion m_desiredPosition; - btScalar m_kd; - btScalar m_kp; + bool m_use_multi_dof_params; + btVector3 m_kd; + btVector3 m_kp; btScalar m_erp; btScalar m_rhsClamp; //maximum error + btVector3 m_maxAppliedImpulseMultiDof; + btVector3 m_damping; public: btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse); @@ -44,16 +47,32 @@ public: btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal); - virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.f) + virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.0) + { + m_desiredVelocity = velTarget; + m_kd = btVector3(kd, kd, kd); + m_use_multi_dof_params = false; + } + + virtual void setVelocityTargetMultiDof(const btVector3& velTarget, const btVector3& kd = btVector3(1.0, 1.0, 1.0)) { m_desiredVelocity = velTarget; m_kd = kd; + m_use_multi_dof_params = true; } - virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp = 1.f) + virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp =1.f) + { + m_desiredPosition = posTarget; + m_kp = btVector3(kp, kp, kp); + m_use_multi_dof_params = false; + } + + virtual void setPositionTargetMultiDof(const btQuaternion& posTarget, const btVector3& kp = btVector3(1.f, 1.f, 1.f)) { m_desiredPosition = posTarget; m_kp = kp; + m_use_multi_dof_params = true; } virtual void setErp(btScalar erp) @@ -68,6 +87,28 @@ public: { m_rhsClamp = rhsClamp; } + + btScalar getMaxAppliedImpulseMultiDof(int i) const + { + return m_maxAppliedImpulseMultiDof[i]; + } + + void setMaxAppliedImpulseMultiDof(const btVector3& maxImp) + { + m_maxAppliedImpulseMultiDof = maxImp; + m_use_multi_dof_params = true; + } + + btScalar getDamping(int i) const + { + return m_damping[i]; + } + + void setDamping(const btVector3& damping) + { + m_damping = damping; + } + virtual void debugDraw(class btIDebugDraw* drawer) { //todo(erwincoumans) diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp index 5d864f2757..ed4e0b686d 100644 --- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp +++ b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -532,7 +532,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) J_transpose = J.transpose(); btMatrixXu& tmp = m_scratchTmp; - + //Minv.printMatrix("Minv="); { { BT_PROFILE("J*Minv"); @@ -543,7 +543,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) m_A = tmp * J_transpose; } } - + //J.printMatrix("J"); if (1) { // add cfm to the diagonal of m_A diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp index 4b11fccecb..e81680f019 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp @@ -405,6 +405,10 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt) for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + /* Clear contacts */ + psb->m_nodeRigidContacts.resize(0); + psb->m_faceRigidContacts.resize(0); + psb->m_faceNodeContacts.resize(0); if (psb->isActive()) { @@ -472,10 +476,6 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d { psb->updateFaceTree(true, true); } - /* Clear contacts */ - psb->m_nodeRigidContacts.resize(0); - psb->m_faceRigidContacts.resize(0); - psb->m_faceNodeContacts.resize(0); /* Optimize dbvt's */ // psb->m_ndbvt.optimizeIncremental(1); // psb->m_fdbvt.optimizeIncremental(1); diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h index d218d96214..697408355c 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h @@ -29,7 +29,7 @@ class btDeformableMousePickingForce : public btDeformableLagrangianForce public: typedef btAlignedObjectArray<btVector3> TVStack; - btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce) + btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, const btVector3& mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce) { } diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBody.h b/thirdparty/bullet/BulletSoftBody/btSoftBody.h index f578487b8c..dfde8fd1e4 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBody.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBody.h @@ -1317,8 +1317,8 @@ public: } for (int k = 0; k < m_faceNodeContacts.size(); ++k) { - int i = indices[k]; - btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[i]; + int idx = indices[k]; + btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[idx]; btSoftBody::Node* node = c.m_node; btSoftBody::Face* face = c.m_face; const btVector3& w = c.m_bary; diff --git a/thirdparty/bullet/LinearMath/btIDebugDraw.h b/thirdparty/bullet/LinearMath/btIDebugDraw.h index 82ec19a69b..df4db2ff5a 100644 --- a/thirdparty/bullet/LinearMath/btIDebugDraw.h +++ b/thirdparty/bullet/LinearMath/btIDebugDraw.h @@ -4,8 +4,8 @@ Copyright (c) 2003-2009 Erwin Coumans 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, +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. @@ -21,7 +21,7 @@ subject to the following restrictions: ///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations. ///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld. -///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum. +///A class that implements the btIDebugDraw interface will need to provide non-empty implementations of the the drawLine and getDebugMode methods at a minimum. ///For color arguments the X,Y,Z components refer to Red, Green and Blue each in the range [0..1] class btIDebugDraw { diff --git a/thirdparty/bullet/LinearMath/btScalar.h b/thirdparty/bullet/LinearMath/btScalar.h index 0402146af1..b239217bb6 100644 --- a/thirdparty/bullet/LinearMath/btScalar.h +++ b/thirdparty/bullet/LinearMath/btScalar.h @@ -25,7 +25,7 @@ subject to the following restrictions: #include <float.h> /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ -#define BT_BULLET_VERSION 308 +#define BT_BULLET_VERSION 317 inline int btGetVersion() { diff --git a/thirdparty/bullet/LinearMath/btSerializer.h b/thirdparty/bullet/LinearMath/btSerializer.h index 4d1c760e24..f18442f23d 100644 --- a/thirdparty/bullet/LinearMath/btSerializer.h +++ b/thirdparty/bullet/LinearMath/btSerializer.h @@ -480,8 +480,8 @@ public: } buffer[9] = '3'; - buffer[10] = '0'; - buffer[11] = '8'; + buffer[10] = '1'; + buffer[11] = '7'; } virtual void startSerialization() @@ -499,7 +499,6 @@ public: writeDNA(); //if we didn't pre-allocate a buffer, we need to create a contiguous buffer now - int mysize = 0; if (!m_totalSize) { if (m_buffer) @@ -511,14 +510,12 @@ public: unsigned char* currentPtr = m_buffer; writeHeader(m_buffer); currentPtr += BT_HEADER_LENGTH; - mysize += BT_HEADER_LENGTH; for (int i = 0; i < m_chunkPtrs.size(); i++) { int curLength = sizeof(btChunk) + m_chunkPtrs[i]->m_length; memcpy(currentPtr, m_chunkPtrs[i], curLength); btAlignedFree(m_chunkPtrs[i]); currentPtr += curLength; - mysize += curLength; } } diff --git a/thirdparty/bullet/VERSION.txt b/thirdparty/bullet/VERSION.txt new file mode 100644 index 0000000000..78c8a7428a --- /dev/null +++ b/thirdparty/bullet/VERSION.txt @@ -0,0 +1 @@ +3.17 diff --git a/thirdparty/bullet/patches/bullet-fix-warnings.patch b/thirdparty/bullet/patches/bullet-fix-warnings.patch new file mode 100644 index 0000000000..69cde1b16e --- /dev/null +++ b/thirdparty/bullet/patches/bullet-fix-warnings.patch @@ -0,0 +1,42 @@ +diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBody.h b/thirdparty/bullet/BulletSoftBody/btSoftBody.h +index f578487b8c..dfde8fd1e4 100644 +--- a/thirdparty/bullet/BulletSoftBody/btSoftBody.h ++++ b/thirdparty/bullet/BulletSoftBody/btSoftBody.h +@@ -1317,8 +1317,8 @@ public: + } + for (int k = 0; k < m_faceNodeContacts.size(); ++k) + { +- int i = indices[k]; +- btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[i]; ++ int idx = indices[k]; ++ btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[idx]; + btSoftBody::Node* node = c.m_node; + btSoftBody::Face* face = c.m_face; + const btVector3& w = c.m_bary; +diff --git a/thirdparty/bullet/LinearMath/btSerializer.h b/thirdparty/bullet/LinearMath/btSerializer.h +index ce4fc34e20..11592d2ccd 100644 +--- a/thirdparty/bullet/LinearMath/btSerializer.h ++++ b/thirdparty/bullet/LinearMath/btSerializer.h +@@ -499,7 +499,6 @@ public: + writeDNA(); + + //if we didn't pre-allocate a buffer, we need to create a contiguous buffer now +- int mysize = 0; + if (!m_totalSize) + { + if (m_buffer) +@@ -511,14 +510,12 @@ public: + unsigned char* currentPtr = m_buffer; + writeHeader(m_buffer); + currentPtr += BT_HEADER_LENGTH; +- mysize += BT_HEADER_LENGTH; + for (int i = 0; i < m_chunkPtrs.size(); i++) + { + int curLength = sizeof(btChunk) + m_chunkPtrs[i]->m_length; + memcpy(currentPtr, m_chunkPtrs[i], curLength); + btAlignedFree(m_chunkPtrs[i]); + currentPtr += curLength; +- mysize += curLength; + } + } + |