TorqueGameEngines/Torque3D
1
0
Fork 0
mirror of https://github.com/TorqueGameEngines/Torque3D.git synced 2026-07-12 15:14:35 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

Bullet 2.82 update

Browse source
This commit is contained in:
rextimmy 2014-06-10 22:40:30 +10:00
parent d0a64026b0
commit 416c50690e
146 changed files with 12202 additions and 1422 deletions
Download patch file Download diff file Expand all files Collapse all files

43
Engine/lib/bullet/src/BulletDynamics/CMakeLists.txt
View file

@ -6,6 +6,7 @@ SET(BulletDynamics_SRCS
Character/btKinematicCharacterController.cpp
ConstraintSolver/btConeTwistConstraint.cpp
ConstraintSolver/btContactConstraint.cpp
ConstraintSolver/btFixedConstraint.cpp
ConstraintSolver/btGearConstraint.cpp
ConstraintSolver/btGeneric6DofConstraint.cpp
ConstraintSolver/btGeneric6DofSpringConstraint.cpp
@ -23,6 +24,15 @@ SET(BulletDynamics_SRCS
Dynamics/Bullet-C-API.cpp
Vehicle/btRaycastVehicle.cpp
Vehicle/btWheelInfo.cpp
Featherstone/btMultiBody.cpp
Featherstone/btMultiBodyConstraintSolver.cpp
Featherstone/btMultiBodyDynamicsWorld.cpp
Featherstone/btMultiBodyJointLimitConstraint.cpp
Featherstone/btMultiBodyConstraint.cpp
Featherstone/btMultiBodyPoint2Point.cpp
Featherstone/btMultiBodyJointMotor.cpp
MLCPSolvers/btDantzigLCP.cpp
MLCPSolvers/btMLCPSolver.cpp
)
SET(Root_HDRS
@ -34,6 +44,7 @@ SET(ConstraintSolver_HDRS
ConstraintSolver/btConstraintSolver.h
ConstraintSolver/btContactConstraint.h
ConstraintSolver/btContactSolverInfo.h
ConstraintSolver/btFixedConstraint.h
ConstraintSolver/btGearConstraint.h
ConstraintSolver/btGeneric6DofConstraint.h
ConstraintSolver/btGeneric6DofSpringConstraint.h
@ -62,6 +73,29 @@ SET(Vehicle_HDRS
Vehicle/btWheelInfo.h
)
SET(Featherstone_HDRS
Featherstone/btMultiBody.h
Featherstone/btMultiBodyConstraintSolver.h
Featherstone/btMultiBodyDynamicsWorld.h
Featherstone/btMultiBodyLink.h
Featherstone/btMultiBodyLinkCollider.h
Featherstone/btMultiBodySolverConstraint.h
Featherstone/btMultiBodyConstraint.h
Featherstone/btMultiBodyJointLimitConstraint.h
Featherstone/btMultiBodyConstraint.h
Featherstone/btMultiBodyPoint2Point.h
Featherstone/btMultiBodyJointMotor.h
)
SET(MLCPSolvers_HDRS
MLCPSolvers/btDantzigLCP.h
MLCPSolvers/btDantzigSolver.h
MLCPSolvers/btMLCPSolver.h
MLCPSolvers/btMLCPSolverInterface.h
MLCPSolvers/btPATHSolver.h
MLCPSolvers/btSolveProjectedGaussSeidel.h
)
SET(Character_HDRS
Character/btCharacterControllerInterface.h
Character/btKinematicCharacterController.h
@ -75,6 +109,8 @@ SET(BulletDynamics_HDRS
${Dynamics_HDRS}
${Vehicle_HDRS}
${Character_HDRS}
${Featherstone_HDRS}
${MLCPSolvers_HDRS}
)
@ -91,7 +127,9 @@ IF (INSTALL_LIBS)
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
INSTALL(TARGETS BulletDynamics DESTINATION .)
ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
INSTALL(TARGETS BulletDynamics DESTINATION lib${LIB_SUFFIX})
INSTALL(TARGETS BulletDynamics RUNTIME DESTINATION bin
LIBRARY DESTINATION lib${LIB_SUFFIX}
ARCHIVE DESTINATION lib${LIB_SUFFIX})
INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN
".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
@ -108,7 +146,8 @@ DESTINATION ${INCLUDE_INSTALL_DIR}/BulletDynamics)
SET_PROPERTY(SOURCE ${Dynamics_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Dynamics)
SET_PROPERTY(SOURCE ${Vehicle_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Vehicle)
SET_PROPERTY(SOURCE ${Character_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Character)
SET_PROPERTY(SOURCE ${Featherstone_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Featherstone)
SET_PROPERTY(SOURCE ${MLCPSolvers_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/MLCPSolvers)
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
ENDIF (INSTALL_LIBS)

3
Engine/lib/bullet/src/BulletDynamics/Character/btCharacterControllerInterface.h
View file

@ -31,7 +31,7 @@ public:
virtual void setWalkDirection(const btVector3& walkDirection) = 0;
virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
virtual void reset () = 0;
virtual void reset ( btCollisionWorld* collisionWorld ) = 0;
virtual void warp (const btVector3& origin) = 0;
virtual void preStep ( btCollisionWorld* collisionWorld) = 0;
@ -40,6 +40,7 @@ public:
virtual void jump () = 0;
virtual bool onGround () const = 0;
virtual void setUpInterpolate (bool value) = 0;
};
#endif //BT_CHARACTER_CONTROLLER_INTERFACE_H

169
Engine/lib/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp
View file

@ -14,6 +14,7 @@ subject to the following restrictions:
*/
#include <stdio.h>
#include "LinearMath/btIDebugDraw.h"
#include "BulletCollision/CollisionDispatch/btGhostObject.h"
#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
@ -77,6 +78,9 @@ public:
if (convexResult.m_hitCollisionObject == m_me)
return btScalar(1.0);
if (!convexResult.m_hitCollisionObject->hasContactResponse())
return btScalar(1.0);
btVector3 hitNormalWorld;
if (normalInWorldSpace)
{
@ -146,7 +150,11 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho
m_jumpSpeed = 10.0; // ?
m_wasOnGround = false;
m_wasJumping = false;
m_interpolateUp = true;
setMaxSlope(btRadians(45.0));
m_currentStepOffset = 0;
full_drop = false;
bounce_fix = false;
}
btKinematicCharacterController::~btKinematicCharacterController ()
@ -187,6 +195,12 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
m_manifoldArray.resize(0);
btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
continue;
if (collisionPair->m_algorithm)
collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
@ -260,7 +274,10 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
{
// we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
if (m_interpolateUp == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
m_currentPosition = m_targetPosition;
}
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
@ -325,7 +342,8 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
{
if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
{
updateTargetPositionBasedOnCollision (m_touchingNormal);
//interferes with step movement
//updateTargetPositionBasedOnCollision (m_touchingNormal);
}
}
@ -397,7 +415,8 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
{
btTransform start, end;
btTransform start, end, end_double;
bool runonce = false;
// phase 3: down
/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
@ -406,44 +425,124 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
m_targetPosition -= (step_drop + gravity_drop);*/
btVector3 orig_position = m_targetPosition;
btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
if(downVelocity > 0.0 && downVelocity < m_stepHeight
if(downVelocity > 0.0 && downVelocity > m_fallSpeed
&& (m_wasOnGround || !m_wasJumping))
{
downVelocity = m_stepHeight;
}
downVelocity = m_fallSpeed;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
start.setIdentity ();
end.setIdentity ();
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
while (1)
{
start.setIdentity ();
end.setIdentity ();
end_double.setIdentity ();
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
//set double test for 2x the step drop, to check for a large drop vs small drop
end_double.setOrigin (m_targetPosition - step_drop);
if (m_useGhostObjectSweepTest)
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (!callback.hasHit())
{
//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
} else
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (!callback.hasHit())
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
}
if (m_useGhostObjectSweepTest)
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} else
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
bool has_hit = false;
if (bounce_fix == true)
has_hit = callback.hasHit() || callback2.hasHit();
else
has_hit = callback2.hasHit();
if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
&& (m_wasOnGround || !m_wasJumping))
{
//redo the velocity calculation when falling a small amount, for fast stairs motion
//for larger falls, use the smoother/slower interpolated movement by not touching the target position
m_targetPosition = orig_position;
downVelocity = m_stepHeight;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
runonce = true;
continue; //re-run previous tests
}
break;
}
if (callback.hasHit())
if (callback.hasHit() || runonce == true)
{
// we dropped a fraction of the height -> hit floor
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
if (bounce_fix == true)
{
if (full_drop == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
}
else
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
full_drop = false;
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_wasJumping = false;
} else {
// we dropped the full height
full_drop = true;
if (bounce_fix == true)
{
downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
{
m_targetPosition += step_drop; //undo previous target change
downVelocity = m_fallSpeed;
step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
}
}
//printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());
m_currentPosition = m_targetPosition;
}
}
@ -476,13 +575,24 @@ btScalar timeInterval
m_useWalkDirection = false;
m_walkDirection = velocity;
m_normalizedDirection = getNormalizedVector(m_walkDirection);
m_velocityTimeInterval = timeInterval;
m_velocityTimeInterval += timeInterval;
}
void btKinematicCharacterController::reset ()
void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
{
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_wasOnGround = false;
m_wasJumping = false;
m_walkDirection.setValue(0,0,0);
m_velocityTimeInterval = 0.0;
//clear pair cache
btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
while (cache->getOverlappingPairArray().size() > 0)
{
cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
}
}
void btKinematicCharacterController::warp (const btVector3& origin)
@ -653,3 +763,8 @@ btVector3* btKinematicCharacterController::getUpAxisDirections()
void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
{
}
void btKinematicCharacterController::setUpInterpolate(bool value)
{
m_interpolateUp = value;
}

6
Engine/lib/bullet/src/BulletDynamics/Character/btKinematicCharacterController.h
View file

@ -81,6 +81,9 @@ protected:
int m_upAxis;
static btVector3* getUpAxisDirections();
bool m_interpolateUp;
bool full_drop;
bool bounce_fix;
btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
@ -133,7 +136,7 @@ public:
virtual void setVelocityForTimeInterval(const btVector3& velocity,
btScalar timeInterval);
void reset ();
void reset ( btCollisionWorld* collisionWorld );
void warp (const btVector3& origin);
void preStep ( btCollisionWorld* collisionWorld);
@ -161,6 +164,7 @@ public:
}
bool onGround () const;
void setUpInterpolate (bool value);
};
#endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H

10
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
View file

@ -53,6 +53,7 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform&
m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
{
m_rbBFrame = m_rbAFrame;
m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
init();
}
@ -136,6 +137,9 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[info->rowskip+1] = -1;
info->m_J2linearAxis[2*info->rowskip+2] = -1;
btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
@ -725,7 +729,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
{
if(m_swingSpan1 < m_fixThresh)
{ // hinge around Y axis
if(!(btFuzzyZero(y)))
// if(!(btFuzzyZero(y)))
if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
{
m_solveSwingLimit = true;
if(m_swingSpan2 >= m_fixThresh)
@ -747,7 +752,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
}
else
{ // hinge around Z axis
if(!btFuzzyZero(z))
// if(!btFuzzyZero(z))
if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
{
m_solveSwingLimit = true;
if(m_swingSpan1 >= m_fixThresh)

62
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
View file

@ -40,6 +40,15 @@ and swing 1 and 2 are along the z and y axes respectively.
#include "btJacobianEntry.h"
#include "btTypedConstraint.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btConeTwistConstraintData2 btConeTwistConstraintDoubleData
#define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData"
#else
#define btConeTwistConstraintData2 btConeTwistConstraintData
#define btConeTwistConstraintDataName "btConeTwistConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
class btRigidBody;
enum btConeTwistFlags
@ -295,7 +304,30 @@ public:
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btConeTwistConstraintDoubleData
{
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame;
btTransformDoubleData m_rbBFrame;
//limits
double m_swingSpan1;
double m_swingSpan2;
double m_twistSpan;
double m_limitSoftness;
double m_biasFactor;
double m_relaxationFactor;
double m_damping;
};
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
///this structure is not used, except for loading pre-2.82 .bullet files
struct btConeTwistConstraintData
{
btTypedConstraintData m_typeConstraintData;
@ -315,12 +347,12 @@ struct btConeTwistConstraintData
char m_pad[4];
};
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
//
SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
{
return sizeof(btConeTwistConstraintData);
return sizeof(btConeTwistConstraintData2);
}
@ -328,21 +360,21 @@ SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() cons
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer;
btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*) dataBuffer;
btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
m_rbAFrame.serializeFloat(cone->m_rbAFrame);
m_rbBFrame.serializeFloat(cone->m_rbBFrame);
m_rbAFrame.serialize(cone->m_rbAFrame);
m_rbBFrame.serialize(cone->m_rbBFrame);
cone->m_swingSpan1 = float(m_swingSpan1);
cone->m_swingSpan2 = float(m_swingSpan2);
cone->m_twistSpan = float(m_twistSpan);
cone->m_limitSoftness = float(m_limitSoftness);
cone->m_biasFactor = float(m_biasFactor);
cone->m_relaxationFactor = float(m_relaxationFactor);
cone->m_damping = float(m_damping);
cone->m_swingSpan1 = m_swingSpan1;
cone->m_swingSpan2 = m_swingSpan2;
cone->m_twistSpan = m_twistSpan;
cone->m_limitSoftness = m_limitSoftness;
cone->m_biasFactor = m_biasFactor;
cone->m_relaxationFactor = m_relaxationFactor;
cone->m_damping = m_damping;
return "btConeTwistConstraintData";
return btConeTwistConstraintDataName;
}

16
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
View file

@ -28,6 +28,14 @@ class btIDebugDraw;
class btStackAlloc;
class btDispatcher;
/// btConstraintSolver provides solver interface
enum btConstraintSolverType
{
BT_SEQUENTIAL_IMPULSE_SOLVER=1,
BT_MLCP_SOLVER=2
};
class btConstraintSolver
{
@ -38,12 +46,16 @@ public:
virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;}
///solve a group of constraints
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0;
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer,btDispatcher* dispatcher) = 0;
virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */, btStackAlloc* /* stackAlloc */) {;}
virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */) {;}
///clear internal cached data and reset random seed
virtual void reset() = 0;
virtual btConstraintSolverType getSolverType() const=0;
};

129
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp Normal file
View file

@ -0,0 +1,129 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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 "btFixedConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h"
#include <new>
btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
{
m_pivotInA = frameInA.getOrigin();
m_pivotInB = frameInB.getOrigin();
m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
}
btFixedConstraint::~btFixedConstraint ()
{
}
void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
{
info->m_numConstraintRows = 6;
info->nub = 6;
}
void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
{
//fix the 3 linear degrees of freedom
const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
btVector3 a1 = worldOrnA*m_pivotInA;
{
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
if (info->m_J2linearAxis)
{
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[info->rowskip+1] = -1;
info->m_J2linearAxis[2*info->rowskip+2] = -1;
}
btVector3 a2 = worldOrnB*m_pivotInB;
{
// btVector3 a2n = -a2;
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// set right hand side for the linear dofs
btScalar k = info->fps * info->erp;
btVector3 linearError = k*(a2+worldPosB-a1-worldPosA);
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = linearError[j];
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
//fix the 3 angular degrees of freedom
int start_row = 3;
int s = info->rowskip;
int start_index = start_row * s;
// 3 rows to make body rotations equal
info->m_J1angularAxis[start_index] = 1;
info->m_J1angularAxis[start_index + s + 1] = 1;
info->m_J1angularAxis[start_index + s*2+2] = 1;
if ( info->m_J2angularAxis)
{
info->m_J2angularAxis[start_index] = -1;
info->m_J2angularAxis[start_index + s+1] = -1;
info->m_J2angularAxis[start_index + s*2+2] = -1;
}
// set right hand side for the angular dofs
btVector3 diff;
btScalar angle;
btMatrix3x3 mrelCur = worldOrnA *worldOrnB.inverse();
btQuaternion qrelCur;
mrelCur.getRotation(qrelCur);
btTransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
diff*=-angle;
for (j=0; j<3; j++)
{
info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
}
}

49
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h Normal file
View file

@ -0,0 +1,49 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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_FIXED_CONSTRAINT_H
#define BT_FIXED_CONSTRAINT_H
#include "btTypedConstraint.h"
ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
{
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btQuaternion m_relTargetAB;
public:
btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
virtual ~btFixedConstraint();
virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info);
virtual void setParam(int num, btScalar value, int axis = -1)
{
btAssert(0);
}
virtual btScalar getParam(int num, int axis = -1) const
{
btAssert(0);
return 0.f;
}
};
#endif //BT_FIXED_CONSTRAINT_H

98
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.h
View file

@ -19,6 +19,18 @@ subject to the following restrictions:
#define BT_GEAR_CONSTRAINT_H
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btGearConstraintData btGearConstraintDoubleData
#define btGearConstraintDataName "btGearConstraintDoubleData"
#else
#define btGearConstraintData btGearConstraintFloatData
#define btGearConstraintDataName "btGearConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
///See Bullet/Demos/ConstraintDemo for an example use.
class btGearConstraint : public btTypedConstraint
@ -39,18 +51,102 @@ public:
///internal method used by the constraint solver, don't use them directly
virtual void getInfo2 (btConstraintInfo2* info);
void setAxisA(btVector3& axisA)
{
m_axisInA = axisA;
}
void setAxisB(btVector3& axisB)
{
m_axisInB = axisB;
}
void setRatio(btScalar ratio)
{
m_ratio = ratio;
}
const btVector3& getAxisA() const
{
return m_axisInA;
}
const btVector3& getAxisB() const
{
return m_axisInB;
}
btScalar getRatio() const
{
return m_ratio;
}
virtual void setParam(int num, btScalar value, int axis = -1)
{
(void) num;
(void) value;
(void) axis;
btAssert(0);
};
}
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const
{
(void) num;
(void) axis;
btAssert(0);
return 0.f;
}
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btGearConstraintFloatData
{
btTypedConstraintFloatData m_typeConstraintData;
btVector3FloatData m_axisInA;
btVector3FloatData m_axisInB;
float m_ratio;
char m_padding[4];
};
struct btGearConstraintDoubleData
{
btTypedConstraintDoubleData m_typeConstraintData;
btVector3DoubleData m_axisInA;
btVector3DoubleData m_axisInB;
double m_ratio;
};
SIMD_FORCE_INLINE int btGearConstraint::calculateSerializeBufferSize() const
{
return sizeof(btGearConstraintData);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);
m_axisInA.serialize( gear->m_axisInA );
m_axisInB.serialize( gear->m_axisInB );
gear->m_ratio = m_ratio;
return btGearConstraintDataName;
}
#endif //BT_GEAR_CONSTRAINT_H

15
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
View file

@ -781,17 +781,16 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
if (powered || limit)
{ // if the joint is powered, or has joint limits, add in the extra row
btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
if(rotational)
{
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
}
if((!rotational))
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
if((!rotational))
{
if (m_useOffsetForConstraintFrame)
{

44
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
View file

@ -35,6 +35,14 @@ class btRigidBody;
#ifdef BT_USE_DOUBLE_PRECISION
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2"
#else
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintData
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
//! Rotation Limit structure for generic joints
class btRotationalLimitMotor
@ -561,7 +569,7 @@ public:
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btGeneric6DofConstraintData
{
btTypedConstraintData m_typeConstraintData;
@ -578,35 +586,51 @@ struct btGeneric6DofConstraintData
int m_useOffsetForConstraintFrame;
};
struct btGeneric6DofConstraintDoubleData2
{
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformDoubleData m_rbBFrame;
btVector3DoubleData m_linearUpperLimit;
btVector3DoubleData m_linearLowerLimit;
btVector3DoubleData m_angularUpperLimit;
btVector3DoubleData m_angularLowerLimit;
int m_useLinearReferenceFrameA;
int m_useOffsetForConstraintFrame;
};
SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
{
return sizeof(btGeneric6DofConstraintData);
return sizeof(btGeneric6DofConstraintData2);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer;
btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
m_frameInA.serializeFloat(dof->m_rbAFrame);
m_frameInB.serializeFloat(dof->m_rbBFrame);
m_frameInA.serialize(dof->m_rbAFrame);
m_frameInB.serialize(dof->m_rbBFrame);
int i;
for (i=0;i<3;i++)
{
dof->m_angularLowerLimit.m_floats[i] = float(m_angularLimits[i].m_loLimit);
dof->m_angularUpperLimit.m_floats[i] = float(m_angularLimits[i].m_hiLimit);
dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]);
dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i];
dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i];
}
dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
return "btGeneric6DofConstraintData";
return btGeneric6DofConstraintDataName;
}

33
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
View file

@ -21,6 +21,15 @@ subject to the following restrictions:
#include "btTypedConstraint.h"
#include "btGeneric6DofConstraint.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintDoubleData2
#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintDoubleData2"
#else
#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintData
#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF
@ -65,7 +74,6 @@ public:
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btGeneric6DofSpringConstraintData
{
btGeneric6DofConstraintData m_6dofData;
@ -76,26 +84,37 @@ struct btGeneric6DofSpringConstraintData
float m_springDamping[6];
};
struct btGeneric6DofSpringConstraintDoubleData2
{
btGeneric6DofConstraintDoubleData2 m_6dofData;
int m_springEnabled[6];
double m_equilibriumPoint[6];
double m_springStiffness[6];
double m_springDamping[6];
};
SIMD_FORCE_INLINE int btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
{
return sizeof(btGeneric6DofSpringConstraintData);
return sizeof(btGeneric6DofSpringConstraintData2);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btGeneric6DofSpringConstraintData* dof = (btGeneric6DofSpringConstraintData*)dataBuffer;
btGeneric6DofSpringConstraintData2* dof = (btGeneric6DofSpringConstraintData2*)dataBuffer;
btGeneric6DofConstraint::serialize(&dof->m_6dofData,serializer);
int i;
for (i=0;i<6;i++)
{
dof->m_equilibriumPoint[i] = (float)m_equilibriumPoint[i];
dof->m_springDamping[i] = (float)m_springDamping[i];
dof->m_equilibriumPoint[i] = m_equilibriumPoint[i];
dof->m_springDamping[i] = m_springDamping[i];
dof->m_springEnabled[i] = m_springEnabled[i]? 1 : 0;
dof->m_springStiffness[i] = (float)m_springStiffness[i];
dof->m_springStiffness[i] = m_springStiffness[i];
}
return "btGeneric6DofSpringConstraintData";
return btGeneric6DofSpringConstraintDataName;
}
#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H

14
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
View file

@ -369,6 +369,10 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
info->m_J1angularAxis[i*skip+1]=0;
info->m_J1angularAxis[i*skip+2]=0;
info->m_J2linearAxis[i*skip]=0;
info->m_J2linearAxis[i*skip+1]=0;
info->m_J2linearAxis[i*skip+2]=0;
info->m_J2angularAxis[i*skip]=0;
info->m_J2angularAxis[i*skip+1]=0;
info->m_J2angularAxis[i*skip+2]=0;
@ -384,6 +388,10 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[skip + 1] = 1;
info->m_J1linearAxis[2 * skip + 2] = 1;
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[skip + 1] = -1;
info->m_J2linearAxis[2 * skip + 2] = -1;
}
@ -797,7 +805,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i];
// compute three elements of right hand side
btScalar rhs = k * p.dot(ofs);

37
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
View file

@ -28,8 +28,8 @@ subject to the following restrictions:
class btRigidBody;
#ifdef BT_USE_DOUBLE_PRECISION
#define btHingeConstraintData btHingeConstraintDoubleData
#define btHingeConstraintDataName "btHingeConstraintDoubleData"
#define btHingeConstraintData btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
#define btHingeConstraintDataName "btHingeConstraintDoubleData2"
#else
#define btHingeConstraintData btHingeConstraintFloatData
#define btHingeConstraintDataName "btHingeConstraintFloatData"
@ -302,7 +302,10 @@ public:
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
//only for backward compatibility
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
///this structure is not used, except for loading pre-2.82 .bullet files
struct btHingeConstraintDoubleData
{
btTypedConstraintData m_typeConstraintData;
@ -321,7 +324,9 @@ struct btHingeConstraintDoubleData
float m_relaxationFactor;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
struct btHingeConstraintFloatData
{
btTypedConstraintData m_typeConstraintData;
@ -344,6 +349,30 @@ struct btHingeConstraintFloatData
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btHingeConstraintDoubleData2
{
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformDoubleData m_rbBFrame;
int m_useReferenceFrameA;
int m_angularOnly;
int m_enableAngularMotor;
double m_motorTargetVelocity;
double m_maxMotorImpulse;
double m_lowerLimit;
double m_upperLimit;
double m_limitSoftness;
double m_biasFactor;
double m_relaxationFactor;
char m_padding1[4];
};
SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const
{
return sizeof(btHingeConstraintData);

7
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
View file

@ -116,10 +116,9 @@ void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
/*info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[s+1] = -1;
info->m_J2linearAxis[2*s+2] = -1;
*/
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[info->rowskip+1] = -1;
info->m_J2linearAxis[2*info->rowskip+2] = -1;
btVector3 a2 = body1_trans.getBasis()*getPivotInB();

22
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
View file

@ -24,10 +24,10 @@ class btRigidBody;
#ifdef BT_USE_DOUBLE_PRECISION
#define btPoint2PointConstraintData btPoint2PointConstraintDoubleData
#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData"
#define btPoint2PointConstraintData2 btPoint2PointConstraintDoubleData2
#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData2"
#else
#define btPoint2PointConstraintData btPoint2PointConstraintFloatData
#define btPoint2PointConstraintData2 btPoint2PointConstraintFloatData
#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
@ -133,6 +133,17 @@ struct btPoint2PointConstraintFloatData
btVector3FloatData m_pivotInB;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btPoint2PointConstraintDoubleData2
{
btTypedConstraintDoubleData m_typeConstraintData;
btVector3DoubleData m_pivotInA;
btVector3DoubleData m_pivotInB;
};
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
///this structure is not used, except for loading pre-2.82 .bullet files
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btPoint2PointConstraintDoubleData
{
@ -140,18 +151,19 @@ struct btPoint2PointConstraintDoubleData
btVector3DoubleData m_pivotInA;
btVector3DoubleData m_pivotInB;
};
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const
{
return sizeof(btPoint2PointConstraintData);
return sizeof(btPoint2PointConstraintData2);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer;
btPoint2PointConstraintData2* p2pData = (btPoint2PointConstraintData2*)dataBuffer;
btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
m_pivotInA.serialize(p2pData->m_pivotInA);

395
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View file

@ -14,6 +14,8 @@ subject to the following restrictions:
*/
//#define COMPUTE_IMPULSE_DENOM 1
//#define BT_ADDITIONAL_DEBUG
//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
#include "btSequentialImpulseConstraintSolver.h"
@ -63,8 +65,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
@ -77,12 +79,12 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128,body2.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse;
body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
#else
resolveSingleConstraintRowGeneric(body1,body2,c);
@ -93,8 +95,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
void 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_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
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;
@ -116,8 +118,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
c.m_appliedImpulse = sum;
}
body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
}
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
@ -127,8 +129,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
@ -138,24 +140,24 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse;
body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
#else
resolveSingleConstraintRowLowerLimit(body1,body2,c);
#endif
}
// Project Gauss Seidel or the equivalent Sequential Impulse
// Projected Gauss Seidel or the equivalent Sequential Impulse
void 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_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
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;
@ -169,8 +171,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
{
c.m_appliedImpulse = sum;
}
body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
}
@ -183,8 +185,8 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
{
gNumSplitImpulseRecoveries++;
btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
@ -198,8 +200,8 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
{
c.m_appliedPushImpulse = sum;
}
body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
}
}
@ -215,8 +217,8 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
@ -226,12 +228,12 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse;
body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
#else
resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
@ -278,7 +280,7 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
{
btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
@ -297,6 +299,9 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
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();
@ -306,6 +311,8 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
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);
}
@ -324,8 +331,7 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel,
static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
{
@ -349,7 +355,6 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
{
solverConstraint.m_contactNormal = normalAxis;
btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
@ -365,15 +370,30 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
if (body0)
{
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
solverConstraint.m_contactNormal1 = normalAxis;
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
}
solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor();
}else
{
btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
solverConstraint.m_contactNormal1.setZero();
solverConstraint.m_relpos1CrossNormal.setZero();
solverConstraint.m_angularComponentA .setZero();
}
if (body1)
{
solverConstraint.m_contactNormal2 = -normalAxis;
btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor();
} else
{
solverConstraint.m_contactNormal2.setZero();
solverConstraint.m_relpos2CrossNormal.setZero();
solverConstraint.m_angularComponentB.setZero();
}
{
@ -398,9 +418,9 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity: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_contactNormal.dot(body1?solverBodyB.m_linearVelocity: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;
@ -411,8 +431,8 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
}
}
@ -436,7 +456,8 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv
btVector3 normalAxis(0,0,0);
solverConstraint.m_contactNormal = normalAxis;
solverConstraint.m_contactNormal1 = normalAxis;
solverConstraint.m_contactNormal2 = -normalAxis;
btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
@ -477,9 +498,9 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity: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_contactNormal.dot(body1?solverBodyB.m_linearVelocity: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;
@ -490,8 +511,8 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
}
}
@ -513,7 +534,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConst
}
int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep)
{
int solverBodyIdA = -1;
@ -531,11 +552,19 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
{
solverBodyIdA = m_tmpSolverBodyPool.size();
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
initSolverBody(&solverBody,&body);
initSolverBody(&solverBody,&body,timeStep);
body.setCompanionId(solverBodyIdA);
} else
{
return 0;//assume first one is a fixed solver body
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
}
}
@ -548,8 +577,8 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
btVector3& rel_pos1, btVector3& rel_pos2)
btScalar& relaxation,
const btVector3& rel_pos1, const btVector3& rel_pos2)
{
const btVector3& pos1 = cp.getPositionWorldOnA();
@ -563,8 +592,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
relaxation = 1.f;
@ -597,9 +626,24 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
solverConstraint.m_jacDiagABInv = denom;
}
solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
if (rb0)
{
solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
} else
{
solverConstraint.m_contactNormal1.setZero();
solverConstraint.m_relpos1CrossNormal.setZero();
}
if (rb1)
{
solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
}else
{
solverConstraint.m_contactNormal2.setZero();
solverConstraint.m_relpos2CrossNormal.setZero();
}
btScalar restitution = 0.f;
btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
@ -611,8 +655,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
// btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
vel = vel1 - vel2;
rel_vel = cp.m_normalWorldOnB.dot(vel);
btVector3 vel = vel1 - vel2;
btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
@ -632,9 +676,9 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
{
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal*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 (rb1)
bodyB->internalApplyImpulse(solverConstraint.m_contactNormal*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
{
solverConstraint.m_appliedImpulse = 0.f;
@ -643,10 +687,17 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?bodyA->m_linearVelocity:btVector3(0,0,0))
+ solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:btVector3(0,0,0));
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?bodyB->m_linearVelocity:btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:btVector3(0,0,0));
btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0);
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)
+ solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
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;
@ -675,7 +726,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
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_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhsPenetration = 0.f;
} else
@ -713,9 +764,9 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolver
{
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
if (rb0)
bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
if (rb1)
bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
} else
{
frictionConstraint1.m_appliedImpulse = 0.f;
@ -729,9 +780,9 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolver
{
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
if (rb0)
bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
if (rb1)
bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
} else
{
frictionConstraint2.m_appliedImpulse = 0.f;
@ -749,8 +800,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
colObj0 = (btCollisionObject*)manifold->getBody0();
colObj1 = (btCollisionObject*)manifold->getBody1();
int solverBodyIdA = getOrInitSolverBody(*colObj0);
int solverBodyIdB = getOrInitSolverBody(*colObj1);
int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
// btRigidBody* bodyA = btRigidBody::upcast(colObj0);
// btRigidBody* bodyB = btRigidBody::upcast(colObj1);
@ -761,7 +812,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
///avoid collision response between two static objects
if (!solverBodyA || (!solverBodyA->m_originalBody && (!solverBodyB || !solverBodyB->m_originalBody)))
if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
return;
int rollingFriction=1;
@ -775,19 +826,35 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
btVector3 rel_pos1;
btVector3 rel_pos2;
btScalar relaxation;
btScalar rel_vel;
btVector3 vel;
int frictionIndex = m_tmpSolverContactConstraintPool.size();
btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
// btRigidBody* rb0 = btRigidBody::upcast(colObj0);
// btRigidBody* rb1 = btRigidBody::upcast(colObj1);
btRigidBody* rb0 = btRigidBody::upcast(colObj0);
btRigidBody* rb1 = btRigidBody::upcast(colObj1);
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_originalContactPoint = &cp;
setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
const btVector3& pos1 = cp.getPositionWorldOnA();
const btVector3& pos2 = cp.getPositionWorldOnB();
rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
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();
@ -796,9 +863,11 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
btVector3 angVelA,angVelB;
solverBodyA->getAngularVelocity(angVelA);
solverBodyB->getAngularVelocity(angVelB);
btVector3 angVelA(0,0,0),angVelB(0,0,0);
if (rb0)
angVelA = rb0->getAngularVelocity();
if (rb1)
angVelB = rb1->getAngularVelocity();
btVector3 relAngVel = angVelB-angVelA;
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
@ -852,6 +921,10 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
{
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);
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
@ -859,17 +932,16 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
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);
}
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);
} else
{
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
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);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
@ -877,9 +949,6 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
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);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
{
@ -894,8 +963,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
}
setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
@ -904,15 +973,29 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
}
}
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
{
int i;
btPersistentManifold* manifold = 0;
// btCollisionObject* colObj0=0,*colObj1=0;
for (i=0;i<numManifolds;i++)
{
manifold = manifoldPtr[i];
convertContact(manifold,infoGlobal);
}
}
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
m_fixedBodyId = -1;
BT_PROFILE("solveGroupCacheFriendlySetup");
(void)stackAlloc;
(void)debugDrawer;
m_maxOverrideNumSolverIterations = 0;
#ifdef BT_DEBUG
#ifdef BT_ADDITIONAL_DEBUG
//make sure that dynamic bodies exist for all (enabled) constraints
for (int i=0;i<numConstraints;i++)
{
@ -979,7 +1062,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btAssert(found);
}
}
#endif //BT_DEBUG
#endif //BT_ADDITIONAL_DEBUG
for (int i = 0; i < numBodies; i++)
@ -991,14 +1074,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
m_tmpSolverBodyPool.reserve(numBodies+1);
m_tmpSolverBodyPool.resize(0);
btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
initSolverBody(&fixedBody,0);
//btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
//initSolverBody(&fixedBody,0);
//convert all bodies
for (int i=0;i<numBodies;i++)
{
int bodyId = getOrInitSolverBody(*bodies[i]);
int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
btRigidBody* body = btRigidBody::upcast(bodies[i]);
if (body && body->getInvMass())
{
@ -1007,9 +1091,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE)
{
gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
}
solverBody.m_linearVelocity += body->getTotalForce()*body->getInvMass()*infoGlobal.m_timeStep;
solverBody.m_angularVelocity += (body->getTotalTorque()-gyroForce)*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
}
}
@ -1079,8 +1162,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btRigidBody& rbA = constraint->getRigidBodyA();
btRigidBody& rbB = constraint->getRigidBodyB();
int solverBodyIdA = getOrInitSolverBody(rbA);
int solverBodyIdB = getOrInitSolverBody(rbB);
int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
@ -1119,9 +1202,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btTypedConstraint::btConstraintInfo2 info2;
info2.fps = 1.f/infoGlobal.m_timeStep;
info2.erp = infoGlobal.m_erp;
info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
info2.m_J2linearAxis = 0;
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
@ -1162,14 +1245,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
}
{
btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
sum += iMJlB.dot(solverConstraint.m_contactNormal);
sum += iMJlB.dot(solverConstraint.m_contactNormal2);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
btScalar fsum = btFabs(sum);
btAssert(fsum > SIMD_EPSILON);
@ -1177,15 +1260,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
}
///fix rhs
///todo: add force/torque accelerators
{
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
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)
+ solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
+ solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
rel_vel = vel1Dotn+vel2Dotn;
btScalar restitution = 0.f;
btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
btScalar velocityError = restitution - rel_vel * info2.m_damping;
@ -1194,6 +1284,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_appliedImpulse = 0.f;
}
}
}
@ -1201,18 +1292,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
}
}
{
int i;
btPersistentManifold* manifold = 0;
// btCollisionObject* colObj0=0,*colObj1=0;
convertContacts(manifoldPtr,numManifolds,infoGlobal);
for (i=0;i<numManifolds;i++)
{
manifold = manifoldPtr[i];
convertContact(manifold,infoGlobal);
}
}
}
// btContactSolverInfo info = infoGlobal;
@ -1251,7 +1332,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
}
btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
{
int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
@ -1304,14 +1385,14 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
{
for (int j=0;j<numConstraints;j++)
{
if (constraints[j]->isEnabled())
{
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
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];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
}
///solve all contact constraints using SIMD, if available
@ -1371,7 +1452,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
@ -1390,7 +1472,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
}
@ -1432,14 +1515,14 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
{
for (int j=0;j<numConstraints;j++)
{
if (constraints[j]->isEnabled())
{
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
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];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
}
///solve all contact constraints
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
@ -1487,7 +1570,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
}
void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
int iteration;
if (infoGlobal.m_splitImpulse)
@ -1527,20 +1610,20 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
}
}
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
BT_PROFILE("solveGroupCacheFriendlyIterations");
{
///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
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--)
{
solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
}
}
@ -1580,10 +1663,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
btJointFeedback* fb = constr->getJointFeedback();
if (fb)
{
fb->m_appliedForceBodyA += solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
fb->m_appliedForceBodyB += -solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
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_relpos1CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
}
@ -1605,9 +1688,15 @@ 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);
m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
m_tmpSolverBodyPool[i].m_angularVelocity+
m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity);
m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity);
if (infoGlobal.m_splitImpulse)
m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
@ -1627,15 +1716,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/)
{
BT_PROFILE("solveGroup");
//you need to provide at least some bodies
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);

31
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View file

@ -18,7 +18,6 @@ subject to the following restrictions:
class btIDebugDraw;
class btPersistentManifold;
class btStackAlloc;
class btDispatcher;
class btCollisionObject;
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
@ -43,7 +42,7 @@ protected:
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
int m_maxOverrideNumSolverIterations;
int m_fixedBodyId;
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,
@ -57,10 +56,11 @@ protected:
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, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
btSolverConstraint& addRollingFrictionConstraint(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, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
btVector3& rel_pos1, 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);
@ -71,6 +71,8 @@ protected:
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
@ -83,8 +85,8 @@ protected:
const btSolverConstraint& contactConstraint);
//internal method
int getOrInitSolverBody(btCollisionObject& body);
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
void resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
@ -97,12 +99,12 @@ protected:
protected:
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
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);
btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
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,btStackAlloc* stackAlloc);
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
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);
public:
@ -112,7 +114,7 @@ public:
btSequentialImpulseConstraintSolver();
virtual ~btSequentialImpulseConstraintSolver();
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
@ -132,6 +134,11 @@ public:
return m_btSeed2;
}
virtual btConstraintSolverType getSolverType() const
{
return BT_SEQUENTIAL_IMPULSE_SOLVER;
}
};

7
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
View file

@ -426,6 +426,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
}
else
{ // old way - maybe incorrect if bodies are not on the slider axis
@ -440,6 +442,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
}
// compute two elements of right hand side
@ -479,6 +483,9 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
info->m_J1linearAxis[srow+0] = ax1[0];
info->m_J1linearAxis[srow+1] = ax1[1];
info->m_J1linearAxis[srow+2] = ax1[2];
info->m_J2linearAxis[srow+0] = -ax1[0];
info->m_J2linearAxis[srow+1] = -ax1[1];
info->m_J2linearAxis[srow+2] = -ax1[2];
// linear torque decoupling step:
//
// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies

46
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
View file

@ -25,7 +25,13 @@ TODO:
#ifndef BT_SLIDER_CONSTRAINT_H
#define BT_SLIDER_CONSTRAINT_H
#ifdef BT_USE_DOUBLE_PRECISION
#define btSliderConstraintData2 btSliderConstraintDoubleData
#define btSliderConstraintDataName "btSliderConstraintDoubleData"
#else
#define btSliderConstraintData2 btSliderConstraintData
#define btSliderConstraintDataName "btSliderConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
@ -283,7 +289,10 @@ public:
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btSliderConstraintData
{
btTypedConstraintData m_typeConstraintData;
@ -302,31 +311,48 @@ struct btSliderConstraintData
};
struct btSliderConstraintDoubleData
{
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformDoubleData m_rbBFrame;
double m_linearUpperLimit;
double m_linearLowerLimit;
double m_angularUpperLimit;
double m_angularLowerLimit;
int m_useLinearReferenceFrameA;
int m_useOffsetForConstraintFrame;
};
SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const
{
return sizeof(btSliderConstraintData);
return sizeof(btSliderConstraintData2);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btSliderConstraintData* sliderData = (btSliderConstraintData*) dataBuffer;
btSliderConstraintData2* sliderData = (btSliderConstraintData2*) dataBuffer;
btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
m_frameInA.serializeFloat(sliderData->m_rbAFrame);
m_frameInB.serializeFloat(sliderData->m_rbBFrame);
m_frameInA.serialize(sliderData->m_rbAFrame);
m_frameInB.serialize(sliderData->m_rbBFrame);
sliderData->m_linearUpperLimit = float(m_upperLinLimit);
sliderData->m_linearLowerLimit = float(m_lowerLinLimit);
sliderData->m_linearUpperLimit = m_upperLinLimit;
sliderData->m_linearLowerLimit = m_lowerLinLimit;
sliderData->m_angularUpperLimit = float(m_upperAngLimit);
sliderData->m_angularLowerLimit = float(m_lowerAngLimit);
sliderData->m_angularUpperLimit = m_upperAngLimit;
sliderData->m_angularLowerLimit = m_lowerAngLimit;
sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA;
sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame;
return "btSliderConstraintData";
return btSliderConstraintDataName;
}

15
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h
View file

@ -105,7 +105,7 @@ operator+(const btSimdScalar& v1, const btSimdScalar& v2)
#endif
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED64 (struct) btSolverBody
ATTRIBUTE_ALIGNED16 (struct) btSolverBody
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btTransform m_worldTransform;
@ -118,6 +118,8 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverBody
btVector3 m_turnVelocity;
btVector3 m_linearVelocity;
btVector3 m_angularVelocity;
btVector3 m_externalForceImpulse;
btVector3 m_externalTorqueImpulse;
btRigidBody* m_originalBody;
void setWorldTransform(const btTransform& worldTransform)
@ -130,6 +132,17 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverBody
return m_worldTransform;
}
SIMD_FORCE_INLINE void getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity ) const
{
if (m_originalBody)
velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity+m_externalTorqueImpulse).cross(rel_pos);
else
velocity.setValue(0,0,0);
}
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
{
if (m_originalBody)

5
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
View file

@ -32,10 +32,10 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 m_relpos1CrossNormal;
btVector3 m_contactNormal;
btVector3 m_contactNormal1;
btVector3 m_relpos2CrossNormal;
//btVector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
btVector3 m_angularComponentA;
btVector3 m_angularComponentB;
@ -55,6 +55,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
{
void* m_originalContactPoint;
btScalar m_unusedPadding4;
int m_numRowsForNonContactConstraint;
};
int m_overrideNumSolverIterations;

10
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
View file

@ -109,7 +109,7 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
///fills the dataBuffer and returns the struct name (and 0 on failure)
const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btTypedConstraintData* tcd = (btTypedConstraintData*) dataBuffer;
btTypedConstraintData2* tcd = (btTypedConstraintData2*) dataBuffer;
tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
@ -123,14 +123,14 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
tcd->m_objectType = m_objectType;
tcd->m_needsFeedback = m_needsFeedback;
tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations;
tcd->m_breakingImpulseThreshold = float(m_breakingImpulseThreshold);
tcd->m_breakingImpulseThreshold = m_breakingImpulseThreshold;
tcd->m_isEnabled = m_isEnabled? 1: 0;
tcd->m_userConstraintId =m_userConstraintId;
tcd->m_userConstraintType =m_userConstraintType;
tcd->m_appliedImpulse = float(m_appliedImpulse);
tcd->m_dbgDrawSize = float(m_dbgDrawSize );
tcd->m_appliedImpulse = m_appliedImpulse;
tcd->m_dbgDrawSize = m_dbgDrawSize;
tcd->m_disableCollisionsBetweenLinkedBodies = false;
@ -142,7 +142,7 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
if (m_rbB.getConstraintRef(i) == this)
tcd->m_disableCollisionsBetweenLinkedBodies = true;
return "btTypedConstraintData";
return btTypedConstraintDataName;
}
btRigidBody& btTypedConstraint::getFixedBody()

64
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
View file

@ -21,6 +21,15 @@ subject to the following restrictions:
#include "btSolverConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btTypedConstraintData2 btTypedConstraintDoubleData
#define btTypedConstraintDataName "btTypedConstraintDoubleData"
#else
#define btTypedConstraintData2 btTypedConstraintFloatData
#define btTypedConstraintDataName "btTypedConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
class btSerializer;
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
@ -34,6 +43,7 @@ enum btTypedConstraintType
CONTACT_CONSTRAINT_TYPE,
D6_SPRING_CONSTRAINT_TYPE,
GEAR_CONSTRAINT_TYPE,
FIXED_CONSTRAINT_TYPE,
MAX_CONSTRAINT_TYPE
};
@ -356,6 +366,33 @@ SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScal
}
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btTypedConstraintFloatData
{
btRigidBodyFloatData *m_rbA;
btRigidBodyFloatData *m_rbB;
char *m_name;
int m_objectType;
int m_userConstraintType;
int m_userConstraintId;
int m_needsFeedback;
float m_appliedImpulse;
float m_dbgDrawSize;
int m_disableCollisionsBetweenLinkedBodies;
int m_overrideNumSolverIterations;
float m_breakingImpulseThreshold;
int m_isEnabled;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
#define BT_BACKWARDS_COMPATIBLE_SERIALIZATION
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
///this structure is not used, except for loading pre-2.82 .bullet files
struct btTypedConstraintData
{
btRigidBodyData *m_rbA;
@ -377,10 +414,35 @@ struct btTypedConstraintData
int m_isEnabled;
};
#endif //BACKWARDS_COMPATIBLE
struct btTypedConstraintDoubleData
{
btRigidBodyDoubleData *m_rbA;
btRigidBodyDoubleData *m_rbB;
char *m_name;
int m_objectType;
int m_userConstraintType;
int m_userConstraintId;
int m_needsFeedback;
double m_appliedImpulse;
double m_dbgDrawSize;
int m_disableCollisionsBetweenLinkedBodies;
int m_overrideNumSolverIterations;
double m_breakingImpulseThreshold;
int m_isEnabled;
char padding[4];
};
SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
{
return sizeof(btTypedConstraintData);
return sizeof(btTypedConstraintData2);
}

39
Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View file

@ -87,7 +87,6 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
btTypedConstraint** m_sortedConstraints;
int m_numConstraints;
btIDebugDraw* m_debugDrawer;
btStackAlloc* m_stackAlloc;
btDispatcher* m_dispatcher;
btAlignedObjectArray<btCollisionObject*> m_bodies;
@ -104,7 +103,6 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
m_sortedConstraints(NULL),
m_numConstraints(0),
m_debugDrawer(NULL),
m_stackAlloc(stackAlloc),
m_dispatcher(dispatcher)
{
@ -135,7 +133,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
if (islandId<0)
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
} else
{
//also add all non-contact constraints/joints for this island
@ -163,7 +161,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
if (m_solverInfo->m_minimumSolverBatchSize<=1)
{
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
} else
{
@ -190,7 +188,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher);
m_bodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
@ -210,7 +208,9 @@ m_gravity(0,-10,0),
m_localTime(0),
m_synchronizeAllMotionStates(false),
m_applySpeculativeContactRestitution(false),
m_profileTimings(0)
m_profileTimings(0),
m_fixedTimeStep(0),
m_latencyMotionStateInterpolation(true)
{
if (!m_constraintSolver)
@ -232,7 +232,7 @@ m_profileTimings(0)
{
void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback),16);
m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, m_stackAlloc, dispatcher);
m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, 0, dispatcher);
}
}
@ -359,7 +359,9 @@ void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
{
btTransform interpolatedTransform;
btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),
(m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(),
interpolatedTransform);
body->getMotionState()->setWorldTransform(interpolatedTransform);
}
}
@ -403,6 +405,7 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
if (maxSubSteps)
{
//fixed timestep with interpolation
m_fixedTimeStep = fixedTimeStep;
m_localTime += timeStep;
if (m_localTime >= fixedTimeStep)
{
@ -413,7 +416,8 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
{
//variable timestep
fixedTimeStep = timeStep;
m_localTime = timeStep;
m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep;
m_fixedTimeStep = 0;
if (btFuzzyZero(timeStep))
{
numSimulationSubSteps = 0;
@ -724,7 +728,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
m_solverIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}
@ -746,12 +750,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands()
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
{
if (colObj0->isActive() || colObj1->isActive())
{
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
(colObj1)->getIslandTag());
}
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
}
}
}
@ -770,12 +769,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands()
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
{
if (colObj0->isActive() || colObj1->isActive())
{
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
(colObj1)->getIslandTag());
}
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
}
}
}
@ -1131,7 +1125,6 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
{
//don't integrate/update velocities here, it happens in the constraint solver
//damping
body->applyDamping(timeStep);
body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());

15
Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View file

@ -53,6 +53,7 @@ protected:
//for variable timesteps
btScalar m_localTime;
btScalar m_fixedTimeStep;
//for variable timesteps
bool m_ownsIslandManager;
@ -64,6 +65,8 @@ protected:
int m_profileTimings;
bool m_latencyMotionStateInterpolation;
btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds;
virtual void predictUnconstraintMotion(btScalar timeStep);
@ -74,7 +77,7 @@ protected:
virtual void solveConstraints(btContactSolverInfo& solverInfo);
void updateActivationState(btScalar timeStep);
virtual void updateActivationState(btScalar timeStep);
void updateActions(btScalar timeStep);
@ -216,6 +219,16 @@ public:
///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo)
virtual void serialize(btSerializer* serializer);
///Interpolate motion state between previous and current transform, instead of current and next transform.
///This can relieve discontinuities in the rendering, due to penetrations
void setLatencyMotionStateInterpolation(bool latencyInterpolation )
{
m_latencyMotionStateInterpolation = latencyInterpolation;
}
bool getLatencyMotionStateInterpolation() const
{
return m_latencyMotionStateInterpolation;
}
};
#endif //BT_DISCRETE_DYNAMICS_WORLD_H

3
Engine/lib/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h
View file

@ -33,7 +33,8 @@ enum btDynamicsWorldType
BT_SIMPLE_DYNAMICS_WORLD=1,
BT_DISCRETE_DYNAMICS_WORLD=2,
BT_CONTINUOUS_DYNAMICS_WORLD=3,
BT_SOFT_RIGID_DYNAMICS_WORLD=4
BT_SOFT_RIGID_DYNAMICS_WORLD=4,
BT_GPU_DYNAMICS_WORLD=5
};
///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.

6
Engine/lib/bullet/src/BulletDynamics/Dynamics/btRigidBody.h
View file

@ -97,7 +97,7 @@ class btRigidBody : public btCollisionObject
protected:
ATTRIBUTE_ALIGNED64(btVector3 m_deltaLinearVelocity);
ATTRIBUTE_ALIGNED16(btVector3 m_deltaLinearVelocity);
btVector3 m_deltaAngularVelocity;
btVector3 m_angularFactor;
btVector3 m_invMass;
@ -363,11 +363,13 @@ public:
inline void setLinearVelocity(const btVector3& lin_vel)
{
m_updateRevision++;
m_linearVelocity = lin_vel;
}
inline void setAngularVelocity(const btVector3& ang_vel)
{
m_updateRevision++;
m_angularVelocity = ang_vel;
}
@ -484,11 +486,13 @@ public:
void setAngularFactor(const btVector3& angFac)
{
m_updateRevision++;
m_angularFactor = angFac;
}
void setAngularFactor(btScalar angFac)
{
m_updateRevision++;
m_angularFactor.setValue(angFac,angFac,angFac);
}
const btVector3& getAngularFactor() const

4
Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
View file

@ -78,8 +78,8 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
btContactSolverInfo infoGlobal;
infoGlobal.m_timeStep = timeStep;
m_constraintSolver->prepareSolve(0,numManifolds);
m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1);
m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc);
m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_dispatcher1);
m_constraintSolver->allSolved(infoGlobal,m_debugDrawer);
}
///integrate transforms

1009
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp Normal file
View file

File diff suppressed because it is too large Load diff

466
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.h Normal file
View file

@ -0,0 +1,466 @@
/*
* PURPOSE:
* Class representing an articulated rigid body. Stores the body's
* current state, allows forces and torques to be set, handles
* timestepping and implements Featherstone's algorithm.
*
* COPYRIGHT:
* Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
* Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
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_H
#define BT_MULTIBODY_H
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "btMultiBodyLink.h"
class btMultiBodyLinkCollider;
class btMultiBody
{
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
//
// initialization
//
btMultiBody(int n_links, // NOT including the base
btScalar mass, // mass of base
const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal
bool fixed_base_, // whether the base is fixed (true) or can move (false)
bool can_sleep_);
~btMultiBody();
void setupPrismatic(int i, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia, // in my frame; assumed diagonal
int parent,
const btQuaternion &rot_parent_to_this, // rotate points in parent frame to my frame.
const btVector3 &joint_axis, // in my frame
const btVector3 &r_vector_when_q_zero, // vector from parent COM to my COM, in my frame, when q = 0.
bool disableParentCollision=false
);
void setupRevolute(int i, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &zero_rot_parent_to_this, // rotate points in parent frame to this frame, when q = 0
const btVector3 &joint_axis, // in my frame
const btVector3 &parent_axis_position, // vector from parent COM to joint axis, in PARENT frame
const btVector3 &my_axis_position, // vector from joint axis to my COM, in MY frame
bool disableParentCollision=false);
const btMultibodyLink& getLink(int index) const
{
return links[index];
}
btMultibodyLink& getLink(int index)
{
return links[index];
}
void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base
{
m_baseCollider = collider;
}
const btMultiBodyLinkCollider* getBaseCollider() const
{
return m_baseCollider;
}
btMultiBodyLinkCollider* getBaseCollider()
{
return m_baseCollider;
}
//
// get parent
// input: link num from 0 to num_links-1
// output: link num from 0 to num_links-1, OR -1 to mean the base.
//
int getParent(int link_num) const;
//
// get number of links, masses, moments of inertia
//
int getNumLinks() const { return links.size(); }
btScalar getBaseMass() const { return base_mass; }
const btVector3 & getBaseInertia() const { return base_inertia; }
btScalar getLinkMass(int i) const;
const btVector3 & getLinkInertia(int i) const;
//
// change mass (incomplete: can only change base mass and inertia at present)
//
void setBaseMass(btScalar mass) { base_mass = mass; }
void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; }
//
// get/set pos/vel/rot/omega for the base link
//
const btVector3 & getBasePos() const { return base_pos; } // in world frame
const btVector3 getBaseVel() const
{
return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]);
} // in world frame
const btQuaternion & getWorldToBaseRot() const
{
return base_quat;
} // rotates world vectors into base frame
btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); } // in world frame
void setBasePos(const btVector3 &pos)
{
base_pos = pos;
}
void setBaseVel(const btVector3 &vel)
{
m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2];
}
void setWorldToBaseRot(const btQuaternion &rot)
{
base_quat = rot;
}
void setBaseOmega(const btVector3 &omega)
{
m_real_buf[0]=omega[0];
m_real_buf[1]=omega[1];
m_real_buf[2]=omega[2];
}
//
// get/set pos/vel for child links (i = 0 to num_links-1)
//
btScalar getJointPos(int i) const;
btScalar getJointVel(int i) const;
void setJointPos(int i, btScalar q);
void setJointVel(int i, btScalar qdot);
//
// direct access to velocities as a vector of 6 + num_links elements.
// (omega first, then v, then joint velocities.)
//
const btScalar * getVelocityVector() const
{
return &m_real_buf[0];
}
/* btScalar * getVelocityVector()
{
return &real_buf[0];
}
*/
//
// get the frames of reference (positions and orientations) of the child links
// (i = 0 to num_links-1)
//
const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords
const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
//
// transform vectors in local frame of link i to world frame (or vice versa)
//
btVector3 localPosToWorld(int i, const btVector3 &vec) const;
btVector3 localDirToWorld(int i, const btVector3 &vec) const;
btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
//
// calculate kinetic energy and angular momentum
// useful for debugging.
//
btScalar getKineticEnergy() const;
btVector3 getAngularMomentum() const;
//
// set external forces and torques. Note all external forces/torques are given in the WORLD frame.
//
void clearForcesAndTorques();
void clearVelocities();
void addBaseForce(const btVector3 &f)
{
base_force += f;
}
void addBaseTorque(const btVector3 &t) { base_torque += t; }
void addLinkForce(int i, const btVector3 &f);
void addLinkTorque(int i, const btVector3 &t);
void addJointTorque(int i, btScalar Q);
const btVector3 & getBaseForce() const { return base_force; }
const btVector3 & getBaseTorque() const { return base_torque; }
const btVector3 & getLinkForce(int i) const;
const btVector3 & getLinkTorque(int i) const;
btScalar getJointTorque(int i) const;
//
// dynamics routines.
//
// timestep the velocities (given the external forces/torques set using addBaseForce etc).
// also sets up caches for calcAccelerationDeltas.
//
// Note: the caller must provide three vectors which are used as
// temporary scratch space. The idea here is to reduce dynamic
// memory allocation: the same scratch vectors can be re-used
// again and again for different Multibodies, instead of each
// btMultiBody allocating (and then deallocating) their own
// individual scratch buffers. This gives a considerable speed
// improvement, at least on Windows (where dynamic memory
// allocation appears to be fairly slow).
//
void stepVelocities(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m);
// calcAccelerationDeltas
// input: force vector (in same format as jacobian, i.e.:
// 3 torque values, 3 force values, num_links joint torque values)
// output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
// (existing contents of output array are replaced)
// stepVelocities must have been called first.
void calcAccelerationDeltas(const btScalar *force, btScalar *output,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v) const;
// apply a delta-vee directly. used in sequential impulses code.
void applyDeltaVee(const btScalar * delta_vee)
{
for (int i = 0; i < 6 + getNumLinks(); ++i)
{
m_real_buf[i] += delta_vee[i];
}
}
void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier)
{
btScalar sum = 0;
for (int i = 0; i < 6 + getNumLinks(); ++i)
{
sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
}
btScalar l = btSqrt(sum);
/*
static btScalar maxl = -1e30f;
if (l>maxl)
{
maxl=l;
// printf("maxl=%f\n",maxl);
}
*/
if (l>m_maxAppliedImpulse)
{
// printf("exceeds 100: l=%f\n",maxl);
multiplier *= m_maxAppliedImpulse/l;
}
for (int i = 0; i < 6 + getNumLinks(); ++i)
{
sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
m_real_buf[i] += delta_vee[i] * multiplier;
}
}
// timestep the positions (given current velocities).
void stepPositions(btScalar dt);
//
// contacts
//
// This routine fills out a contact constraint jacobian for this body.
// the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
// 'normal' & 'contact_point' are both given in world coordinates.
void fillContactJacobian(int link,
const btVector3 &contact_point,
const btVector3 &normal,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
//
// sleeping
//
void setCanSleep(bool canSleep)
{
can_sleep = canSleep;
}
bool isAwake() const { return awake; }
void wakeUp();
void goToSleep();
void checkMotionAndSleepIfRequired(btScalar timestep);
bool hasFixedBase() const
{
return fixed_base;
}
int getCompanionId() const
{
return m_companionId;
}
void setCompanionId(int id)
{
//printf("for %p setCompanionId(%d)\n",this, id);
m_companionId = id;
}
void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links
{
links.resize(numLinks);
}
btScalar getLinearDamping() const
{
return m_linearDamping;
}
void setLinearDamping( btScalar damp)
{
m_linearDamping = damp;
}
btScalar getAngularDamping() const
{
return m_angularDamping;
}
bool getUseGyroTerm() const
{
return m_useGyroTerm;
}
void setUseGyroTerm(bool useGyro)
{
m_useGyroTerm = useGyro;
}
btScalar getMaxAppliedImpulse() const
{
return m_maxAppliedImpulse;
}
void setMaxAppliedImpulse(btScalar maxImp)
{
m_maxAppliedImpulse = maxImp;
}
void setHasSelfCollision(bool hasSelfCollision)
{
m_hasSelfCollision = hasSelfCollision;
}
bool hasSelfCollision() const
{
return m_hasSelfCollision;
}
private:
btMultiBody(const btMultiBody &); // not implemented
void operator=(const btMultiBody &); // not implemented
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
private:
btMultiBodyLinkCollider* m_baseCollider;//can be NULL
btVector3 base_pos; // position of COM of base (world frame)
btQuaternion base_quat; // rotates world points into base frame
btScalar base_mass; // mass of the base
btVector3 base_inertia; // inertia of the base (in local frame; diagonal)
btVector3 base_force; // external force applied to base. World frame.
btVector3 base_torque; // external torque applied to base. World frame.
btAlignedObjectArray<btMultibodyLink> links; // array of links, excluding the base. index from 0 to num_links-1.
btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
//
// real_buf:
// offset size array
// 0 6 + num_links v (base_omega; base_vel; joint_vels)
// 6+num_links num_links D
//
// vector_buf:
// offset size array
// 0 num_links h_top
// num_links num_links h_bottom
//
// matrix_buf:
// offset size array
// 0 num_links+1 rot_from_parent
//
btAlignedObjectArray<btScalar> m_real_buf;
btAlignedObjectArray<btVector3> vector_buf;
btAlignedObjectArray<btMatrix3x3> matrix_buf;
//std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
btMatrix3x3 cached_inertia_top_left;
btMatrix3x3 cached_inertia_top_right;
btMatrix3x3 cached_inertia_lower_left;
btMatrix3x3 cached_inertia_lower_right;
bool fixed_base;
// Sleep parameters.
bool awake;
bool can_sleep;
btScalar sleep_timer;
int m_companionId;
btScalar m_linearDamping;
btScalar m_angularDamping;
bool m_useGyroTerm;
btScalar m_maxAppliedImpulse;
bool m_hasSelfCollision;
};
#endif

527
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp Normal file
View file

@ -0,0 +1,527 @@
#include "btMultiBodyConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
:m_bodyA(bodyA),
m_bodyB(bodyB),
m_linkA(linkA),
m_linkB(linkB),
m_num_rows(numRows),
m_isUnilateral(isUnilateral),
m_maxAppliedImpulse(100)
{
m_jac_size_A = (6 + bodyA->getNumLinks());
m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0));
m_pos_offset = ((1 + m_jac_size_both)*m_num_rows);
m_data.resize((2 + m_jac_size_both) * m_num_rows);
}
btMultiBodyConstraint::~btMultiBodyConstraint()
{
}
btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
btMultiBodyJacobianData& data,
btScalar* jacOrgA,btScalar* jacOrgB,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity,
btScalar lowerLimit,
btScalar upperLimit)
{
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 6;
constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
if (constraintRow.m_deltaVelAindex <0)
{
constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
} else
{
btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
}
constraintRow.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
for (int i=0;i<ndofA;i++)
data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
if (constraintRow.m_deltaVelBindex <0)
{
constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
constraintRow.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
for (int i=0;i<ndofB;i++)
data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
}
{
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;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
jacA = &data.m_jacobians[constraintRow.m_jacAindex];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar l =lambdaA[i];
denom0 += j*l;
}
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
jacB = &data.m_jacobians[constraintRow.m_jacBindex];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
btScalar l =lambdaB[i];
denom1 += j*l;
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
btScalar d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
constraintRow.m_jacDiagABInv = 1.f/(d);
} else
{
constraintRow.m_jacDiagABInv = 1.f;
}
}
//compute rhs and remaining constraintRow fields
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
}
constraintRow.m_friction = 0.f;
constraintRow.m_appliedImpulse = 0.f;
constraintRow.m_appliedPushImpulse = 0.f;
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse)
{
//combine position and velocity into rhs
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
}
constraintRow.m_cfm = 0.f;
constraintRow.m_lowerLimit = lowerLimit;
constraintRow.m_upperLimit = upperLimit;
}
return rel_vel;
}
void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
{
for (int i = 0; i < ndof; ++i)
data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
}
void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar position,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
btVector3 rel_pos1 = posAworld;
btVector3 rel_pos2 = posBworld;
solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB;
solverConstraint.m_linkA = m_linkA;
solverConstraint.m_linkB = m_linkB;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
const btVector3& pos1 = posAworld;
const btVector3& pos2 = posBworld;
btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(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 = 1.f;
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
if (solverConstraint.m_deltaVelAindex <0)
{
solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
} else
{
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
}
solverConstraint.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
} else
{
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = contactNormalOnB;
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
{
solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
solverConstraint.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
} else
{
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormalOnB;
}
{
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;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
lambdaA = &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)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
}
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
lambdaB = &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)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
btScalar d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
} else
{
solverConstraint.m_jacDiagABInv = 1.f;
}
}
//compute rhs and remaining solverConstraint fields
btScalar restitution = 0.f;
btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop;
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
} else
{
if (rb0)
{
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
}
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
} else
{
if (rb1)
{
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
}
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= btScalar(0.))
{
restitution = 0.f;
};
}
///warm starting (or zero if disabled)
/*
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
} else
{
if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,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);
}
}
} else
*/
{
solverConstraint.m_appliedImpulse = 0.f;
}
solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = -m_maxAppliedImpulse;
solverConstraint.m_upperLimit = m_maxAppliedImpulse;
}
}

166
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h Normal file
View file

@ -0,0 +1,166 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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_CONSTRAINT_H
#define BT_MULTIBODY_CONSTRAINT_H
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "btMultiBody.h"
class btMultiBody;
struct btSolverInfo;
#include "btMultiBodySolverConstraint.h"
struct btMultiBodyJacobianData
{
btAlignedObjectArray<btScalar> m_jacobians;
btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse;
btAlignedObjectArray<btScalar> m_deltaVelocities;
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
btAlignedObjectArray<btSolverBody>* m_solverBodyPool;
int m_fixedBodyId;
};
class btMultiBodyConstraint
{
protected:
btMultiBody* m_bodyA;
btMultiBody* m_bodyB;
int m_linkA;
int m_linkB;
int m_num_rows;
int m_jac_size_A;
int m_jac_size_both;
int m_pos_offset;
bool m_isUnilateral;
btScalar m_maxAppliedImpulse;
// data block laid out as follows:
// cached impulses. (one per row.)
// jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
// positions. (one per row.)
btAlignedObjectArray<btScalar> m_data;
void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar position,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
btMultiBodyJacobianData& data,
btScalar* jacOrgA,btScalar* jacOrgB,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity,
btScalar lowerLimit,
btScalar upperLimit);
public:
btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
virtual ~btMultiBodyConstraint();
virtual int getIslandIdA() const =0;
virtual int getIslandIdB() const =0;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)=0;
int getNumRows() const
{
return m_num_rows;
}
btMultiBody* getMultiBodyA()
{
return m_bodyA;
}
btMultiBody* getMultiBodyB()
{
return m_bodyB;
}
// current constraint position
// constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
// NOTE: ignored position for friction rows.
btScalar getPosition(int row) const
{
return m_data[m_pos_offset + row];
}
void setPosition(int row, btScalar pos)
{
m_data[m_pos_offset + row] = pos;
}
bool isUnilateral() const
{
return m_isUnilateral;
}
// jacobian blocks.
// each of size 6 + num_links. (jacobian2 is null if no body2.)
// format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
btScalar* jacobianA(int row)
{
return &m_data[m_num_rows + row * m_jac_size_both];
}
const btScalar* jacobianA(int row) const
{
return &m_data[m_num_rows + (row * m_jac_size_both)];
}
btScalar* jacobianB(int row)
{
return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A];
}
const btScalar* jacobianB(int row) const
{
return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A];
}
btScalar getMaxAppliedImpulse() const
{
return m_maxAppliedImpulse;
}
void setMaxAppliedImpulse(btScalar maxImp)
{
m_maxAppliedImpulse = maxImp;
}
};
#endif //BT_MULTIBODY_CONSTRAINT_H

795
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp Normal file
View file

@ -0,0 +1,795 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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 "btMultiBodyConstraintSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
#include "btMultiBodyConstraint.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#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 val = 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++)
{
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
//if (iteration < constraint.m_overrideNumSolverIterations)
//resolveSingleConstraintRowGenericMultiBody(constraint);
resolveSingleConstraintRowGeneric(constraint);
}
//solve featherstone normal contact
for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
{
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
if (iteration < infoGlobal.m_numIterations)
resolveSingleConstraintRowGeneric(constraint);
}
//solve featherstone frictional contact
for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++)
{
if (iteration < infoGlobal.m_numIterations)
{
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j];
btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
//adjust friction limits here
if (totalImpulse>btScalar(0))
{
frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
resolveSingleConstraintRowGeneric(frictionConstraint);
}
}
}
return val;
}
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);
m_multiBodyFrictionContactConstraints.resize(0);
m_data.m_jacobians.resize(0);
m_data.m_deltaVelocitiesUnitImpulse.resize(0);
m_data.m_deltaVelocities.resize(0);
for (int i=0;i<numBodies;i++)
{
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
if (fcA)
{
fcA->m_multiBody->setCompanionId(-1);
}
}
btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
return val;
}
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;
}
void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
btScalar deltaVelADotn=0;
btScalar deltaVelBDotn=0;
btSolverBody* bodyA = 0;
btSolverBody* bodyB = 0;
int ndofA=0;
int ndofB=0;
if (c.m_multiBodyA)
{
ndofA = c.m_multiBodyA->getNumLinks() + 6;
for (int i = 0; i < ndofA; ++i)
deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
} else
{
bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
}
if (c.m_multiBodyB)
{
ndofB = c.m_multiBodyB->getNumLinks() + 6;
for (int i = 0; i < ndofB; ++i)
deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
} else
{
bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
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;
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;
}
if (c.m_multiBodyA)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
} else
{
bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
}
if (c.m_multiBodyB)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
} else
{
bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
}
}
void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
btScalar deltaVelADotn=0;
btScalar deltaVelBDotn=0;
int ndofA=0;
int ndofB=0;
if (c.m_multiBodyA)
{
ndofA = c.m_multiBodyA->getNumLinks() + 6;
for (int i = 0; i < ndofA; ++i)
deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
}
if (c.m_multiBodyB)
{
ndofB = c.m_multiBodyB->getNumLinks() + 6;
for (int i = 0; i < ndofB; ++i)
deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
}
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)
{
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;
}
if (c.m_multiBodyA)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
}
if (c.m_multiBodyB)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
}
}
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;
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 = 1.f;
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 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->fillContactJacobian(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->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
} else
{
btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = contactNormal;
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 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->fillContactJacobian(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->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
} else
{
btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormal;
}
{
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;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 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)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + contactNormal.dot(vec);
}
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 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)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + contactNormal.dot(vec);
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
btScalar d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
} else
{
solverConstraint.m_jacDiagABInv = 1.f;
}
}
//compute rhs and remaining solverConstraint fields
btScalar restitution = 0.f;
btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 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)
{
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
}
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 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)
{
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
}
solverConstraint.m_friction = cp.m_combinedFriction;
restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= btScalar(0.))
{
restitution = 0.f;
};
}
///warm starting (or zero if disabled)
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVee(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);
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVee(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);
}
}
} else
{
solverConstraint.m_appliedImpulse = 0.f;
}
solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
}
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_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;
setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
return solverConstraint;
}
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;
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];
///avoid collision response between two static objects
// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
// return;
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_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;
bool isFriction = false;
setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
// const btVector3& pos1 = cp.getPositionWorldOnA();
// const btVector3& pos2 = cp.getPositionWorldOnB();
/////setup the friction constraints
#define ENABLE_FRICTION
#ifdef ENABLE_FRICTION
solverConstraint.m_frictionIndex = frictionIndex;
#if ROLLING_FRICTION
btVector3 angVelA(0,0,0),angVelB(0,0,0);
if (rb0)
angVelA = rb0->getAngularVelocity();
if (rb1)
angVelB = rb1->getAngularVelocity();
btVector3 relAngVel = angVelB-angVelA;
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
{
//only a single rollingFriction per manifold
rollingFriction--;
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
{
relAngVel.normalize();
applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
if (relAngVel.length()>0.001)
addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
} else
{
addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
btVector3 axis0,axis1;
btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
if (axis0.length()>0.001)
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);
}
}
#endif //ROLLING_FRICTION
///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
///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,
///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
///
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
{/*
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)
{
cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
cp.m_lateralFrictionDir2.normalize();//??
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
} else
*/
{
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
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_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) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
{
cp.m_lateralFrictionInitialized = true;
}
}
} else
{
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
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_contactCFM2);
//setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
//todo:
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
}
#endif //ENABLE_FRICTION
}
}
}
void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
{
btPersistentManifold* manifold = 0;
for (int i=0;i<numManifolds;i++)
{
btPersistentManifold* manifold= manifoldPtr[i];
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
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
{
convertMultiBodyContact(manifold,infoGlobal);
}
}
//also convert the multibody constraints, if any
for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
{
btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
m_data.m_fixedBodyId = m_fixedBodyId;
c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal);
}
}
btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
{
return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,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 start\n");
m_tmpMultiBodyConstraints = multiBodyConstraints;
m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
m_tmpMultiBodyConstraints = 0;
m_tmpNumMultiBodyConstraints = 0;
}

85
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h Normal file
View file

@ -0,0 +1,85 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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_CONSTRAINT_SOLVER_H
#define BT_MULTIBODY_CONSTRAINT_SOLVER_H
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "btMultiBodySolverConstraint.h"
class btMultiBody;
#include "btMultiBodyConstraint.h"
ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
{
protected:
btMultiBodyConstraintArray m_multiBodyNonContactConstraints;
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
btMultiBodyJacobianData m_data;
//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
btMultiBodyConstraint** m_tmpMultiBodyConstraints;
int m_tmpNumMultiBodyConstraints;
void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c);
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 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);
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);
public:
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 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

578
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp Normal file
View file

@ -0,0 +1,578 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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 "btMultiBodyDynamicsWorld.h"
#include "btMultiBodyConstraintSolver.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include "LinearMath/btQuickprof.h"
#include "btMultiBodyConstraint.h"
void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
{
m_multiBodies.push_back(body);
}
void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
{
m_multiBodies.remove(body);
}
void btMultiBodyDynamicsWorld::calculateSimulationIslands()
{
BT_PROFILE("calculateSimulationIslands");
getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
{
//merge islands based on speculative contact manifolds too
for (int i=0;i<this->m_predictiveManifolds.size();i++)
{
btPersistentManifold* manifold = m_predictiveManifolds[i];
const btCollisionObject* colObj0 = manifold->getBody0();
const btCollisionObject* colObj1 = manifold->getBody1();
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
{
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
}
}
}
{
int i;
int numConstraints = int(m_constraints.size());
for (i=0;i< numConstraints ; i++ )
{
btTypedConstraint* constraint = m_constraints[i];
if (constraint->isEnabled())
{
const btRigidBody* colObj0 = &constraint->getRigidBodyA();
const btRigidBody* colObj1 = &constraint->getRigidBodyB();
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
{
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
}
}
}
}
//merge islands linked by Featherstone link colliders
for (int i=0;i<m_multiBodies.size();i++)
{
btMultiBody* body = m_multiBodies[i];
{
btMultiBodyLinkCollider* prev = body->getBaseCollider();
for (int b=0;b<body->getNumLinks();b++)
{
btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
((prev) && (!(prev)->isStaticOrKinematicObject())))
{
int tagPrev = prev->getIslandTag();
int tagCur = cur->getIslandTag();
getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
}
if (cur && !cur->isStaticOrKinematicObject())
prev = cur;
}
}
}
//merge islands linked by multibody constraints
{
for (int i=0;i<this->m_multiBodyConstraints.size();i++)
{
btMultiBodyConstraint* c = m_multiBodyConstraints[i];
int tagA = c->getIslandIdA();
int tagB = c->getIslandIdB();
if (tagA>=0 && tagB>=0)
getSimulationIslandManager()->getUnionFind().unite(tagA, tagB);
}
}
//Store the island id in each body
getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
}
void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
{
BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState");
for ( int i=0;i<m_multiBodies.size();i++)
{
btMultiBody* body = m_multiBodies[i];
if (body)
{
body->checkMotionAndSleepIfRequired(timeStep);
if (!body->isAwake())
{
btMultiBodyLinkCollider* col = body->getBaseCollider();
if (col && col->getActivationState() == ACTIVE_TAG)
{
col->setActivationState( WANTS_DEACTIVATION);
col->setDeactivationTime(0.f);
}
for (int b=0;b<body->getNumLinks();b++)
{
btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
if (col && col->getActivationState() == ACTIVE_TAG)
{
col->setActivationState( WANTS_DEACTIVATION);
col->setDeactivationTime(0.f);
}
}
} else
{
btMultiBodyLinkCollider* col = body->getBaseCollider();
if (col && col->getActivationState() != DISABLE_DEACTIVATION)
col->setActivationState( ACTIVE_TAG );
for (int b=0;b<body->getNumLinks();b++)
{
btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
if (col && col->getActivationState() != DISABLE_DEACTIVATION)
col->setActivationState( ACTIVE_TAG );
}
}
}
}
btDiscreteDynamicsWorld::updateActivationState(timeStep);
}
SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs)
{
int islandId;
const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
return islandId;
}
class btSortConstraintOnIslandPredicate2
{
public:
bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const
{
int rIslandId0,lIslandId0;
rIslandId0 = btGetConstraintIslandId2(rhs);
lIslandId0 = btGetConstraintIslandId2(lhs);
return lIslandId0 < rIslandId0;
}
};
SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
{
int islandId;
int islandTagA = lhs->getIslandIdA();
int islandTagB = lhs->getIslandIdB();
islandId= islandTagA>=0?islandTagA:islandTagB;
return islandId;
}
class btSortMultiBodyConstraintOnIslandPredicate
{
public:
bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const
{
int rIslandId0,lIslandId0;
rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
return lIslandId0 < rIslandId0;
}
};
struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
{
btContactSolverInfo* m_solverInfo;
btMultiBodyConstraintSolver* m_solver;
btMultiBodyConstraint** m_multiBodySortedConstraints;
int m_numMultiBodyConstraints;
btTypedConstraint** m_sortedConstraints;
int m_numConstraints;
btIDebugDraw* m_debugDrawer;
btDispatcher* m_dispatcher;
btAlignedObjectArray<btCollisionObject*> m_bodies;
btAlignedObjectArray<btPersistentManifold*> m_manifolds;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver,
btDispatcher* dispatcher)
:m_solverInfo(NULL),
m_solver(solver),
m_multiBodySortedConstraints(NULL),
m_numConstraints(0),
m_debugDrawer(NULL),
m_dispatcher(dispatcher)
{
}
MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other)
{
btAssert(0);
(void)other;
return *this;
}
SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
{
btAssert(solverInfo);
m_solverInfo = solverInfo;
m_multiBodySortedConstraints = sortedMultiBodyConstraints;
m_numMultiBodyConstraints = numMultiBodyConstraints;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_bodies.resize (0);
m_manifolds.resize (0);
m_constraints.resize (0);
m_multiBodyConstraints.resize(0);
}
virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
{
if (islandId<0)
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
} else
{
//also add all non-contact constraints/joints for this island
btTypedConstraint** startConstraint = 0;
btMultiBodyConstraint** startMultiBodyConstraint = 0;
int numCurConstraints = 0;
int numCurMultiBodyConstraints = 0;
int i;
//find the first constraint for this island
for (i=0;i<m_numConstraints;i++)
{
if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
{
startConstraint = &m_sortedConstraints[i];
break;
}
}
//count the number of constraints in this island
for (;i<m_numConstraints;i++)
{
if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
{
numCurConstraints++;
}
}
for (i=0;i<m_numMultiBodyConstraints;i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
break;
}
}
//count the number of multi body constraints in this island
for (;i<m_numMultiBodyConstraints;i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
numCurMultiBodyConstraints++;
}
}
if (m_solverInfo->m_minimumSolverBatchSize<=1)
{
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
} else
{
for (i=0;i<numBodies;i++)
m_bodies.push_back(bodies[i]);
for (i=0;i<numManifolds;i++)
m_manifolds.push_back(manifolds[i]);
for (i=0;i<numCurConstraints;i++)
m_constraints.push_back(startConstraint[i]);
for (i=0;i<numCurMultiBodyConstraints;i++)
m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
{
processConstraints();
} else
{
//printf("deferred\n");
}
}
}
}
void processConstraints()
{
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
m_bodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
}
};
btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
m_multiBodyConstraintSolver(constraintSolver)
{
//split impulse is not yet supported for Featherstone hierarchies
getSolverInfo().m_splitImpulse = false;
getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS;
m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher);
}
btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
{
delete m_solverMultiBodyIslandCallback;
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
BT_PROFILE("solveConstraints");
m_sortedConstraints.resize( m_constraints.size());
int i;
for (i=0;i<getNumConstraints();i++)
{
m_sortedConstraints[i] = m_constraints[i];
}
m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
for (i=0;i<m_multiBodyConstraints.size();i++)
{
m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
}
m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
{
BT_PROFILE("btMultiBody addForce and stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
scratch_r.resize(bod->getNumLinks()+1);
scratch_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);
bod->clearForcesAndTorques();
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
}
}
}
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
btDiscreteDynamicsWorld::integrateTransforms(timeStep);
{
BT_PROFILE("btMultiBody stepPositions");
//integrate and update the Featherstone hierarchies
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
for (int b=0;b<m_multiBodies.size();b++)
{
btMultiBody* bod = m_multiBodies[b];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
int nLinks = bod->getNumLinks();
///base + num links
world_to_local.resize(nLinks+1);
local_origin.resize(nLinks+1);
bod->stepPositions(timeStep);
world_to_local[0] = bod->getWorldToBaseRot();
local_origin[0] = bod->getBasePos();
if (bod->getBaseCollider())
{
btVector3 posr = local_origin[0];
float pos[4]={posr.x(),posr.y(),posr.z(),1};
float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
bod->getBaseCollider()->setWorldTransform(tr);
}
for (int k=0;k<bod->getNumLinks();k++)
{
const int parent = bod->getParent(k);
world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1];
local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k)));
}
for (int m=0;m<bod->getNumLinks();m++)
{
btMultiBodyLinkCollider* col = bod->getLink(m).m_collider;
if (col)
{
int link = col->m_link;
btAssert(link == m);
int index = link+1;
btVector3 posr = local_origin[index];
float pos[4]={posr.x(),posr.y(),posr.z(),1};
float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
}
}
} else
{
bod->clearVelocities();
}
}
}
}
void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
{
m_multiBodyConstraints.push_back(constraint);
}
void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
{
m_multiBodyConstraints.remove(constraint);
}

56
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h Normal file
View file

@ -0,0 +1,56 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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_DYNAMICS_WORLD_H
#define BT_MULTIBODY_DYNAMICS_WORLD_H
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
class btMultiBody;
class btMultiBodyConstraint;
class btMultiBodyConstraintSolver;
struct MultiBodyInplaceSolverIslandCallback;
///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet
///This implementation is still preliminary/experimental.
class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld
{
protected:
btAlignedObjectArray<btMultiBody*> m_multiBodies;
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
virtual void calculateSimulationIslands();
virtual void updateActivationState(btScalar timeStep);
virtual void solveConstraints(btContactSolverInfo& solverInfo);
virtual void integrateTransforms(btScalar timeStep);
public:
btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
virtual ~btMultiBodyDynamicsWorld ();
virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
virtual void removeMultiBody(btMultiBody* body);
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

133
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp Normal file
View file

@ -0,0 +1,133 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyJointLimitConstraint.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
:btMultiBodyConstraint(body,body,link,link,2,true),
m_lowerBound(lower),
m_upperBound(upper)
{
// the data.m_jacobians never change, so may as well
// initialize them here
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
// row 0: the lower bound
jacobianA(0)[6 + link] = 1;
// row 1: the upper bound
jacobianB(1)[6 + link] = -1;
}
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
{
}
int btMultiBodyJointLimitConstraint::getIslandIdA() const
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
return -1;
}
int btMultiBodyJointLimitConstraint::getIslandIdB() const
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
return -1;
}
void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
// row 0: the lower bound
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);
// row 1: the upper bound
setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
for (int row=0;row<getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,-m_maxAppliedImpulse,m_maxAppliedImpulse);
{
btScalar penetration = getPosition(row);
btScalar positionalError = 0.f;
btScalar velocityError = - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = penetrationImpulse;
}
}
}
}

44
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h Normal file
View file

@ -0,0 +1,44 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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_JOINT_LIMIT_CONSTRAINT_H
#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
#include "btMultiBodyConstraint.h"
struct btSolverInfo;
class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
{
protected:
btScalar m_lowerBound;
btScalar m_upperBound;
public:
btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
virtual ~btMultiBodyJointLimitConstraint();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
};
#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H

89
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp Normal file
View file

@ -0,0 +1,89 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyJointMotor.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
:btMultiBodyConstraint(body,body,link,link,1,true),
m_desiredVelocity(desiredVelocity)
{
m_maxAppliedImpulse = maxMotorImpulse;
// the data.m_jacobians never change, so may as well
// initialize them here
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
// row 0: the lower bound
jacobianA(0)[6 + link] = 1;
}
btMultiBodyJointMotor::~btMultiBodyJointMotor()
{
}
int btMultiBodyJointMotor::getIslandIdA() const
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
return -1;
}
int btMultiBodyJointMotor::getIslandIdB() const
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
return -1;
}
void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
for (int row=0;row<getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
btScalar penetration = 0;
fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,m_desiredVelocity,-m_maxAppliedImpulse,m_maxAppliedImpulse);
}
}

47
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h Normal file
View file

@ -0,0 +1,47 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_JOINT_MOTOR_H
#define BT_MULTIBODY_JOINT_MOTOR_H
#include "btMultiBodyConstraint.h"
struct btSolverInfo;
class btMultiBodyJointMotor : public btMultiBodyConstraint
{
protected:
btScalar m_desiredVelocity;
public:
btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
virtual ~btMultiBodyJointMotor();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
};
#endif //BT_MULTIBODY_JOINT_MOTOR_H

110
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h Normal file
View file

@ -0,0 +1,110 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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_LINK_H
#define BT_MULTIBODY_LINK_H
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btVector3.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
enum btMultiBodyLinkFlags
{
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
};
//
// Link struct
//
struct btMultibodyLink
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btScalar joint_pos; // qi
btScalar mass; // mass of link
btVector3 inertia; // inertia of link (local frame; diagonal)
int parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
btQuaternion zero_rot_parent_to_this; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
// for prismatic: axis_top = zero;
// axis_bottom = unit vector along the joint axis.
// for revolute: axis_top = unit vector along the rotation axis (u);
// axis_bottom = u cross d_vector.
btVector3 axis_top;
btVector3 axis_bottom;
btVector3 d_vector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only.
// e_vector is constant, but depends on the joint type
// prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
// revolute: vector from parent's COM to the pivot point, in PARENT's frame.
btVector3 e_vector;
bool is_revolute; // true = revolute, false = prismatic
btQuaternion cached_rot_parent_to_this; // rotates vectors in parent frame to vectors in local frame
btVector3 cached_r_vector; // vector from COM of parent to COM of this link, in local frame.
btVector3 applied_force; // In WORLD frame
btVector3 applied_torque; // In WORLD frame
btScalar joint_torque;
class btMultiBodyLinkCollider* m_collider;
int m_flags;
// ctor: set some sensible defaults
btMultibodyLink()
: joint_pos(0),
mass(1),
parent(-1),
zero_rot_parent_to_this(1, 0, 0, 0),
is_revolute(false),
cached_rot_parent_to_this(1, 0, 0, 0),
joint_torque(0),
m_collider(0),
m_flags(0)
{
inertia.setValue(1, 1, 1);
axis_top.setValue(0, 0, 0);
axis_bottom.setValue(1, 0, 0);
d_vector.setValue(0, 0, 0);
e_vector.setValue(0, 0, 0);
cached_r_vector.setValue(0, 0, 0);
applied_force.setValue( 0, 0, 0);
applied_torque.setValue(0, 0, 0);
}
// routine to update cached_rot_parent_to_this and cached_r_vector
void updateCache()
{
if (is_revolute)
{
cached_rot_parent_to_this = btQuaternion(axis_top,-joint_pos) * zero_rot_parent_to_this;
cached_r_vector = d_vector + quatRotate(cached_rot_parent_to_this,e_vector);
} else
{
// cached_rot_parent_to_this never changes, so no need to update
cached_r_vector = e_vector + joint_pos * axis_bottom;
}
}
};
#endif //BT_MULTIBODY_LINK_H

92
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h Normal file
View file

@ -0,0 +1,92 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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_FEATHERSTONE_LINK_COLLIDER_H
#define BT_FEATHERSTONE_LINK_COLLIDER_H
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "btMultiBody.h"
class btMultiBodyLinkCollider : public btCollisionObject
{
//protected:
public:
btMultiBody* m_multiBody;
int m_link;
btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
:m_multiBody(multiBody),
m_link(link)
{
m_checkCollideWith = true;
//we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands
//this means that some constraints might point to bodies that are not in the islands, causing crashes
//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
{
m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
}
// else
//{
// m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT);
//}
m_internalType = CO_FEATHERSTONE_LINK;
}
static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
{
if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
return (btMultiBodyLinkCollider*)colObj;
return 0;
}
static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
{
if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
return (btMultiBodyLinkCollider*)colObj;
return 0;
}
virtual bool checkCollideWithOverride(const btCollisionObject* co) const
{
const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
if (!other)
return true;
if (other->m_multiBody != this->m_multiBody)
return true;
if (!m_multiBody->hasSelfCollision())
return false;
//check if 'link' has collision disabled
if (m_link>=0)
{
const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.parent == other->m_link)
return false;
}
if (other->m_link>=0)
{
const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.parent == this->m_link)
return false;
}
return true;
}
};
#endif //BT_FEATHERSTONE_LINK_COLLIDER_H

143
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp Normal file
View file

@ -0,0 +1,143 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyPoint2Point.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(body,0,link,-1,3,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB)
{
}
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,3,false),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB)
{
}
btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
{
}
int btMultiBodyPoint2Point::getIslandIdA() const
{
if (m_rigidBodyA)
return m_rigidBodyA->getIslandTag();
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyPoint2Point::getIslandIdB() const
{
if (m_rigidBodyB)
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// int i=1;
for (int i=0;i<3;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
btVector3 contactNormalOnB(0,0,0);
contactNormalOnB[i] = -1;
btScalar penetration = 0;
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
if (m_rigidBodyA)
{
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
} else
{
if (m_bodyA)
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
}
btVector3 pivotBworld = m_pivotInB;
if (m_rigidBodyB)
{
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
} else
{
if (m_bodyB)
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
}
btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
btScalar relaxation = 1.f;
fillMultiBodyConstraintMixed(constraintRow, data,
contactNormalOnB,
pivotAworld, pivotBworld,
position,
infoGlobal,
relaxation,
false);
constraintRow.m_lowerLimit = -m_maxAppliedImpulse;
constraintRow.m_upperLimit = m_maxAppliedImpulse;
}
}

60
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h Normal file
View file

@ -0,0 +1,60 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_POINT2POINT_H
#define BT_MULTIBODY_POINT2POINT_H
#include "btMultiBodyConstraint.h"
class btMultiBodyPoint2Point : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
public:
btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
virtual ~btMultiBodyPoint2Point();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInB() const
{
return m_pivotInB;
}
void setPivotInB(const btVector3& pivotInB)
{
m_pivotInB = pivotInB;
}
};
#endif //BT_MULTIBODY_POINT2POINT_H

82
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h Normal file
View file

@ -0,0 +1,82 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 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,
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_SOLVER_CONSTRAINT_H
#define BT_MULTIBODY_SOLVER_CONSTRAINT_H
#include "LinearMath/btVector3.h"
#include "LinearMath/btAlignedObjectArray.h"
class btMultiBody;
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
{
BT_DECLARE_ALIGNED_ALLOCATOR();
int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
btVector3 m_relpos1CrossNormal;
btVector3 m_contactNormal1;
int m_jacAindex;
int m_deltaVelBindex;
btVector3 m_relpos2CrossNormal;
btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
int m_jacBindex;
btVector3 m_angularComponentA;
btVector3 m_angularComponentB;
mutable btSimdScalar m_appliedPushImpulse;
mutable btSimdScalar m_appliedImpulse;
btScalar m_friction;
btScalar m_jacDiagABInv;
btScalar m_rhs;
btScalar m_cfm;
btScalar m_lowerLimit;
btScalar m_upperLimit;
btScalar m_rhsPenetration;
union
{
void* m_originalContactPoint;
btScalar m_unusedPadding4;
};
int m_overrideNumSolverIterations;
int m_frictionIndex;
int m_solverBodyIdA;
btMultiBody* m_multiBodyA;
int m_linkA;
int m_solverBodyIdB;
btMultiBody* m_multiBodyB;
int m_linkB;
enum btSolverConstraintType
{
BT_SOLVER_CONTACT_1D = 0,
BT_SOLVER_FRICTION_1D
};
};
typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray;
#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H

2079
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp Normal file
View file

File diff suppressed because it is too large Load diff

77
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h Normal file
View file

@ -0,0 +1,77 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of *
* The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i)
satisfies one of
(1) x = lo, w >= 0
(2) x = hi, w <= 0
(3) lo < x < hi, w = 0
A is a matrix of dimension n*n, everything else is a vector of size n*1.
lo and hi can be +/- dInfinity as needed. the first `nub' variables are
unbounded, i.e. hi and lo are assumed to be +/- dInfinity.
we restrict lo(i) <= 0 and hi(i) >= 0.
the original data (A,b) may be modified by this function.
if the `findex' (friction index) parameter is nonzero, it points to an array
of index values. in this case constraints that have findex[i] >= 0 are
special. all non-special constraints are solved for, then the lo and hi values
for the special constraints are set:
hi[i] = abs( hi[i] * x[findex[i]] )
lo[i] = -hi[i]
and the solution continues. this mechanism allows a friction approximation
to be implemented. the first `nub' variables are assumed to have findex < 0.
*/
#ifndef _BT_LCP_H_
#define _BT_LCP_H_
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
struct btDantzigScratchMemory
{
btAlignedObjectArray<btScalar> m_scratch;
btAlignedObjectArray<btScalar> L;
btAlignedObjectArray<btScalar> d;
btAlignedObjectArray<btScalar> delta_w;
btAlignedObjectArray<btScalar> delta_x;
btAlignedObjectArray<btScalar> Dell;
btAlignedObjectArray<btScalar> ell;
btAlignedObjectArray<btScalar*> Arows;
btAlignedObjectArray<int> p;
btAlignedObjectArray<int> C;
btAlignedObjectArray<bool> state;
};
//return false if solving failed
bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w,
int nub, btScalar *lo, btScalar *hi, int *findex,btDantzigScratchMemory& scratch);
#endif //_BT_LCP_H_

112
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h Normal file
View file

@ -0,0 +1,112 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 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,
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.
*/
///original version written by Erwin Coumans, October 2013
#ifndef BT_DANTZIG_SOLVER_H
#define BT_DANTZIG_SOLVER_H
#include "btMLCPSolverInterface.h"
#include "btDantzigLCP.h"
class btDantzigSolver : public btMLCPSolverInterface
{
protected:
btScalar m_acceptableUpperLimitSolution;
btAlignedObjectArray<char> m_tempBuffer;
btAlignedObjectArray<btScalar> m_A;
btAlignedObjectArray<btScalar> m_b;
btAlignedObjectArray<btScalar> m_x;
btAlignedObjectArray<btScalar> m_lo;
btAlignedObjectArray<btScalar> m_hi;
btAlignedObjectArray<int> m_dependencies;
btDantzigScratchMemory m_scratchMemory;
public:
btDantzigSolver()
:m_acceptableUpperLimitSolution(btScalar(1000))
{
}
virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
{
bool result = true;
int n = b.rows();
if (n)
{
int nub = 0;
btAlignedObjectArray<btScalar> ww;
ww.resize(n);
const btScalar* Aptr = A.getBufferPointer();
m_A.resize(n*n);
for (int i=0;i<n*n;i++)
{
m_A[i] = Aptr[i];
}
m_b.resize(n);
m_x.resize(n);
m_lo.resize(n);
m_hi.resize(n);
m_dependencies.resize(n);
for (int i=0;i<n;i++)
{
m_lo[i] = lo[i];
m_hi[i] = hi[i];
m_b[i] = b[i];
m_x[i] = x[i];
m_dependencies[i] = limitDependency[i];
}
result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0],m_scratchMemory);
if (!result)
return result;
// printf("numAllocas = %d\n",numAllocas);
for (int i=0;i<n;i++)
{
volatile btScalar xx = m_x[i];
if (xx != m_x[i])
return false;
if (x[i] >= m_acceptableUpperLimitSolution)
{
return false;
}
if (x[i] <= -m_acceptableUpperLimitSolution)
{
return false;
}
}
for (int i=0;i<n;i++)
{
x[i] = m_x[i];
}
}
return result;
}
};
#endif //BT_DANTZIG_SOLVER_H

626
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp Normal file
View file

@ -0,0 +1,626 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 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,
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.
*/
///original version written by Erwin Coumans, October 2013
#include "btMLCPSolver.h"
#include "LinearMath/btMatrixX.h"
#include "LinearMath/btQuickprof.h"
#include "btSolveProjectedGaussSeidel.h"
btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver)
:m_solver(solver),
m_fallback(0)
{
}
btMLCPSolver::~btMLCPSolver()
{
}
bool gUseMatrixMultiply = false;
bool interleaveContactAndFriction = false;
btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies, numBodiesUnUsed, manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
{
BT_PROFILE("gather constraint data");
int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2;
int numBodies = m_tmpSolverBodyPool.size();
m_allConstraintArray.resize(0);
m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
// printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size());
int dindex = 0;
for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++)
{
m_allConstraintArray.push_back(m_tmpSolverNonContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1;
}
///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
int firstContactConstraintOffset=dindex;
if (interleaveContactAndFriction)
{
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
{
m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1;
m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact));
m_limitDependencies[dindex++] = findex +firstContactConstraintOffset;
if (numFrictionPerContact==2)
{
m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
}
}
} else
{
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
{
m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1;
}
for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++)
{
m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i]);
m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
}
}
if (!m_allConstraintArray.size())
{
m_A.resize(0,0);
m_b.resize(0);
m_x.resize(0);
m_lo.resize(0);
m_hi.resize(0);
return 0.f;
}
}
if (gUseMatrixMultiply)
{
BT_PROFILE("createMLCP");
createMLCP(infoGlobal);
}
else
{
BT_PROFILE("createMLCPFast");
createMLCPFast(infoGlobal);
}
return 0.f;
}
bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
{
bool result = true;
if (m_A.rows()==0)
return true;
//if using split impulse, we solve 2 separate (M)LCPs
if (infoGlobal.m_splitImpulse)
{
btMatrixXu Acopy = m_A;
btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
// printf("solve first LCP\n");
result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
if (result)
result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations );
} else
{
result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
}
return result;
}
struct btJointNode
{
int jointIndex; // pointer to enclosing dxJoint object
int otherBodyIndex; // *other* body this joint is connected to
int nextJointNodeIndex;//-1 for null
int constraintRowIndex;
};
void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
{
int numContactRows = interleaveContactAndFriction ? 3 : 1;
int numConstraintRows = m_allConstraintArray.size();
int n = numConstraintRows;
{
BT_PROFILE("init b (rhs)");
m_b.resize(numConstraintRows);
m_bSplit.resize(numConstraintRows);
//m_b.setZero();
for (int i=0;i<numConstraintRows ;i++)
{
if (m_allConstraintArray[i].m_jacDiagABInv)
{
m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
}
}
}
btScalar* w = 0;
int nub = 0;
m_lo.resize(numConstraintRows);
m_hi.resize(numConstraintRows);
{
BT_PROFILE("init lo/ho");
for (int i=0;i<numConstraintRows;i++)
{
if (0)//m_limitDependencies[i]>=0)
{
m_lo[i] = -BT_INFINITY;
m_hi[i] = BT_INFINITY;
} else
{
m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
m_hi[i] = m_allConstraintArray[i].m_upperLimit;
}
}
}
//
int m=m_allConstraintArray.size();
int numBodies = m_tmpSolverBodyPool.size();
btAlignedObjectArray<int> bodyJointNodeArray;
{
BT_PROFILE("bodyJointNodeArray.resize");
bodyJointNodeArray.resize(numBodies,-1);
}
btAlignedObjectArray<btJointNode> jointNodeArray;
{
BT_PROFILE("jointNodeArray.reserve");
jointNodeArray.reserve(2*m_allConstraintArray.size());
}
static btMatrixXu J3;
{
BT_PROFILE("J3.resize");
J3.resize(2*m,8);
}
static btMatrixXu JinvM3;
{
BT_PROFILE("JinvM3.resize/setZero");
JinvM3.resize(2*m,8);
JinvM3.setZero();
J3.setZero();
}
int cur=0;
int rowOffset = 0;
static btAlignedObjectArray<int> ofs;
{
BT_PROFILE("ofs resize");
ofs.resize(0);
ofs.resizeNoInitialize(m_allConstraintArray.size());
}
{
BT_PROFILE("Compute J and JinvM");
int c=0;
int numRows = 0;
for (int i=0;i<m_allConstraintArray.size();i+=numRows,c++)
{
ofs[c] = rowOffset;
int sbA = m_allConstraintArray[i].m_solverBodyIdA;
int sbB = m_allConstraintArray[i].m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
if (orgBodyA)
{
{
int slotA=-1;
//find free jointNode slot for sbA
slotA =jointNodeArray.size();
jointNodeArray.expand();//NonInitializing();
int prevSlot = bodyJointNodeArray[sbA];
bodyJointNodeArray[sbA] = slotA;
jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
jointNodeArray[slotA].jointIndex = c;
jointNodeArray[slotA].constraintRowIndex = i;
jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
}
for (int row=0;row<numRows;row++,cur++)
{
btVector3 normalInvMass = m_allConstraintArray[i+row].m_contactNormal1 * orgBodyA->getInvMass();
btVector3 relPosCrossNormalInvInertia = m_allConstraintArray[i+row].m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
for (int r=0;r<3;r++)
{
J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal1[r]);
J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos1CrossNormal[r]);
JinvM3.setElem(cur,r,normalInvMass[r]);
JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]);
}
J3.setElem(cur,3,0);
JinvM3.setElem(cur,3,0);
J3.setElem(cur,7,0);
JinvM3.setElem(cur,7,0);
}
} else
{
cur += numRows;
}
if (orgBodyB)
{
{
int slotB=-1;
//find free jointNode slot for sbA
slotB =jointNodeArray.size();
jointNodeArray.expand();//NonInitializing();
int prevSlot = bodyJointNodeArray[sbB];
bodyJointNodeArray[sbB] = slotB;
jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
jointNodeArray[slotB].jointIndex = c;
jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
jointNodeArray[slotB].constraintRowIndex = i;
}
for (int row=0;row<numRows;row++,cur++)
{
btVector3 normalInvMassB = m_allConstraintArray[i+row].m_contactNormal2*orgBodyB->getInvMass();
btVector3 relPosInvInertiaB = m_allConstraintArray[i+row].m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
for (int r=0;r<3;r++)
{
J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal2[r]);
J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos2CrossNormal[r]);
JinvM3.setElem(cur,r,normalInvMassB[r]);
JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]);
}
J3.setElem(cur,3,0);
JinvM3.setElem(cur,3,0);
J3.setElem(cur,7,0);
JinvM3.setElem(cur,7,0);
}
}
else
{
cur += numRows;
}
rowOffset+=numRows;
}
}
//compute JinvM = J*invM.
const btScalar* JinvM = JinvM3.getBufferPointer();
const btScalar* Jptr = J3.getBufferPointer();
{
BT_PROFILE("m_A.resize");
m_A.resize(n,n);
}
{
BT_PROFILE("m_A.setZero");
m_A.setZero();
}
int c=0;
{
int numRows = 0;
BT_PROFILE("Compute A");
for (int i=0;i<m_allConstraintArray.size();i+= numRows,c++)
{
int row__ = ofs[c];
int sbA = m_allConstraintArray[i].m_solverBodyIdA;
int sbB = m_allConstraintArray[i].m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
{
int startJointNodeA = bodyJointNodeArray[sbA];
while (startJointNodeA>=0)
{
int j0 = jointNodeArray[startJointNodeA].jointIndex;
int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
if (j0<c)
{
int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintArray[cr0].m_solverBodyIdB == sbA) ? 8*numRowsOther : 0;
//printf("%d joint i %d and j0: %d: ",count++,i,j0);
m_A.multiplyAdd2_p8r ( JinvMrow,
Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]);
}
startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
}
}
{
int startJointNodeB = bodyJointNodeArray[sbB];
while (startJointNodeB>=0)
{
int j1 = jointNodeArray[startJointNodeB].jointIndex;
int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
if (j1<c)
{
int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintArray[cj1].m_solverBodyIdB == sbB) ? 8*numRowsOther : 0;
m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows,
Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]);
}
startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
}
}
}
{
BT_PROFILE("compute diagonal");
// compute diagonal blocks of m_A
int row__ = 0;
int numJointRows = m_allConstraintArray.size();
int jj=0;
for (;row__<numJointRows;)
{
int sbA = m_allConstraintArray[row__].m_solverBodyIdA;
int sbB = m_allConstraintArray[row__].m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
const btScalar *Jrow = Jptr + 2*8*(size_t)row__;
m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__);
if (orgBodyB)
{
m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__);
}
row__ += infom;
jj++;
}
}
}
///todo: use proper cfm values from the constraints (getInfo2)
if (1)
{
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
{
float cfm = 0.00001f;
m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
}
}
///fill the upper triangle of the matrix, to make it symmetric
{
BT_PROFILE("fill the upper triangle ");
m_A.copyLowerToUpperTriangle();
}
{
BT_PROFILE("resize/init x");
m_x.resize(numConstraintRows);
m_xSplit.resize(numConstraintRows);
if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
{
for (int i=0;i<m_allConstraintArray.size();i++)
{
const btSolverConstraint& c = m_allConstraintArray[i];
m_x[i]=c.m_appliedImpulse;
m_xSplit[i] = c.m_appliedPushImpulse;
}
} else
{
m_x.setZero();
m_xSplit.setZero();
}
}
}
void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
{
int numBodies = this->m_tmpSolverBodyPool.size();
int numConstraintRows = m_allConstraintArray.size();
m_b.resize(numConstraintRows);
if (infoGlobal.m_splitImpulse)
m_bSplit.resize(numConstraintRows);
for (int i=0;i<numConstraintRows ;i++)
{
if (m_allConstraintArray[i].m_jacDiagABInv)
{
m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
if (infoGlobal.m_splitImpulse)
m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
}
}
static btMatrixXu Minv;
Minv.resize(6*numBodies,6*numBodies);
Minv.setZero();
for (int i=0;i<numBodies;i++)
{
const btSolverBody& rb = m_tmpSolverBodyPool[i];
const btVector3& invMass = rb.m_invMass;
setElem(Minv,i*6+0,i*6+0,invMass[0]);
setElem(Minv,i*6+1,i*6+1,invMass[1]);
setElem(Minv,i*6+2,i*6+2,invMass[2]);
btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody;
for (int r=0;r<3;r++)
for (int c=0;c<3;c++)
setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
}
static btMatrixXu J;
J.resize(numConstraintRows,6*numBodies);
J.setZero();
m_lo.resize(numConstraintRows);
m_hi.resize(numConstraintRows);
for (int i=0;i<numConstraintRows;i++)
{
m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
m_hi[i] = m_allConstraintArray[i].m_upperLimit;
int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA;
int bodyIndex1 = m_allConstraintArray[i].m_solverBodyIdB;
if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody)
{
setElem(J,i,6*bodyIndex0+0,m_allConstraintArray[i].m_contactNormal1[0]);
setElem(J,i,6*bodyIndex0+1,m_allConstraintArray[i].m_contactNormal1[1]);
setElem(J,i,6*bodyIndex0+2,m_allConstraintArray[i].m_contactNormal1[2]);
setElem(J,i,6*bodyIndex0+3,m_allConstraintArray[i].m_relpos1CrossNormal[0]);
setElem(J,i,6*bodyIndex0+4,m_allConstraintArray[i].m_relpos1CrossNormal[1]);
setElem(J,i,6*bodyIndex0+5,m_allConstraintArray[i].m_relpos1CrossNormal[2]);
}
if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody)
{
setElem(J,i,6*bodyIndex1+0,m_allConstraintArray[i].m_contactNormal2[0]);
setElem(J,i,6*bodyIndex1+1,m_allConstraintArray[i].m_contactNormal2[1]);
setElem(J,i,6*bodyIndex1+2,m_allConstraintArray[i].m_contactNormal2[2]);
setElem(J,i,6*bodyIndex1+3,m_allConstraintArray[i].m_relpos2CrossNormal[0]);
setElem(J,i,6*bodyIndex1+4,m_allConstraintArray[i].m_relpos2CrossNormal[1]);
setElem(J,i,6*bodyIndex1+5,m_allConstraintArray[i].m_relpos2CrossNormal[2]);
}
}
static btMatrixXu J_transpose;
J_transpose= J.transpose();
static btMatrixXu tmp;
{
{
BT_PROFILE("J*Minv");
tmp = J*Minv;
}
{
BT_PROFILE("J*tmp");
m_A = tmp*J_transpose;
}
}
if (1)
{
///todo: use proper cfm values from the constraints (getInfo2)
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
{
float cfm = 0.0001f;
m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
}
}
m_x.resize(numConstraintRows);
if (infoGlobal.m_splitImpulse)
m_xSplit.resize(numConstraintRows);
// m_x.setZero();
for (int i=0;i<m_allConstraintArray.size();i++)
{
const btSolverConstraint& c = m_allConstraintArray[i];
m_x[i]=c.m_appliedImpulse;
if (infoGlobal.m_splitImpulse)
m_xSplit[i] = c.m_appliedPushImpulse;
}
}
btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
bool result = true;
{
BT_PROFILE("solveMLCP");
// printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
result = solveMLCP(infoGlobal);
}
//check if solution is valid, and otherwise fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations
if (result)
{
BT_PROFILE("process MLCP results");
for (int i=0;i<m_allConstraintArray.size();i++)
{
{
btSolverConstraint& c = m_allConstraintArray[i];
int sbA = c.m_solverBodyIdA;
int sbB = c.m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_x[i]);
solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_x[i]);
if (infoGlobal.m_splitImpulse)
{
solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_xSplit[i]);
solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_xSplit[i]);
c.m_appliedPushImpulse = m_xSplit[i];
}
c.m_appliedImpulse = m_x[i];
}
}
}
else
{
m_fallback++;
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
}
return 0.f;
}

81
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h Normal file
View file

@ -0,0 +1,81 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 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,
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.
*/
///original version written by Erwin Coumans, October 2013
#ifndef BT_MLCP_SOLVER_H
#define BT_MLCP_SOLVER_H
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "LinearMath/btMatrixX.h"
#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
class btMLCPSolver : public btSequentialImpulseConstraintSolver
{
protected:
btMatrixXu m_A;
btVectorXu m_b;
btVectorXu m_x;
btVectorXu m_lo;
btVectorXu m_hi;
///when using 'split impulse' we solve two separate (M)LCPs
btVectorXu m_bSplit;
btVectorXu m_xSplit;
btVectorXu m_bSplit1;
btVectorXu m_xSplit2;
btAlignedObjectArray<int> m_limitDependencies;
btConstraintArray m_allConstraintArray;
btMLCPSolverInterface* m_solver;
int m_fallback;
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 void createMLCP(const btContactSolverInfo& infoGlobal);
virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
//return true is it solves the problem successfully
virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
public:
btMLCPSolver( btMLCPSolverInterface* solver);
virtual ~btMLCPSolver();
void setMLCPSolver(btMLCPSolverInterface* solver)
{
m_solver = solver;
}
int getNumFallbacks() const
{
return m_fallback;
}
void setNumFallbacks(int num)
{
m_fallback = num;
}
virtual btConstraintSolverType getSolverType() const
{
return BT_MLCP_SOLVER;
}
};
#endif //BT_MLCP_SOLVER_H

33
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h Normal file
View file

@ -0,0 +1,33 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 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,
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.
*/
///original version written by Erwin Coumans, October 2013
#ifndef BT_MLCP_SOLVER_INTERFACE_H
#define BT_MLCP_SOLVER_INTERFACE_H
#include "LinearMath/btMatrixX.h"
class btMLCPSolverInterface
{
public:
virtual ~btMLCPSolverInterface()
{
}
//return true is it solves the problem successfully
virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)=0;
};
#endif //BT_MLCP_SOLVER_INTERFACE_H

151
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btPATHSolver.h Normal file
View file

@ -0,0 +1,151 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 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,
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.
*/
///original version written by Erwin Coumans, October 2013
#ifndef BT_PATH_SOLVER_H
#define BT_PATH_SOLVER_H
//#define BT_USE_PATH
#ifdef BT_USE_PATH
extern "C" {
#include "PATH/SimpleLCP.h"
#include "PATH/License.h"
#include "PATH/Error_Interface.h"
};
void __stdcall MyError(Void *data, Char *msg)
{
printf("Path Error: %s\n",msg);
}
void __stdcall MyWarning(Void *data, Char *msg)
{
printf("Path Warning: %s\n",msg);
}
Error_Interface e;
#include "btMLCPSolverInterface.h"
#include "Dantzig/lcp.h"
class btPathSolver : public btMLCPSolverInterface
{
public:
btPathSolver()
{
License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0");
e.error_data = 0;
e.warning = MyWarning;
e.error = MyError;
Error_SetInterface(&e);
}
virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
{
MCP_Termination status;
int numVariables = b.rows();
if (0==numVariables)
return true;
/* - variables - the number of variables in the problem
- m_nnz - the number of nonzeros in the M matrix
- m_i - a vector of size m_nnz containing the row indices for M
- m_j - a vector of size m_nnz containing the column indices for M
- m_ij - a vector of size m_nnz containing the data for M
- q - a vector of size variables
- lb - a vector of size variables containing the lower bounds on x
- ub - a vector of size variables containing the upper bounds on x
*/
btAlignedObjectArray<double> values;
btAlignedObjectArray<int> rowIndices;
btAlignedObjectArray<int> colIndices;
for (int i=0;i<A.rows();i++)
{
for (int j=0;j<A.cols();j++)
{
if (A(i,j)!=0.f)
{
//add 1, because Path starts at 1, instead of 0
rowIndices.push_back(i+1);
colIndices.push_back(j+1);
values.push_back(A(i,j));
}
}
}
int numNonZero = rowIndices.size();
btAlignedObjectArray<double> zResult;
zResult.resize(numVariables);
btAlignedObjectArray<double> rhs;
btAlignedObjectArray<double> upperBounds;
btAlignedObjectArray<double> lowerBounds;
for (int i=0;i<numVariables;i++)
{
upperBounds.push_back(hi[i]);
lowerBounds.push_back(lo[i]);
rhs.push_back(-b[i]);
}
SimpleLCP(numVariables,numNonZero,&rowIndices[0],&colIndices[0],&values[0],&rhs[0],&lowerBounds[0],&upperBounds[0], &status, &zResult[0]);
if (status != MCP_Solved)
{
static const char* gReturnMsgs[] = {
"Invalid return",
"MCP_Solved: The problem was solved",
"MCP_NoProgress: A stationary point was found",
"MCP_MajorIterationLimit: Major iteration limit met",
"MCP_MinorIterationLimit: Cumulative minor iteration limit met",
"MCP_TimeLimit: Ran out of time",
"MCP_UserInterrupt: Control-C, typically",
"MCP_BoundError: Problem has a bound error",
"MCP_DomainError: Could not find starting point",
"MCP_Infeasible: Problem has no solution",
"MCP_Error: An error occurred within the code",
"MCP_LicenseError: License could not be found",
"MCP_OK"
};
printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
printf("using Projected Gauss Seidel fallback\n");
return false;
} else
{
for (int i=0;i<numVariables;i++)
{
x[i] = zResult[i];
//check for #NAN
if (x[i] != zResult[i])
return false;
}
return true;
}
}
};
#endif //BT_USE_PATH
#endif //BT_PATH_SOLVER_H

80
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h Normal file
View file

@ -0,0 +1,80 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 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,
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.
*/
///original version written by Erwin Coumans, October 2013
#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
#include "btMLCPSolverInterface.h"
class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
{
public:
virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
{
//A is a m-n matrix, m rows, n columns
btAssert(A.rows() == b.rows());
int i, j, numRows = A.rows();
float delta;
for (int k = 0; k <numIterations; k++)
{
for (i = 0; i <numRows; i++)
{
delta = 0.0f;
if (useSparsity)
{
for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
{
int j = A.m_rowNonZeroElements1[i][h];
if (j != i)//skip main diagonal
{
delta += A(i,j) * x[j];
}
}
} else
{
for (j = 0; j <i; j++)
delta += A(i,j) * x[j];
for (j = i+1; j<numRows; j++)
delta += A(i,j) * x[j];
}
float aDiag = A(i,i);
x [i] = (b [i] - delta) / A(i,i);
float s = 1.f;
if (limitDependency[i]>=0)
{
s = x[limitDependency[i]];
if (s<0)
s=1;
}
if (x[i]<lo[i]*s)
x[i]=lo[i]*s;
if (x[i]>hi[i]*s)
x[i]=hi[i]*s;
}
}
return true;
}
};
#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H