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

Bullet Library v2.81

Browse source
This commit is contained in:
LuisAntonRebollo 2013-07-04 20:50:16 +02:00
parent 64fef8b2ad
commit 1eb94f4828
462 changed files with 59613 additions and 8036 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1,4 +1,4 @@
INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src } )
INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src )
@ -6,6 +6,7 @@ SET(BulletDynamics_SRCS
Character/btKinematicCharacterController.cpp
ConstraintSolver/btConeTwistConstraint.cpp
ConstraintSolver/btContactConstraint.cpp
ConstraintSolver/btGearConstraint.cpp
ConstraintSolver/btGeneric6DofConstraint.cpp
ConstraintSolver/btGeneric6DofSpringConstraint.cpp
ConstraintSolver/btHinge2Constraint.cpp
@ -16,7 +17,6 @@ SET(BulletDynamics_SRCS
ConstraintSolver/btSolve2LinearConstraint.cpp
ConstraintSolver/btTypedConstraint.cpp
ConstraintSolver/btUniversalConstraint.cpp
Dynamics/btContinuousDynamicsWorld.cpp
Dynamics/btDiscreteDynamicsWorld.cpp
Dynamics/btRigidBody.cpp
Dynamics/btSimpleDynamicsWorld.cpp
@ -34,6 +34,7 @@ SET(ConstraintSolver_HDRS
ConstraintSolver/btConstraintSolver.h
ConstraintSolver/btContactConstraint.h
ConstraintSolver/btContactSolverInfo.h
ConstraintSolver/btGearConstraint.h
ConstraintSolver/btGeneric6DofConstraint.h
ConstraintSolver/btGeneric6DofSpringConstraint.h
ConstraintSolver/btHinge2Constraint.h
@ -50,7 +51,6 @@ SET(ConstraintSolver_HDRS
)
SET(Dynamics_HDRS
Dynamics/btActionInterface.h
Dynamics/btContinuousDynamicsWorld.h
Dynamics/btDiscreteDynamicsWorld.h
Dynamics/btDynamicsWorld.h
Dynamics/btSimpleDynamicsWorld.h
@ -85,23 +85,30 @@ IF (BUILD_SHARED_LIBS)
TARGET_LINK_LIBRARIES(BulletDynamics BulletCollision LinearMath)
ENDIF (BUILD_SHARED_LIBS)
IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
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)
INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} DESTINATION include FILES_MATCHING PATTERN "*.h")
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
IF (INSTALL_LIBS)
IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
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(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN
".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
INSTALL(FILES ../btBulletDynamicsCommon.h
DESTINATION ${INCLUDE_INSTALL_DIR}/BulletDynamics)
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES FRAMEWORK true)
SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES PUBLIC_HEADER "${Root_HDRS}")
# Have to list out sub-directories manually:
SET_PROPERTY(SOURCE ${ConstraintSolver_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/ConstraintSolver)
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)
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES FRAMEWORK true)
SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES PUBLIC_HEADER "${Root_HDRS}")
# Have to list out sub-directories manually:
SET_PROPERTY(SOURCE ${ConstraintSolver_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/ConstraintSolver)
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)
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
ENDIF (INSTALL_LIBS)

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

@ -13,8 +13,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef CHARACTER_CONTROLLER_INTERFACE_H
#define CHARACTER_CONTROLLER_INTERFACE_H
#ifndef BT_CHARACTER_CONTROLLER_INTERFACE_H
#define BT_CHARACTER_CONTROLLER_INTERFACE_H
#include "LinearMath/btVector3.h"
#include "BulletDynamics/Dynamics/btActionInterface.h"
@ -42,4 +42,5 @@ public:
virtual bool onGround () const = 0;
};
#endif
#endif //BT_CHARACTER_CONTROLLER_INTERFACE_H

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

@ -23,8 +23,6 @@ subject to the following restrictions:
#include "LinearMath/btDefaultMotionState.h"
#include "btKinematicCharacterController.h"
static btVector3 upAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
// static helper method
static btVector3
@ -66,20 +64,40 @@ protected:
class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
{
public:
btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
: btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
, m_me(me)
, m_up(up)
, m_minSlopeDot(minSlopeDot)
{
m_me = me;
}
virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
{
if (convexResult.m_hitCollisionObject == m_me)
return 1.0;
return btScalar(1.0);
btVector3 hitNormalWorld;
if (normalInWorldSpace)
{
hitNormalWorld = convexResult.m_hitNormalLocal;
} else
{
///need to transform normal into worldspace
hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
}
btScalar dotUp = m_up.dot(hitNormalWorld);
if (dotUp < m_minSlopeDot) {
return btScalar(1.0);
}
return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
}
protected:
btCollisionObject* m_me;
const btVector3 m_up;
btScalar m_minSlopeDot;
};
/*
@ -112,7 +130,7 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector
btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
{
m_upAxis = upAxis;
m_addedMargin = 0.02f;
m_addedMargin = 0.02;
m_walkDirection.setValue(0,0,0);
m_useGhostObjectSweepTest = true;
m_ghostObject = ghostObject;
@ -121,6 +139,14 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho
m_convexShape=convexShape;
m_useWalkDirection = true; // use walk direction by default, legacy behavior
m_velocityTimeInterval = 0.0;
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_gravity = 9.8 * 3 ; // 3G acceleration.
m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
m_jumpSpeed = 10.0; // ?
m_wasOnGround = false;
m_wasJumping = false;
setMaxSlope(btRadians(45.0));
}
btKinematicCharacterController::~btKinematicCharacterController ()
@ -134,7 +160,21 @@ btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
{
// Here we must refresh the overlapping paircache as the penetrating movement itself or the
// previous recovery iteration might have used setWorldTransform and pushed us into an object
// that is not in the previous cache contents from the last timestep, as will happen if we
// are pushed into a new AABB overlap. Unhandled this means the next convex sweep gets stuck.
//
// Do this by calling the broadphase's setAabb with the moved AABB, this will update the broadphase
// paircache and the ghostobject's internal paircache at the same time. /BW
btVector3 minAabb, maxAabb;
m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb,maxAabb);
collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(),
minAabb,
maxAabb,
collisionWorld->getDispatcher());
bool penetration = false;
collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
@ -160,18 +200,20 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
{
const btManifoldPoint&pt = manifold->getContactPoint(p);
if (pt.getDistance() < 0.0)
btScalar dist = pt.getDistance();
if (dist < 0.0)
{
if (pt.getDistance() < maxPen)
if (dist < maxPen)
{
maxPen = pt.getDistance();
maxPen = dist;
m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
}
m_currentPosition += pt.m_normalWorldOnB * directionSign * pt.getDistance() * btScalar(0.2);
m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
penetration = true;
} else {
//printf("touching %f\n", pt.getDistance());
//printf("touching %f\n", dist);
}
}
@ -189,16 +231,16 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
{
// phase 1: up
btTransform start, end;
m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;
m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
start.setIdentity ();
end.setIdentity ();
/* FIXME: Handle penetration properly */
start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f));
start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@ -213,9 +255,15 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
if (callback.hasHit())
{
// 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);
// Only modify the position if the hit was a slope and not a wall or ceiling.
if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
{
// 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);
}
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
} else {
m_currentStepOffset = m_stepHeight;
m_currentPosition = m_targetPosition;
@ -265,6 +313,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
// phase 2: forward and strafe
btTransform start, end;
m_targetPosition = m_currentPosition + walkMove;
start.setIdentity ();
end.setIdentity ();
@ -275,7 +324,9 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
if (m_touchingContact)
{
if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
{
updateTargetPositionBasedOnCollision (m_touchingNormal);
}
}
int maxIter = 10;
@ -284,8 +335,9 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
{
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@ -310,18 +362,10 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
if (callback.hasHit())
{
// we moved only a fraction
btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
if (hitDistance<0.f)
{
// printf("neg dist?\n");
}
btScalar hitDistance;
hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
/* If the distance is farther than the collision margin, move */
if (hitDistance > m_addedMargin)
{
// printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
}
// m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
btVector3 currentDir = m_targetPosition - m_currentPosition;
@ -339,6 +383,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
// printf("currentDir: don't normalize a zero vector\n");
break;
}
} else {
// we moved whole way
m_currentPosition = m_targetPosition;
@ -355,9 +400,21 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
btTransform start, end;
// phase 3: down
btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight;
m_targetPosition -= (step_drop + gravity_drop);
/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
m_targetPosition -= (step_drop + gravity_drop);*/
btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
if(downVelocity > 0.0 && downVelocity < m_stepHeight
&& (m_wasOnGround || !m_wasJumping))
{
downVelocity = m_stepHeight;
}
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
start.setIdentity ();
end.setIdentity ();
@ -365,7 +422,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@ -381,6 +438,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
// we dropped a fraction of the height -> hit floor
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_wasJumping = false;
} else {
// we dropped the full height
@ -411,7 +471,7 @@ btScalar timeInterval
// printf("setVelocity!\n");
// printf(" interval: %f\n", timeInterval);
// printf(" velocity: (%f, %f, %f)\n",
// velocity.x(), velocity.y(), velocity.z());
// velocity.x(), velocity.y(), velocity.z());
m_useWalkDirection = false;
m_walkDirection = velocity;
@ -445,7 +505,7 @@ void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
// printf("character could not recover from penetration = %d\n", numPenetrationLoops);
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
@ -457,6 +517,8 @@ void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld
}
#include <stdio.h>
void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
{
// printf("playerStep(): ");
@ -468,6 +530,21 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
return; // no motion
}
m_wasOnGround = onGround();
// Update fall velocity.
m_verticalVelocity -= m_gravity * dt;
if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
{
m_verticalVelocity = m_jumpSpeed;
}
if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
{
m_verticalVelocity = -btFabs(m_fallSpeed);
}
m_verticalOffset = m_verticalVelocity * dt;
btTransform xform;
xform = m_ghostObject->getWorldTransform ();
@ -481,13 +558,13 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
//printf(" time: %f", m_velocityTimeInterval);
// still have some time left for moving!
btScalar dtMoving =
(dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
(dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
m_velocityTimeInterval -= dt;
// how far will we move while we are moving?
btVector3 move = m_walkDirection * dtMoving;
// printf(" dtMoving: %f", dtMoving);
//printf(" dtMoving: %f", dtMoving);
// okay, step
stepForwardAndStrafe(collisionWorld, move);
@ -525,6 +602,9 @@ void btKinematicCharacterController::jump ()
if (!canJump())
return;
m_verticalVelocity = m_jumpSpeed;
m_wasJumping = true;
#if 0
currently no jumping.
btTransform xform;
@ -536,12 +616,40 @@ void btKinematicCharacterController::jump ()
#endif
}
void btKinematicCharacterController::setGravity(btScalar gravity)
{
m_gravity = gravity;
}
btScalar btKinematicCharacterController::getGravity() const
{
return m_gravity;
}
void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
{
m_maxSlopeRadians = slopeRadians;
m_maxSlopeCosine = btCos(slopeRadians);
}
btScalar btKinematicCharacterController::getMaxSlope() const
{
return m_maxSlopeRadians;
}
bool btKinematicCharacterController::onGround () const
{
return true;
return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
}
void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
btVector3* btKinematicCharacterController::getUpAxisDirections()
{
static btVector3 sUpAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
return sUpAxisDirection;
}
void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
{
}

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

@ -13,8 +13,9 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef KINEMATIC_CHARACTER_CONTROLLER_H
#define KINEMATIC_CHARACTER_CONTROLLER_H
#ifndef BT_KINEMATIC_CHARACTER_CONTROLLER_H
#define BT_KINEMATIC_CHARACTER_CONTROLLER_H
#include "LinearMath/btVector3.h"
@ -24,6 +25,7 @@ subject to the following restrictions:
class btCollisionShape;
class btConvexShape;
class btRigidBody;
class btCollisionWorld;
class btCollisionDispatcher;
@ -32,17 +34,23 @@ class btPairCachingGhostObject;
///btKinematicCharacterController is an object that supports a sliding motion in a world.
///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations.
///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user.
class btKinematicCharacterController : public btCharacterControllerInterface
ATTRIBUTE_ALIGNED16(class) btKinematicCharacterController : public btCharacterControllerInterface
{
protected:
btScalar m_halfHeight;
btPairCachingGhostObject* m_ghostObject;
btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
btScalar m_verticalVelocity;
btScalar m_verticalOffset;
btScalar m_fallSpeed;
btScalar m_jumpSpeed;
btScalar m_maxJumpHeight;
btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
btScalar m_maxSlopeCosine; // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
btScalar m_gravity;
btScalar m_turnAngle;
@ -65,11 +73,15 @@ protected:
bool m_touchingContact;
btVector3 m_touchingNormal;
bool m_wasOnGround;
bool m_wasJumping;
bool m_useGhostObjectSweepTest;
bool m_useWalkDirection;
float m_velocityTimeInterval;
btScalar m_velocityTimeInterval;
int m_upAxis;
static btVector3* getUpAxisDirections();
btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
@ -80,6 +92,9 @@ protected:
void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
~btKinematicCharacterController ();
@ -105,14 +120,14 @@ public:
/// This should probably be called setPositionIncrementPerSimulatorStep.
/// This is neither a direction nor a velocity, but the amount to
/// increment the position each simulation iteration, regardless
/// of dt.
/// increment the position each simulation iteration, regardless
/// of dt.
/// This call will reset any velocity set by setVelocityForTimeInterval().
virtual void setWalkDirection(const btVector3& walkDirection);
/// Caller provides a velocity with which the character should move for
/// the given time period. After the time period, velocity is reset
/// to zero.
/// the given time period. After the time period, velocity is reset
/// to zero.
/// This call will reset any walk direction set by setWalkDirection().
/// Negative time intervals will result in no motion.
virtual void setVelocityForTimeInterval(const btVector3& velocity,
@ -128,8 +143,17 @@ public:
void setJumpSpeed (btScalar jumpSpeed);
void setMaxJumpHeight (btScalar maxJumpHeight);
bool canJump () const;
void jump ();
void setGravity(btScalar gravity);
btScalar getGravity() const;
/// The max slope determines the maximum angle that the controller can walk up.
/// The slope angle is measured in radians.
void setMaxSlope(btScalar slopeRadians);
btScalar getMaxSlope() const;
btPairCachingGhostObject* getGhostObject();
void setUseGhostSweepTest(bool useGhostObjectSweepTest)
{
@ -139,4 +163,4 @@ public:
bool onGround () const;
};
#endif // KINEMATIC_CHARACTER_CONTROLLER_H
#endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H

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

@ -36,11 +36,6 @@ SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axi
}
btConeTwistConstraint::btConeTwistConstraint()
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
{
}
btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
@ -73,6 +68,10 @@ void btConeTwistConstraint::init()
setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
m_damping = btScalar(0.01);
m_fixThresh = CONETWIST_DEF_FIX_THRESH;
m_flags = 0;
m_linCFM = btScalar(0.f);
m_linERP = btScalar(0.7f);
m_angCFM = btScalar(0.f);
}
@ -145,13 +144,18 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// set right hand side
btScalar k = info->fps * info->erp;
btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
btScalar k = info->fps * linERP;
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
{
info->cfm[j*info->rowskip] = m_linCFM;
}
}
int row = 3;
int srow = row * info->rowskip;
@ -200,7 +204,10 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
btScalar k = info->fps * m_biasFactor;
info->m_constraintError[srow] = k * m_swingCorrection;
info->cfm[srow] = 0.0f;
if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
{
info->cfm[srow] = m_angCFM;
}
// m_swingCorrection is always positive or 0
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
@ -220,7 +227,10 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
J2[srow+2] = -ax1[2];
btScalar k = info->fps * m_biasFactor;
info->m_constraintError[srow] = k * m_twistCorrection;
info->cfm[srow] = 0.0f;
if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
{
info->cfm[srow] = m_angCFM;
}
if(m_twistSpan > 0.0f)
{
@ -311,9 +321,9 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 vel1;
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
btVector3 vel2;
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
btVector3 vel = vel1 - vel2;
for (int i=0;i<3;i++)
@ -330,8 +340,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
}
}
@ -342,8 +352,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
// compute current and predicted transforms
btTransform trACur = m_rbA.getCenterOfMassTransform();
btTransform trBCur = m_rbB.getCenterOfMassTransform();
btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
btTransform trAPred; trAPred.setIdentity();
btVector3 zerovec(0,0,0);
btTransformUtil::integrateTransform(
@ -417,15 +427,15 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
btScalar impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
}
}
else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
{
btVector3 angVelA; bodyA.getAngularVelocity(angVelA);
btVector3 angVelB; bodyB.getAngularVelocity(angVelB);
btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
btVector3 relVel = angVelB - angVelA;
if (relVel.length2() > SIMD_EPSILON)
{
@ -437,8 +447,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
btScalar impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
}
}
@ -446,9 +456,9 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
{
///solve angular part
btVector3 angVelA;
bodyA.getAngularVelocity(angVelA);
bodyA.internalGetAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.getAngularVelocity(angVelB);
bodyB.internalGetAngularVelocity(angVelB);
// solve swing limit
if (m_solveSwingLimit)
@ -477,8 +487,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
impulseMag = impulse.length();
btVector3 noTwistSwingAxis = impulse / impulseMag;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
}
@ -496,10 +506,10 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
impulseMag = m_accTwistLimitImpulse - temp;
btVector3 impulse = m_twistAxis * impulseMag;
// btVector3 impulse = m_twistAxis * impulseMag;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
}
}
}
@ -628,6 +638,11 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
btTransform trDeltaAB = trB * trPose * trA.inverse();
btQuaternion qDeltaAB = trDeltaAB.getRotation();
btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
float swingAxisLen2 = swingAxis.length2();
if(btFuzzyZero(swingAxisLen2))
{
return;
}
m_swingAxis = swingAxis;
m_swingAxis.normalize();
m_swingCorrection = qDeltaAB.getAngle();
@ -813,12 +828,11 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
{
vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
vSwingAxis.normalize();
if (fabs(vSwingAxis.x()) > SIMD_EPSILON)
{
// non-zero twist?! this should never happen.
int wtf = 0; wtf = wtf;
}
#if 0
// non-zero twist?! this should never happen.
btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
#endif
// Compute limit for given swing. tricky:
// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
@ -862,8 +876,10 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
else if (swingAngle < 0)
{
// this should never happen!
int wtf = 0; wtf = wtf;
}
#if 0
btAssert(0);
#endif
}
}
btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
@ -908,13 +924,15 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qMinTwist = operator-(qTwist);
qMinTwist = -(qTwist);
twistAngle = qMinTwist.getAngle();
}
if (twistAngle < 0)
{
// this should never happen
int wtf = 0; wtf = wtf;
#if 0
btAssert(0);
#endif
}
vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
@ -961,10 +979,10 @@ void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
{
btTransform trACur = m_rbA.getCenterOfMassTransform();
btTransform trBCur = m_rbB.getCenterOfMassTransform();
btTransform trABCur = trBCur.inverse() * trACur;
btQuaternion qABCur = trABCur.getRotation();
btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
btQuaternion qConstraintCur = trConstraintCur.getRotation();
// btTransform trABCur = trBCur.inverse() * trACur;
// btQuaternion qABCur = trABCur.getRotation();
// btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
//btQuaternion qConstraintCur = trConstraintCur.getRotation();
btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
setMotorTargetInConstraintSpace(qConstraint);
@ -1021,6 +1039,97 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &
}
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
{
switch(num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
if((axis >= 0) && (axis < 3))
{
m_linERP = value;
m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
}
else
{
m_biasFactor = value;
}
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
if((axis >= 0) && (axis < 3))
{
m_linCFM = value;
m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
}
else
{
m_angCFM = value;
m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
}
break;
default:
btAssertConstrParams(0);
break;
}
}
///return the local value of parameter
btScalar btConeTwistConstraint::getParam(int num, int axis) const
{
btScalar retVal = 0;
switch(num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
if((axis >= 0) && (axis < 3))
{
btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
retVal = m_linERP;
}
else if((axis >= 3) && (axis < 6))
{
retVal = m_biasFactor;
}
else
{
btAssertConstrParams(0);
}
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
if((axis >= 0) && (axis < 3))
{
btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
retVal = m_linCFM;
}
else if((axis >= 3) && (axis < 6))
{
btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
retVal = m_angCFM;
}
else
{
btAssertConstrParams(0);
}
break;
default :
btAssertConstrParams(0);
}
return retVal;
}
void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
{
m_rbAFrame = frameA;
m_rbBFrame = frameB;
buildJacobian();
//calculateTransforms();
}

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

@ -33,8 +33,8 @@ and swing 1 and 2 are along the z and y axes respectively.
#ifndef CONETWISTCONSTRAINT_H
#define CONETWISTCONSTRAINT_H
#ifndef BT_CONETWISTCONSTRAINT_H
#define BT_CONETWISTCONSTRAINT_H
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
@ -42,9 +42,15 @@ and swing 1 and 2 are along the z and y axes respectively.
class btRigidBody;
enum btConeTwistFlags
{
BT_CONETWIST_FLAGS_LIN_CFM = 1,
BT_CONETWIST_FLAGS_LIN_ERP = 2,
BT_CONETWIST_FLAGS_ANG_CFM = 4
};
///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
class btConeTwistConstraint : public btTypedConstraint
ATTRIBUTE_ALIGNED16(class) btConeTwistConstraint : public btTypedConstraint
{
#ifdef IN_PARALLELL_SOLVER
public:
@ -99,14 +105,33 @@ public:
btScalar m_maxMotorImpulse;
btVector3 m_accMotorImpulse;
// parameters
int m_flags;
btScalar m_linCFM;
btScalar m_linERP;
btScalar m_angCFM;
protected:
void init();
void computeConeLimitInfo(const btQuaternion& qCone, // in
btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
void computeTwistLimitInfo(const btQuaternion& qTwist, // in
btScalar& twistAngle, btVector3& vTwistAxis); // all outs
void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
btConeTwistConstraint();
virtual void buildJacobian();
virtual void getInfo1 (btConstraintInfo1* info);
@ -119,8 +144,10 @@ public:
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
void updateRHS(btScalar timeStep);
const btRigidBody& getRigidBodyA() const
{
return m_rbA;
@ -221,7 +248,6 @@ public:
}
bool isPastSwingLimit() { return m_solveSwingLimit; }
void setDamping(btScalar damping) { m_damping = damping; }
void enableMotor(bool b) { m_bMotorEnabled = b; }
@ -242,18 +268,82 @@ public:
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
const btTransform& getFrameOffsetA() const
{
return m_rbAFrame;
}
const btTransform& getFrameOffsetB() const
{
return m_rbBFrame;
}
protected:
void init();
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
void computeConeLimitInfo(const btQuaternion& qCone, // in
btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
virtual int calculateSerializeBufferSize() const;
void computeTwistLimitInfo(const btQuaternion& qTwist, // in
btScalar& twistAngle, btVector3& vTwistAxis); // all outs
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
};
#endif //CONETWISTCONSTRAINT_H
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btConeTwistConstraintData
{
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame;
btTransformFloatData m_rbBFrame;
//limits
float m_swingSpan1;
float m_swingSpan2;
float m_twistSpan;
float m_limitSoftness;
float m_biasFactor;
float m_relaxationFactor;
float m_damping;
char m_pad[4];
};
SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
{
return sizeof(btConeTwistConstraintData);
}
///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;
btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
m_rbAFrame.serializeFloat(cone->m_rbAFrame);
m_rbBFrame.serializeFloat(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);
return "btConeTwistConstraintData";
}
#endif //BT_CONETWISTCONSTRAINT_H

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

@ -13,8 +13,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef CONSTRAINT_SOLVER_H
#define CONSTRAINT_SOLVER_H
#ifndef BT_CONSTRAINT_SOLVER_H
#define BT_CONSTRAINT_SOLVER_H
#include "LinearMath/btScalar.h"
@ -49,4 +49,4 @@ public:
#endif //CONSTRAINT_SOLVER_H
#endif //BT_CONSTRAINT_SOLVER_H

59
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
View file

@ -22,10 +22,7 @@ subject to the following restrictions:
#include "LinearMath/btMinMax.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
btContactConstraint::btContactConstraint()
:btTypedConstraint(CONTACT_CONSTRAINT_TYPE)
{
}
btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
@ -59,10 +56,6 @@ void btContactConstraint::buildJacobian()
}
void btContactConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
}
@ -75,9 +68,53 @@ void btContactConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBo
#include "LinearMath/btMinMax.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
#define ASSERT2 btAssert
#define USE_INTERNAL_APPLY_IMPULSE 1
//response between two dynamic objects without friction and no restitution, assuming 0 penetration depth
btScalar resolveSingleCollision(
btRigidBody* body1,
btCollisionObject* colObj2,
const btVector3& contactPositionWorld,
const btVector3& contactNormalOnB,
const btContactSolverInfo& solverInfo,
btScalar distance)
{
btRigidBody* body2 = btRigidBody::upcast(colObj2);
const btVector3& normal = contactNormalOnB;
btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin();
btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
btVector3 vel = vel1 - vel2;
btScalar rel_vel;
rel_vel = normal.dot(vel);
btScalar combinedRestitution = 0.f;
btScalar restitution = combinedRestitution* -rel_vel;
btScalar positionalError = solverInfo.m_erp *-distance /solverInfo.m_timeStep ;
btScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping;
btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld,normal);
btScalar denom1 = body2? body2->computeImpulseDenominator(contactPositionWorld,normal) : 0.f;
btScalar relaxation = 1.f;
btScalar jacDiagABInv = relaxation/(denom0+denom1);
btScalar penetrationImpulse = positionalError * jacDiagABInv;
btScalar velocityImpulse = velocityError * jacDiagABInv;
btScalar normalImpulse = penetrationImpulse+velocityImpulse;
normalImpulse = 0.f > normalImpulse ? 0.f: normalImpulse;
body1->applyImpulse(normal*(normalImpulse), rel_pos1);
if (body2)
body2->applyImpulse(-normal*(normalImpulse), rel_pos2);
return normalImpulse;
}
//bilateral constraint between two dynamic objects
@ -90,7 +127,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
btScalar normalLenSqr = normal.length2();
ASSERT2(btFabs(normalLenSqr) < btScalar(1.1));
btAssert(btFabs(normalLenSqr) < btScalar(1.1));
if (normalLenSqr > btScalar(1.1))
{
impulse = btScalar(0.);

12
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
View file

@ -13,8 +13,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef CONTACT_CONSTRAINT_H
#define CONTACT_CONSTRAINT_H
#ifndef BT_CONTACT_CONSTRAINT_H
#define BT_CONTACT_CONSTRAINT_H
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
@ -30,7 +30,6 @@ protected:
public:
btContactConstraint();
btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
@ -55,11 +54,12 @@ public:
///obsolete methods
virtual void buildJacobian();
///obsolete methods
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
};
///very basic collision resolution without friction
btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld,const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo,btScalar distance);
///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
@ -68,4 +68,4 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
#endif //CONTACT_CONSTRAINT_H
#endif //BT_CONTACT_CONSTRAINT_H

96
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View file

@ -13,21 +13,23 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef CONTACT_SOLVER_INFO
#define CONTACT_SOLVER_INFO
#ifndef BT_CONTACT_SOLVER_INFO
#define BT_CONTACT_SOLVER_INFO
#include "LinearMath/btScalar.h"
enum btSolverMode
{
SOLVER_RANDMIZE_ORDER = 1,
SOLVER_FRICTION_SEPARATE = 2,
SOLVER_USE_WARMSTARTING = 4,
SOLVER_USE_FRICTION_WARMSTARTING = 8,
SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
SOLVER_CACHE_FRIENDLY = 128,
SOLVER_SIMD = 256, //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
SOLVER_CUDA = 512 //will be open sourced during Game Developers Conference 2009. Much faster.
SOLVER_SIMD = 256,
SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
};
struct btContactSolverInfoData
@ -35,7 +37,7 @@ struct btContactSolverInfoData
btScalar m_tau;
btScalar m_damping;
btScalar m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
btScalar m_friction;
btScalar m_timeStep;
btScalar m_restitution;
@ -47,11 +49,15 @@ struct btContactSolverInfoData
btScalar m_globalCfm;//constraint force mixing
int m_splitImpulse;
btScalar m_splitImpulsePenetrationThreshold;
btScalar m_splitImpulseTurnErp;
btScalar m_linearSlop;
btScalar m_warmstartingFactor;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
btScalar m_maxGyroscopicForce;
btScalar m_singleAxisRollingFrictionThreshold;
};
@ -66,20 +72,88 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_tau = btScalar(0.6);
m_damping = btScalar(1.0);
m_friction = btScalar(0.3);
m_timeStep = btScalar(1.f/60.f);
m_restitution = btScalar(0.);
m_maxErrorReduction = btScalar(20.);
m_numIterations = 10;
m_erp = btScalar(0.2);
m_erp2 = btScalar(0.1);
m_erp2 = btScalar(0.8);
m_globalCfm = btScalar(0.);
m_sor = btScalar(1.);
m_splitImpulse = false;
m_splitImpulsePenetrationThreshold = -0.02f;
m_splitImpulse = true;
m_splitImpulsePenetrationThreshold = -.04f;
m_splitImpulseTurnErp = 0.1f;
m_linearSlop = btScalar(0.0);
m_warmstartingFactor=btScalar(0.85);
//m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
m_restingContactRestitutionThreshold = 2;//unused as of 2.81
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag)
m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
}
};
#endif //CONTACT_SOLVER_INFO
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btContactSolverInfoDoubleData
{
double m_tau;
double m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
double m_friction;
double m_timeStep;
double m_restitution;
double m_maxErrorReduction;
double m_sor;
double m_erp;//used as Baumgarte factor
double m_erp2;//used in Split Impulse
double m_globalCfm;//constraint force mixing
double m_splitImpulsePenetrationThreshold;
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_maxGyroscopicForce;
double m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btContactSolverInfoFloatData
{
float m_tau;
float m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
float m_friction;
float m_timeStep;
float m_restitution;
float m_maxErrorReduction;
float m_sor;
float m_erp;//used as Baumgarte factor
float m_erp2;//used in Split Impulse
float m_globalCfm;//constraint force mixing
float m_splitImpulsePenetrationThreshold;
float m_splitImpulseTurnErp;
float m_linearSlop;
float m_warmstartingFactor;
float m_maxGyroscopicForce;
float m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
#endif //BT_CONTACT_SOLVER_INFO

54
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp Normal file
View file

@ -0,0 +1,54 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou.
#include "btGearConstraint.h"
btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio)
:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB),
m_axisInA(axisInA),
m_axisInB(axisInB),
m_ratio(ratio)
{
}
btGearConstraint::~btGearConstraint ()
{
}
void btGearConstraint::getInfo1 (btConstraintInfo1* info)
{
info->m_numConstraintRows = 1;
info->nub = 1;
}
void btGearConstraint::getInfo2 (btConstraintInfo2* info)
{
btVector3 globalAxisA, globalAxisB;
globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA;
globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB;
info->m_J1angularAxis[0] = globalAxisA[0];
info->m_J1angularAxis[1] = globalAxisA[1];
info->m_J1angularAxis[2] = globalAxisA[2];
info->m_J2angularAxis[0] = m_ratio*globalAxisB[0];
info->m_J2angularAxis[1] = m_ratio*globalAxisB[1];
info->m_J2angularAxis[2] = m_ratio*globalAxisB[2];
}

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

@ -0,0 +1,56 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_GEAR_CONSTRAINT_H
#define BT_GEAR_CONSTRAINT_H
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
///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
{
protected:
btVector3 m_axisInA;
btVector3 m_axisInB;
bool m_useFrameA;
btScalar m_ratio;
public:
btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
virtual ~btGearConstraint ();
///internal method used by the constraint solver, don't use them directly
virtual void getInfo1 (btConstraintInfo1* info);
///internal method used by the constraint solver, don't use them directly
virtual void getInfo2 (btConstraintInfo2* info);
virtual void setParam(int num, btScalar value, int axis = -1)
{
btAssert(0);
};
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const
{
btAssert(0);
return 0.f;
}
};
#endif //BT_GEAR_CONSTRAINT_H

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

@ -28,14 +28,10 @@ http://gimpact.sf.net
#define D6_USE_OBSOLETE_METHOD false
#define D6_USE_FRAME_OFFSET true
btGeneric6DofConstraint::btGeneric6DofConstraint()
:btTypedConstraint(D6_CONSTRAINT_TYPE),
m_useLinearReferenceFrameA(true),
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
{
}
@ -44,13 +40,31 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody&
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
m_flags(0),
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
{
calculateTransforms();
}
btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
: btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameB),
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
m_flags(0),
m_useSolveConstraintObsolete(false)
{
///not providing rigidbody A means implicitly using worldspace for body A
m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
calculateTransforms();
}
#define GENERIC_D6_DISABLE_WARMSTARTING 1
@ -116,12 +130,20 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)
{
m_currentLimit = 1;//low limit violation
m_currentLimitError = test_value - m_loLimit;
if(m_currentLimitError>SIMD_PI)
m_currentLimitError-=SIMD_2_PI;
else if(m_currentLimitError<-SIMD_PI)
m_currentLimitError+=SIMD_2_PI;
return 1;
}
else if (test_value> m_hiLimit)
{
m_currentLimit = 2;//High limit violation
m_currentLimitError = test_value - m_hiLimit;
if(m_currentLimitError>SIMD_PI)
m_currentLimitError-=SIMD_2_PI;
else if(m_currentLimitError<-SIMD_PI)
m_currentLimitError+=SIMD_2_PI;
return 2;
};
@ -134,7 +156,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)
btScalar btRotationalLimitMotor::solveAngularLimits(
btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
btRigidBody * body0, btRigidBody * body1 )
{
if (needApplyTorques()==false) return 0.0f;
@ -144,7 +166,7 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
//current error correction
if (m_currentLimit!=0)
{
target_velocity = -m_ERP*m_currentLimitError/(timeStep);
target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
maxMotorForce = m_maxLimitForce;
}
@ -152,10 +174,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
// current velocity difference
btVector3 angVelA;
bodyA.getAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.getAngularVelocity(angVelB);
btVector3 angVelA = body0->getAngularVelocity();
btVector3 angVelB = body1->getAngularVelocity();
btVector3 vel_diff;
vel_diff = angVelA-angVelB;
@ -203,12 +223,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
btVector3 motorImp = clippedMotorImpulse * axis;
//body0->applyTorqueImpulse(motorImp);
//body1->applyTorqueImpulse(-motorImp);
bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
body0->applyTorqueImpulse(motorImp);
body1->applyTorqueImpulse(-motorImp);
return clippedMotorImpulse;
@ -257,8 +273,8 @@ int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_valu
btScalar btTranslationalLimitMotor::solveLinearAxis(
btScalar timeStep,
btScalar jacDiagABInv,
btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
btRigidBody& body1,const btVector3 &pointInA,
btRigidBody& body2,const btVector3 &pointInB,
int limit_index,
const btVector3 & axis_normal_on_a,
const btVector3 & anchorPos)
@ -270,10 +286,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
btVector3 vel1;
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
btVector3 vel2;
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
btScalar rel_vel = axis_normal_on_a.dot(vel);
@ -326,16 +340,10 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
//body1.applyImpulse( impulse_vector, rel_pos1);
//body2.applyImpulse(-impulse_vector, rel_pos2);
btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
body1.applyImpulse( impulse_vector, rel_pos1);
body2.applyImpulse(-impulse_vector, rel_pos2);
return normalImpulse;
}
@ -384,6 +392,22 @@ void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,cons
m_calculatedTransformB = transB * m_frameInB;
calculateLinearInfo();
calculateAngleInfo();
if(m_useOffsetForConstraintFrame)
{ // get weight factors depending on masses
btScalar miA = getRigidBodyA().getInvMass();
btScalar miB = getRigidBodyB().getInvMass();
m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
btScalar miS = miA + miB;
if(miS > btScalar(0.f))
{
m_factA = miB / miS;
}
else
{
m_factA = btScalar(0.5f);
}
m_factB = btScalar(1.0f) - m_factA;
}
}
@ -544,43 +568,59 @@ void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
{
getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(),m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
btAssert(!m_useSolveConstraintObsolete);
const btTransform& transA = m_rbA.getCenterOfMassTransform();
const btTransform& transB = m_rbB.getCenterOfMassTransform();
const btVector3& linVelA = m_rbA.getLinearVelocity();
const btVector3& linVelB = m_rbB.getLinearVelocity();
const btVector3& angVelA = m_rbA.getAngularVelocity();
const btVector3& angVelB = m_rbB.getAngularVelocity();
if(m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
}
else
{ // leave old version for compatibility
int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
}
}
void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
{
btAssert(!m_useSolveConstraintObsolete);
//prepare constraint
calculateTransforms(transA,transB);
int i;
//test linear limits
for(i = 0; i < 3; i++)
{
if(m_linearLimits.needApplyForce(i))
{
}
}
//test angular limits
for (i=0;i<3 ;i++ )
{
if(testAngularLimitMotor(i))
{
}
testAngularLimitMotor(i);
}
int row = setLinearLimits(info,transA,transB,linVelA,linVelB,angVelA,angVelB);
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
if(m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
}
else
{ // leave old version for compatibility
int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
}
}
int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
{
int row = 0;
// int row = 0;
//solve linear limits
btRotationalLimitMotor limot;
for (int i=0;i<3 ;i++ )
@ -593,7 +633,6 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info,const btTra
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_damping = m_linearLimits.m_damping;
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_ERP = m_linearLimits.m_restitution;
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
@ -601,9 +640,25 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info,const btTra
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
row += get_limit_motor_info2(&limot,
transA,transB,linVelA,linVelB,angVelA,angVelB
, info, row, axis, 0);
int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
if(m_useOffsetForConstraintFrame)
{
int indx1 = (i + 1) % 3;
int indx2 = (i + 2) % 3;
int rotAllowed = 1; // rotations around orthos to current axis
if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
{
rotAllowed = 0;
}
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
}
else
{
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
}
}
}
return row;
@ -621,10 +676,21 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o
if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
{
btVector3 axis = d6constraint->getAxis(i);
row += get_limit_motor_info2(
d6constraint->getRotationalLimitMotor(i),
transA,transB,linVelA,linVelB,angVelA,angVelB,
info,row,axis,1);
int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
{
m_angularLimits[i].m_normalCFM = info->cfm[0];
}
if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
{
m_angularLimits[i].m_stopCFM = info->cfm[0];
}
if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
{
m_angularLimits[i].m_stopERP = info->erp;
}
row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
}
}
@ -633,66 +699,6 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o
void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
if (m_useSolveConstraintObsolete)
{
m_timeStep = timeStep;
//calculateTransforms();
int i;
// linear
btVector3 pointInA = m_calculatedTransformA.getOrigin();
btVector3 pointInB = m_calculatedTransformB.getOrigin();
btScalar jacDiagABInv;
btVector3 linear_axis;
for (i=0;i<3;i++)
{
if (m_linearLimits.isLimited(i))
{
jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
if (m_useLinearReferenceFrameA)
linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
else
linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
m_linearLimits.solveLinearAxis(
m_timeStep,
jacDiagABInv,
m_rbA,bodyA,pointInA,
m_rbB,bodyB,pointInB,
i,linear_axis, m_AnchorPos);
}
}
// angular
btVector3 angular_axis;
btScalar angularJacDiagABInv;
for (i=0;i<3;i++)
{
if (m_angularLimits[i].needApplyTorques())
{
// get axis
angular_axis = getAxis(i);
angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB);
}
}
}
}
void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
{
@ -701,6 +707,15 @@ void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
}
void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
{
m_frameInA = frameA;
m_frameInB = frameB;
buildJacobian();
calculateTransforms();
}
btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
{
@ -758,7 +773,7 @@ void btGeneric6DofConstraint::calculateLinearInfo()
int btGeneric6DofConstraint::get_limit_motor_info2(
btRotationalLimitMotor * limot,
const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
{
int srow = row * info->rowskip;
int powered = limot->m_enableMotor;
@ -778,18 +793,51 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
}
if((!rotational))
{
btVector3 ltd; // Linear Torque Decoupling vector
btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
ltd = c.cross(ax1);
info->m_J1angularAxis[srow+0] = ltd[0];
info->m_J1angularAxis[srow+1] = ltd[1];
info->m_J1angularAxis[srow+2] = ltd[2];
if (m_useOffsetForConstraintFrame)
{
btVector3 tmpA, tmpB, relA, relB;
// get vector from bodyB to frameB in WCS
relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
// get its projection to constraint axis
btVector3 projB = ax1 * relB.dot(ax1);
// get vector directed from bodyB to constraint axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
btVector3 projA = ax1 * relA.dot(ax1);
btVector3 orthoA = relA - projA;
// get desired offset between frames A and B along constraint axis
btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
btVector3 totalDist = projA + ax1 * desiredOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * m_factA;
relB = orthoB - totalDist * m_factB;
tmpA = relA.cross(ax1);
tmpB = relB.cross(ax1);
if(m_hasStaticBody && (!rotAllowed))
{
tmpA *= m_factA;
tmpB *= m_factB;
}
int i;
for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
} else
{
btVector3 ltd; // Linear Torque Decoupling vector
btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
ltd = c.cross(ax1);
info->m_J1angularAxis[srow+0] = ltd[0];
info->m_J1angularAxis[srow+1] = ltd[1];
info->m_J1angularAxis[srow+2] = ltd[2];
c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
ltd = -c.cross(ax1);
info->m_J2angularAxis[srow+0] = ltd[0];
info->m_J2angularAxis[srow+1] = ltd[1];
info->m_J2angularAxis[srow+2] = ltd[2];
c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
ltd = -c.cross(ax1);
info->m_J2angularAxis[srow+0] = ltd[0];
info->m_J2angularAxis[srow+1] = ltd[1];
info->m_J2angularAxis[srow+2] = ltd[2];
}
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
@ -797,7 +845,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
info->m_constraintError[srow] = btScalar(0.f);
if (powered)
{
info->cfm[srow] = 0.0f;
info->cfm[srow] = limot->m_normalCFM;
if(!limit)
{
btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
@ -806,7 +854,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
limot->m_loLimit,
limot->m_hiLimit,
tag_vel,
info->fps * info->erp);
info->fps * limot->m_stopERP);
info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
info->m_upperLimit[srow] = limot->m_maxMotorForce;
@ -814,7 +862,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
}
if(limit)
{
btScalar k = info->fps * limot->m_ERP;
btScalar k = info->fps * limot->m_stopERP;
if(!rotational)
{
info->m_constraintError[srow] += k * limot->m_currentLimitError;
@ -823,7 +871,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
{
info->m_constraintError[srow] += -k * limot->m_currentLimitError;
}
info->cfm[srow] = 0.0f;
info->cfm[srow] = limot->m_stopCFM;
if (limot->m_loLimit == limot->m_hiLimit)
{ // limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
@ -891,3 +939,126 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
{
if((axis >= 0) && (axis < 3))
{
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
m_linearLimits.m_stopERP[axis] = value;
m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
break;
case BT_CONSTRAINT_STOP_CFM :
m_linearLimits.m_stopCFM[axis] = value;
m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
break;
case BT_CONSTRAINT_CFM :
m_linearLimits.m_normalCFM[axis] = value;
m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
break;
default :
btAssertConstrParams(0);
}
}
else if((axis >=3) && (axis < 6))
{
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
m_angularLimits[axis - 3].m_stopERP = value;
m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
break;
case BT_CONSTRAINT_STOP_CFM :
m_angularLimits[axis - 3].m_stopCFM = value;
m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
break;
case BT_CONSTRAINT_CFM :
m_angularLimits[axis - 3].m_normalCFM = value;
m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
break;
default :
btAssertConstrParams(0);
}
}
else
{
btAssertConstrParams(0);
}
}
///return the local value of parameter
btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
{
btScalar retVal = 0;
if((axis >= 0) && (axis < 3))
{
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_stopERP[axis];
break;
case BT_CONSTRAINT_STOP_CFM :
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_stopCFM[axis];
break;
case BT_CONSTRAINT_CFM :
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_normalCFM[axis];
break;
default :
btAssertConstrParams(0);
}
}
else if((axis >=3) && (axis < 6))
{
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_stopERP;
break;
case BT_CONSTRAINT_STOP_CFM :
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_stopCFM;
break;
case BT_CONSTRAINT_CFM :
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_normalCFM;
break;
default :
btAssertConstrParams(0);
}
}
else
{
btAssertConstrParams(0);
}
return retVal;
}
void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
{
btVector3 zAxis = axis1.normalized();
btVector3 yAxis = axis2.normalized();
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
btTransform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
// now get constraint frame in local coordinate systems
m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
calculateTransforms();
}

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

@ -24,8 +24,8 @@ http://gimpact.sf.net
*/
#ifndef GENERIC_6DOF_CONSTRAINT_H
#define GENERIC_6DOF_CONSTRAINT_H
#ifndef BT_GENERIC_6DOF_CONSTRAINT_H
#define BT_GENERIC_6DOF_CONSTRAINT_H
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
@ -49,7 +49,9 @@ public:
btScalar m_maxLimitForce;//!< max force on limit
btScalar m_damping;//!< Damping.
btScalar m_limitSoftness;//! Relaxation factor
btScalar m_ERP;//!< Error tolerance factor when joint is at limit
btScalar m_normalCFM;//!< Constraint force mixing factor
btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
btScalar m_bounce;//!< restitution factor
bool m_enableMotor;
@ -71,7 +73,9 @@ public:
m_maxLimitForce = 300.0f;
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
m_ERP = 0.5f;
m_normalCFM = 0.f;
m_stopERP = 0.2f;
m_stopCFM = 0.f;
m_bounce = 0.0f;
m_damping = 1.0f;
m_limitSoftness = 0.5f;
@ -87,7 +91,9 @@ public:
m_limitSoftness = limot.m_limitSoftness;
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
m_ERP = limot.m_ERP;
m_normalCFM = limot.m_normalCFM;
m_stopERP = limot.m_stopERP;
m_stopCFM = limot.m_stopCFM;
m_bounce = limot.m_bounce;
m_currentLimit = limot.m_currentLimit;
m_currentLimitError = limot.m_currentLimitError;
@ -117,7 +123,7 @@ public:
int testLimitValue(btScalar test_value);
//! apply the correction impulses for two bodies
btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
};
@ -134,6 +140,9 @@ public:
btScalar m_limitSoftness;//!< Softness for linear limit
btScalar m_damping;//!< Damping for linear limit
btScalar m_restitution;//! Bounce parameter for linear limit
btVector3 m_normalCFM;//!< Constraint force mixing factor
btVector3 m_stopERP;//!< Error tolerance factor when joint is at limit
btVector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit
//!@}
bool m_enableMotor[3];
btVector3 m_targetVelocity;//!< target motor velocity
@ -147,6 +156,9 @@ public:
m_lowerLimit.setValue(0.f,0.f,0.f);
m_upperLimit.setValue(0.f,0.f,0.f);
m_accumulatedImpulse.setValue(0.f,0.f,0.f);
m_normalCFM.setValue(0.f, 0.f, 0.f);
m_stopERP.setValue(0.2f, 0.2f, 0.2f);
m_stopCFM.setValue(0.f, 0.f, 0.f);
m_limitSoftness = 0.7f;
m_damping = btScalar(1.0f);
@ -168,6 +180,10 @@ public:
m_limitSoftness = other.m_limitSoftness ;
m_damping = other.m_damping;
m_restitution = other.m_restitution;
m_normalCFM = other.m_normalCFM;
m_stopERP = other.m_stopERP;
m_stopCFM = other.m_stopCFM;
for(int i=0; i < 3; i++)
{
m_enableMotor[i] = other.m_enableMotor[i];
@ -198,8 +214,8 @@ public:
btScalar solveLinearAxis(
btScalar timeStep,
btScalar jacDiagABInv,
btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
btRigidBody& body1,const btVector3 &pointInA,
btRigidBody& body2,const btVector3 &pointInB,
int limit_index,
const btVector3 & axis_normal_on_a,
const btVector3 & anchorPos);
@ -207,6 +223,15 @@ public:
};
enum bt6DofFlags
{
BT_6DOF_FLAGS_CFM_NORM = 1,
BT_6DOF_FLAGS_CFM_STOP = 2,
BT_6DOF_FLAGS_ERP_STOP = 4
};
#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/*!
btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
@ -221,27 +246,29 @@ This brings support for limit parameters and motors. </li>
<li> Angulars limits have these possible ranges:
<table border=1 >
<tr
<tr>
<td><b>AXIS</b></td>
<td><b>MIN ANGLE</b></td>
<td><b>MAX ANGLE</b></td>
</tr><tr>
<td>X</td>
<td>-PI</td>
<td>PI</td>
<td>-PI</td>
<td>PI</td>
</tr><tr>
<td>Y</td>
<td>-PI/2</td>
<td>PI/2</td>
<td>-PI/2</td>
<td>PI/2</td>
</tr><tr>
<td>Z</td>
<td>-PI/2</td>
<td>PI/2</td>
<td>-PI</td>
<td>PI</td>
</tr>
</table>
</li>
</ul>
*/
class btGeneric6DofConstraint : public btTypedConstraint
ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint
{
protected:
@ -278,11 +305,17 @@ protected:
btVector3 m_calculatedAxisAngleDiff;
btVector3 m_calculatedAxis[3];
btVector3 m_calculatedLinearDiff;
btScalar m_factA;
btScalar m_factB;
bool m_hasStaticBody;
btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
bool m_useLinearReferenceFrameA;
bool m_useOffsetForConstraintFrame;
int m_flags;
//!@}
btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
@ -295,7 +328,7 @@ protected:
int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
int setLinearLimits(btConstraintInfo2 *info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
void buildLinearJacobian(
btJacobianEntry & jacLinear,const btVector3 & normalWorld,
@ -313,13 +346,14 @@ protected:
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
btGeneric6DofConstraint();
btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
//! Calcs global transform of the offsets
/*!
Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
@ -381,8 +415,6 @@ public:
void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
void updateRHS(btScalar timeStep);
//! Get the rotation axis in global coordinates
@ -403,6 +435,7 @@ public:
*/
btScalar getRelativePivotPosition(int axis_index) const;
void setFrames(const btTransform & frameA, const btTransform & frameB);
//! Test angular limit.
/*!
@ -416,10 +449,20 @@ public:
m_linearLimits.m_lowerLimit = linearLower;
}
void setLinearUpperLimit(const btVector3& linearUpper)
{
m_linearLimits.m_upperLimit = linearUpper;
}
void getLinearLowerLimit(btVector3& linearLower)
{
linearLower = m_linearLimits.m_lowerLimit;
}
void setLinearUpperLimit(const btVector3& linearUpper)
{
m_linearLimits.m_upperLimit = linearUpper;
}
void getLinearUpperLimit(btVector3& linearUpper)
{
linearUpper = m_linearLimits.m_upperLimit;
}
void setAngularLowerLimit(const btVector3& angularLower)
{
@ -427,12 +470,24 @@ public:
m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
}
void getAngularLowerLimit(btVector3& angularLower)
{
for(int i = 0; i < 3; i++)
angularLower[i] = m_angularLimits[i].m_loLimit;
}
void setAngularUpperLimit(const btVector3& angularUpper)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
}
void getAngularUpperLimit(btVector3& angularUpper)
{
for(int i = 0; i < 3; i++)
angularUpper[i] = m_angularLimits[i].m_hiLimit;
}
//! Retrieves the angular limit informacion
btRotationalLimitMotor * getRotationalLimitMotor(int index)
{
@ -483,10 +538,79 @@ public:
int get_limit_motor_info2( btRotationalLimitMotor * limot,
const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
// access for UseFrameOffset
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
void setAxis( const btVector3& axis1, const btVector3& axis2);
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 btGeneric6DofConstraintData
{
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformFloatData m_rbBFrame;
btVector3FloatData m_linearUpperLimit;
btVector3FloatData m_linearLowerLimit;
#endif //GENERIC_6DOF_CONSTRAINT_H
btVector3FloatData m_angularUpperLimit;
btVector3FloatData m_angularLowerLimit;
int m_useLinearReferenceFrameA;
int m_useOffsetForConstraintFrame;
};
SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
{
return sizeof(btGeneric6DofConstraintData);
}
///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;
btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
m_frameInA.serializeFloat(dof->m_rbAFrame);
m_frameInB.serializeFloat(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_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
return "btGeneric6DofConstraintData";
}
#endif //BT_GENERIC_6DOF_CONSTRAINT_H

49
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
View file

@ -21,6 +21,21 @@ subject to the following restrictions:
btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
: btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
{
init();
}
btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
: btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB)
{
init();
}
void btGeneric6DofSpringConstraint::init()
{
m_objectType = D6_SPRING_CONSTRAINT_TYPE;
for(int i = 0; i < 6; i++)
{
m_springEnabled[i] = false;
@ -64,11 +79,13 @@ void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
void btGeneric6DofSpringConstraint::setEquilibriumPoint()
{
calculateTransforms();
for(int i = 0; i < 3; i++)
int i;
for( i = 0; i < 3; i++)
{
m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
}
for(int i = 0; i < 3; i++)
for(i = 0; i < 3; i++)
{
m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
}
@ -86,17 +103,22 @@ void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
}
else
{
m_equilibriumPoint[index + 3] = m_calculatedAxisAngleDiff[index];
m_equilibriumPoint[index] = m_calculatedAxisAngleDiff[index - 3];
}
}
void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val)
{
btAssert((index >= 0) && (index < 6));
m_equilibriumPoint[index] = val;
}
void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
{
// it is assumed that calculateTransforms() have been called before this call
int i;
btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
//btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
for(i = 0; i < 3; i++)
{
if(m_springEnabled[i])
@ -140,5 +162,24 @@ void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
}
void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
{
btVector3 zAxis = axis1.normalized();
btVector3 yAxis = axis2.normalized();
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
btTransform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
// now get constraint frame in local coordinate systems
m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
calculateTransforms();
}

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

@ -13,8 +13,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef GENERIC_6DOF_SPRING_CONSTRAINT_H
#define GENERIC_6DOF_SPRING_CONSTRAINT_H
#ifndef BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
#define BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
#include "LinearMath/btVector3.h"
@ -32,23 +32,71 @@ subject to the following restrictions:
/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] )
/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] )
class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
{
protected:
bool m_springEnabled[6];
btScalar m_equilibriumPoint[6];
btScalar m_springStiffness[6];
btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping)
void init();
void internalUpdateSprings(btConstraintInfo2* info);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
void enableSpring(int index, bool onOff);
void setStiffness(int index, btScalar stiffness);
void setDamping(int index, btScalar damping);
void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF
void setEquilibriumPoint(int index, btScalar val);
virtual void setAxis( const btVector3& axis1, const btVector3& axis2);
virtual void getInfo2 (btConstraintInfo2* info);
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;
};
#endif // GENERIC_6DOF_SPRING_CONSTRAINT_H
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btGeneric6DofSpringConstraintData
{
btGeneric6DofConstraintData m_6dofData;
int m_springEnabled[6];
float m_equilibriumPoint[6];
float m_springStiffness[6];
float m_springDamping[6];
};
SIMD_FORCE_INLINE int btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
{
return sizeof(btGeneric6DofSpringConstraintData);
}
///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;
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_springEnabled[i] = m_springEnabled[i]? 1 : 0;
dof->m_springStiffness[i] = (float)m_springStiffness[i];
}
return "btGeneric6DofSpringConstraintData";
}
#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H

10
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
View file

@ -13,8 +13,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef HINGE2_CONSTRAINT_H
#define HINGE2_CONSTRAINT_H
#ifndef BT_HINGE2_CONSTRAINT_H
#define BT_HINGE2_CONSTRAINT_H
@ -29,13 +29,15 @@ subject to the following restrictions:
// 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2)
// 1 translational (along axis Z) with suspension spring
class btHinge2Constraint : public btGeneric6DofSpringConstraint
ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpringConstraint
{
protected:
btVector3 m_anchor;
btVector3 m_axis1;
btVector3 m_axis2;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
@ -54,5 +56,5 @@ public:
#endif // HINGE2_CONSTRAINT_H
#endif // BT_HINGE2_CONSTRAINT_H

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

@ -23,29 +23,29 @@ subject to the following restrictions:
//#define HINGE_USE_OBSOLETE_SOLVER false
#define HINGE_USE_OBSOLETE_SOLVER false
#define HINGE_USE_FRAME_OFFSET true
#ifndef __SPU__
btHingeConstraint::btHingeConstraint()
: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useReferenceFrameA(false)
{
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
#ifdef _BT_USE_CENTER_LIMIT_
m_limit(),
#endif
m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useReferenceFrameA(useReferenceFrameA)
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
{
m_rbAFrame.getOrigin() = pivotInA;
@ -78,6 +78,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
@ -85,15 +86,22 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
#ifdef _BT_USE_CENTER_LIMIT_
m_limit(),
#endif
m_angularOnly(false), m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useReferenceFrameA(useReferenceFrameA)
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
{
// since no frame is given, assume this to be zero angle and just pick rb transform axis
@ -118,6 +126,7 @@ m_useReferenceFrameA(useReferenceFrameA)
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
@ -125,6 +134,7 @@ m_useReferenceFrameA(useReferenceFrameA)
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
@ -133,11 +143,17 @@ m_useReferenceFrameA(useReferenceFrameA)
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
#ifdef _BT_USE_CENTER_LIMIT_
m_limit(),
#endif
m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useReferenceFrameA(useReferenceFrameA)
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
{
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
@ -145,6 +161,7 @@ m_useReferenceFrameA(useReferenceFrameA)
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
@ -152,15 +169,20 @@ m_useReferenceFrameA(useReferenceFrameA)
btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
#ifdef _BT_USE_CENTER_LIMIT_
m_limit(),
#endif
m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useReferenceFrameA(useReferenceFrameA)
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
{
///not providing rigidbody B means implicitly using worldspace for body B
m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
@ -168,6 +190,7 @@ m_useReferenceFrameA(useReferenceFrameA)
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
@ -222,7 +245,6 @@ void btHingeConstraint::buildJacobian()
btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
@ -259,164 +281,6 @@ void btHingeConstraint::buildJacobian()
}
}
void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
///for backwards compatibility during the transition to 'getInfo/getInfo2'
if (m_useSolveConstraintObsolete)
{
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
btScalar tau = btScalar(0.3);
//linear part
if (!m_angularOnly)
{
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 vel1,vel2;
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
btVector3 vel = vel1 - vel2;
for (int i=0;i<3;i++)
{
const btVector3& normal = m_jac[i].m_linearJointAxis;
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
btScalar rel_vel;
rel_vel = normal.dot(vel);
//positional error (zeroth order error)
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse;
btVector3 impulse_vector = normal * impulse;
btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
}
}
{
///solve angular part
// get axes in world space
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2);
btVector3 angVelA;
bodyA.getAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.getAngularVelocity(angVelB);
btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
btVector3 velrelOrthog = angAorthog-angBorthog;
{
//solve orthogonal angular velocity correction
//btScalar relaxation = btScalar(1.);
btScalar len = velrelOrthog.length();
if (len > btScalar(0.00001))
{
btVector3 normal = velrelOrthog.normalized();
btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
getRigidBodyB().computeAngularImpulseDenominator(normal);
// scale for mass and relaxation
//velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
}
//solve angular positional correction
btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep);
btScalar len2 = angularError.length();
if (len2>btScalar(0.00001))
{
btVector3 normal2 = angularError.normalized();
btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
getRigidBodyB().computeAngularImpulseDenominator(normal2);
//angularError *= (btScalar(1.)/denom2) * relaxation;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
}
// solve limit
if (m_solveLimit)
{
btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign;
btScalar impulseMag = amplitude * m_kHinge;
// Clamp the accumulated impulse
btScalar temp = m_accLimitImpulse;
m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
impulseMag = m_accLimitImpulse - temp;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
}
}
//apply motor
if (m_enableAngularMotor)
{
//todo: add limits too
btVector3 angularLimit(0,0,0);
btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
btScalar projRelVel = velrel.dot(axisA);
btScalar desiredMotorVel = m_motorTargetVelocity;
btScalar motor_relvel = desiredMotorVel - projRelVel;
btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
// accumulated impulse clipping:
btScalar fMaxImpulse = m_maxMotorImpulse;
btScalar newAccImpulse = m_accMotorImpulse + unclippedMotorImpulse;
btScalar clippedMotorImpulse = unclippedMotorImpulse;
if (newAccImpulse > fMaxImpulse)
{
newAccImpulse = fMaxImpulse;
clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
}
else if (newAccImpulse < -fMaxImpulse)
{
newAccImpulse = -fMaxImpulse;
clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
}
m_accMotorImpulse += clippedMotorImpulse;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
}
}
}
}
#endif //__SPU__
@ -461,7 +325,14 @@ void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
{
getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
if(m_useOffsetForConstraintFrame)
{
getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
}
else
{
getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
}
}
@ -507,10 +378,13 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
}
#endif //#if 0
// linear (all fixed)
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[skip + 1] = 1;
info->m_J1linearAxis[2 * skip + 2] = 1;
if (!m_angularOnly)
{
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[skip + 1] = 1;
info->m_J1linearAxis[2 * skip + 2] = 1;
}
@ -532,10 +406,13 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
}
// linear RHS
btScalar k = info->fps * info->erp;
for(i = 0; i < 3; i++)
{
info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
}
if (!m_angularOnly)
{
for(i = 0; i < 3; i++)
{
info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
}
}
// make rotations around X and Y equal
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
@ -592,8 +469,13 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
int limit = 0;
if(getSolveLimit())
{
limit_err = m_correction * m_referenceSign;
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
#ifdef _BT_USE_CENTER_LIMIT_
limit_err = m_limit.getCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
int powered = 0;
@ -620,19 +502,26 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
powered = 0;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
if(powered)
{
info->cfm[srow] = btScalar(0.0);
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
{
info->cfm[srow] = m_normalCFM;
}
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
info->m_lowerLimit[srow] = - m_maxMotorImpulse;
info->m_upperLimit[srow] = m_maxMotorImpulse;
}
if(limit)
{
k = info->fps * info->erp;
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
info->cfm[srow] = btScalar(0.0);
if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
{
info->cfm[srow] = m_stopCFM;
}
if(lostop == histop)
{
// limited low and high simultaneously
@ -650,7 +539,11 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
info->m_upperLimit[srow] = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
#ifdef _BT_USE_CENTER_LIMIT_
btScalar bounce = m_limit.getRelaxationFactor();
#else
btScalar bounce = m_relaxationFactor;
#endif
if(bounce > btScalar(0.0))
{
btScalar vel = angVelA.dot(ax1);
@ -680,14 +573,22 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
}
}
}
#ifdef _BT_USE_CENTER_LIMIT_
info->m_constraintError[srow] *= m_limit.getBiasFactor();
#else
info->m_constraintError[srow] *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
}
void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
{
m_rbAFrame = frameA;
m_rbBFrame = frameB;
buildJacobian();
}
void btHingeConstraint::updateRHS(btScalar timeStep)
@ -707,43 +608,20 @@ btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTran
const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
// btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
return m_referenceSign * angle;
}
#if 0
void btHingeConstraint::testLimit()
{
// Compute limit information
m_hingeAngle = getHingeAngle();
m_correction = btScalar(0.);
m_limitSign = btScalar(0.);
m_solveLimit = false;
if (m_lowerLimit <= m_upperLimit)
{
if (m_hingeAngle <= m_lowerLimit)
{
m_correction = (m_lowerLimit - m_hingeAngle);
m_limitSign = 1.0f;
m_solveLimit = true;
}
else if (m_hingeAngle >= m_upperLimit)
{
m_correction = m_upperLimit - m_hingeAngle;
m_limitSign = -1.0f;
m_solveLimit = true;
}
}
return;
}
#else
void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
{
// Compute limit information
m_hingeAngle = getHingeAngle(transA,transB);
#ifdef _BT_USE_CENTER_LIMIT_
m_limit.test(m_hingeAngle);
#else
m_correction = btScalar(0.);
m_limitSign = btScalar(0.);
m_solveLimit = false;
@ -763,9 +641,10 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t
m_solveLimit = true;
}
}
#endif
return;
}
#endif
static btVector3 vHinge(0, 0, btScalar(1));
@ -785,7 +664,7 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
btScalar targetAngle = qHinge.getAngle();
if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qHinge = operator-(qHinge);
qHinge = -(qHinge);
targetAngle = qHinge.getAngle();
}
if (qHinge.getZ() < 0)
@ -796,6 +675,9 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
{
#ifdef _BT_USE_CENTER_LIMIT_
m_limit.fit(targetAngle);
#else
if (m_lowerLimit < m_upperLimit)
{
if (targetAngle < m_lowerLimit)
@ -803,7 +685,7 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
else if (targetAngle > m_upperLimit)
targetAngle = m_upperLimit;
}
#endif
// compute angular velocity
btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
btScalar dAngle = targetAngle - curAngle;
@ -811,3 +693,342 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
}
void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
{
btAssert(!m_useSolveConstraintObsolete);
int i, s = info->rowskip;
// transforms in world space
btTransform trA = transA*m_rbAFrame;
btTransform trB = transB*m_rbBFrame;
// pivot point
// btVector3 pivotAInW = trA.getOrigin();
// btVector3 pivotBInW = trB.getOrigin();
#if 1
// difference between frames in WCS
btVector3 ofs = trB.getOrigin() - trA.getOrigin();
// now get weight factors depending on masses
btScalar miA = getRigidBodyA().getInvMass();
btScalar miB = getRigidBodyB().getInvMass();
bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
btScalar miS = miA + miB;
btScalar factA, factB;
if(miS > btScalar(0.f))
{
factA = miB / miS;
}
else
{
factA = btScalar(0.5f);
}
factB = btScalar(1.0f) - factA;
// get the desired direction of hinge axis
// as weighted sum of Z-orthos of frameA and frameB in WCS
btVector3 ax1A = trA.getBasis().getColumn(2);
btVector3 ax1B = trB.getBasis().getColumn(2);
btVector3 ax1 = ax1A * factA + ax1B * factB;
ax1.normalize();
// fill first 3 rows
// we want: velA + wA x relA == velB + wB x relB
btTransform bodyA_trans = transA;
btTransform bodyB_trans = transB;
int s0 = 0;
int s1 = s;
int s2 = s * 2;
int nrow = 2; // last filled row
btVector3 tmpA, tmpB, relA, relB, p, q;
// get vector from bodyB to frameB in WCS
relB = trB.getOrigin() - bodyB_trans.getOrigin();
// get its projection to hinge axis
btVector3 projB = ax1 * relB.dot(ax1);
// get vector directed from bodyB to hinge axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
relA = trA.getOrigin() - bodyA_trans.getOrigin();
btVector3 projA = ax1 * relA.dot(ax1);
btVector3 orthoA = relA - projA;
btVector3 totalDist = projA - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to hinge axis
p = orthoB * factA + orthoA * factB;
btScalar len2 = p.length2();
if(len2 > SIMD_EPSILON)
{
p /= btSqrt(len2);
}
else
{
p = trA.getBasis().getColumn(1);
}
// make one more ortho
q = ax1.cross(p);
// fill three rows
tmpA = relA.cross(p);
tmpB = relB.cross(p);
for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
tmpA = relA.cross(q);
tmpB = relB.cross(q);
if(hasStaticBody && getSolveLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
tmpA = relA.cross(ax1);
tmpB = relB.cross(ax1);
if(hasStaticBody)
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation
tmpB *= factB;
tmpA *= factA;
}
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
btScalar k = info->fps * info->erp;
if (!m_angularOnly)
{
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];
// compute three elements of right hand side
btScalar rhs = k * p.dot(ofs);
info->m_constraintError[s0] = rhs;
rhs = k * q.dot(ofs);
info->m_constraintError[s1] = rhs;
rhs = k * ax1.dot(ofs);
info->m_constraintError[s2] = rhs;
}
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
int s3 = 3 * s;
int s4 = 4 * s;
info->m_J1angularAxis[s3 + 0] = p[0];
info->m_J1angularAxis[s3 + 1] = p[1];
info->m_J1angularAxis[s3 + 2] = p[2];
info->m_J1angularAxis[s4 + 0] = q[0];
info->m_J1angularAxis[s4 + 1] = q[1];
info->m_J1angularAxis[s4 + 2] = q[2];
info->m_J2angularAxis[s3 + 0] = -p[0];
info->m_J2angularAxis[s3 + 1] = -p[1];
info->m_J2angularAxis[s3 + 2] = -p[2];
info->m_J2angularAxis[s4 + 0] = -q[0];
info->m_J2angularAxis[s4 + 1] = -q[1];
info->m_J2angularAxis[s4 + 2] = -q[2];
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
k = info->fps * info->erp;
btVector3 u = ax1A.cross(ax1B);
info->m_constraintError[s3] = k * u.dot(p);
info->m_constraintError[s4] = k * u.dot(q);
#endif
// check angular limits
nrow = 4; // last filled row
int srow;
btScalar limit_err = btScalar(0.0);
int limit = 0;
if(getSolveLimit())
{
#ifdef _BT_USE_CENTER_LIMIT_
limit_err = m_limit.getCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
int powered = 0;
if(getEnableAngularMotor())
{
powered = 1;
}
if(limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
info->m_J1angularAxis[srow+0] = ax1[0];
info->m_J1angularAxis[srow+1] = ax1[1];
info->m_J1angularAxis[srow+2] = ax1[2];
info->m_J2angularAxis[srow+0] = -ax1[0];
info->m_J2angularAxis[srow+1] = -ax1[1];
info->m_J2angularAxis[srow+2] = -ax1[2];
btScalar lostop = getLowerLimit();
btScalar histop = getUpperLimit();
if(limit && (lostop == histop))
{ // the joint motor is ineffective
powered = 0;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
if(powered)
{
if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
{
info->cfm[srow] = m_normalCFM;
}
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
info->m_lowerLimit[srow] = - m_maxMotorImpulse;
info->m_upperLimit[srow] = m_maxMotorImpulse;
}
if(limit)
{
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
{
info->cfm[srow] = m_stopCFM;
}
if(lostop == histop)
{
// limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else if(limit == 1)
{ // low limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{ // high limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
#ifdef _BT_USE_CENTER_LIMIT_
btScalar bounce = m_limit.getRelaxationFactor();
#else
btScalar bounce = m_relaxationFactor;
#endif
if(bounce > btScalar(0.0))
{
btScalar vel = angVelA.dot(ax1);
vel -= angVelB.dot(ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if(limit == 1)
{ // low limit
if(vel < 0)
{
btScalar newc = -bounce * vel;
if(newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if(vel > 0)
{
btScalar newc = -bounce * vel;
if(newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
#ifdef _BT_USE_CENTER_LIMIT_
info->m_constraintError[srow] *= m_limit.getBiasFactor();
#else
info->m_constraintError[srow] *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void btHingeConstraint::setParam(int num, btScalar value, int axis)
{
if((axis == -1) || (axis == 5))
{
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
m_stopERP = value;
m_flags |= BT_HINGE_FLAGS_ERP_STOP;
break;
case BT_CONSTRAINT_STOP_CFM :
m_stopCFM = value;
m_flags |= BT_HINGE_FLAGS_CFM_STOP;
break;
case BT_CONSTRAINT_CFM :
m_normalCFM = value;
m_flags |= BT_HINGE_FLAGS_CFM_NORM;
break;
default :
btAssertConstrParams(0);
}
}
else
{
btAssertConstrParams(0);
}
}
///return the local value of parameter
btScalar btHingeConstraint::getParam(int num, int axis) const
{
btScalar retVal = 0;
if((axis == -1) || (axis == 5))
{
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
retVal = m_stopERP;
break;
case BT_CONSTRAINT_STOP_CFM :
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
retVal = m_stopCFM;
break;
case BT_CONSTRAINT_CFM :
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
retVal = m_normalCFM;
break;
default :
btAssertConstrParams(0);
}
}
else
{
btAssertConstrParams(0);
}
return retVal;
}

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

@ -15,8 +15,11 @@ subject to the following restrictions:
/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
#ifndef HINGECONSTRAINT_H
#define HINGECONSTRAINT_H
#ifndef BT_HINGECONSTRAINT_H
#define BT_HINGECONSTRAINT_H
#define _BT_USE_CENTER_LIMIT_ 1
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
@ -24,6 +27,24 @@ subject to the following restrictions:
class btRigidBody;
#ifdef BT_USE_DOUBLE_PRECISION
#define btHingeConstraintData btHingeConstraintDoubleData
#define btHingeConstraintDataName "btHingeConstraintDoubleData"
#else
#define btHingeConstraintData btHingeConstraintFloatData
#define btHingeConstraintDataName "btHingeConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
enum btHingeFlags
{
BT_HINGE_FLAGS_CFM_STOP = 1,
BT_HINGE_FLAGS_ERP_STOP = 2,
BT_HINGE_FLAGS_CFM_NORM = 4
};
/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/// axis defines the orientation of the hinge axis
ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
@ -40,42 +61,55 @@ public:
btScalar m_motorTargetVelocity;
btScalar m_maxMotorImpulse;
btScalar m_limitSoftness;
btScalar m_biasFactor;
btScalar m_relaxationFactor;
btScalar m_lowerLimit;
btScalar m_upperLimit;
btScalar m_kHinge;
#ifdef _BT_USE_CENTER_LIMIT_
btAngularLimit m_limit;
#else
btScalar m_lowerLimit;
btScalar m_upperLimit;
btScalar m_limitSign;
btScalar m_correction;
btScalar m_limitSoftness;
btScalar m_biasFactor;
btScalar m_relaxationFactor;
bool m_solveLimit;
#endif
btScalar m_kHinge;
btScalar m_accLimitImpulse;
btScalar m_hingeAngle;
btScalar m_referenceSign;
btScalar m_referenceSign;
bool m_angularOnly;
bool m_enableAngularMotor;
bool m_solveLimit;
bool m_useSolveConstraintObsolete;
bool m_useOffsetForConstraintFrame;
bool m_useReferenceFrameA;
btScalar m_accMotorImpulse;
int m_flags;
btScalar m_normalCFM;
btScalar m_stopCFM;
btScalar m_stopERP;
public:
btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false);
BT_DECLARE_ALIGNED_ALLOCATOR();
btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
btHingeConstraint();
virtual void buildJacobian();
@ -88,8 +122,8 @@ public:
void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
void getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
void updateRHS(btScalar timeStep);
@ -110,7 +144,19 @@ public:
btRigidBody& getRigidBodyB()
{
return m_rbB;
}
}
btTransform& getFrameOffsetA()
{
return m_rbAFrame;
}
btTransform& getFrameOffsetB()
{
return m_rbBFrame;
}
void setFrames(const btTransform& frameA, const btTransform& frameB);
void setAngularOnly(bool angularOnly)
{
@ -135,13 +181,15 @@ public:
void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
{
#ifdef _BT_USE_CENTER_LIMIT_
m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor);
#else
m_lowerLimit = btNormalizeAngle(low);
m_upperLimit = btNormalizeAngle(high);
m_limitSoftness = _softness;
m_biasFactor = _biasFactor;
m_relaxationFactor = _relaxationFactor;
#endif
}
void setAxis(btVector3& axisInA)
@ -160,21 +208,31 @@ public:
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA));
m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(pivotInA);
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis();
}
btScalar getLowerLimit() const
{
return m_lowerLimit;
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getLow();
#else
return m_lowerLimit;
#endif
}
btScalar getUpperLimit() const
{
return m_upperLimit;
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getHigh();
#else
return m_upperLimit;
#endif
}
@ -193,12 +251,20 @@ public:
inline int getSolveLimit()
{
return m_solveLimit;
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.isLimit();
#else
return m_solveLimit;
#endif
}
inline btScalar getLimitSign()
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getSign();
#else
return m_limitSign;
#endif
}
inline bool getAngularOnly()
@ -217,7 +283,101 @@ public:
{
return m_maxMotorImpulse;
}
// access for UseFrameOffset
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
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;
};
#endif //HINGECONSTRAINT_H
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btHingeConstraintDoubleData
{
btTypedConstraintData m_typeConstraintData;
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformDoubleData m_rbBFrame;
int m_useReferenceFrameA;
int m_angularOnly;
int m_enableAngularMotor;
float m_motorTargetVelocity;
float m_maxMotorImpulse;
float m_lowerLimit;
float m_upperLimit;
float m_limitSoftness;
float m_biasFactor;
float m_relaxationFactor;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btHingeConstraintFloatData
{
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformFloatData m_rbBFrame;
int m_useReferenceFrameA;
int m_angularOnly;
int m_enableAngularMotor;
float m_motorTargetVelocity;
float m_maxMotorImpulse;
float m_lowerLimit;
float m_upperLimit;
float m_limitSoftness;
float m_biasFactor;
float m_relaxationFactor;
};
SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const
{
return sizeof(btHingeConstraintData);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
m_rbAFrame.serialize(hingeData->m_rbAFrame);
m_rbBFrame.serialize(hingeData->m_rbBFrame);
hingeData->m_angularOnly = m_angularOnly;
hingeData->m_enableAngularMotor = m_enableAngularMotor;
hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
#ifdef _BT_USE_CENTER_LIMIT_
hingeData->m_lowerLimit = float(m_limit.getLow());
hingeData->m_upperLimit = float(m_limit.getHigh());
hingeData->m_limitSoftness = float(m_limit.getSoftness());
hingeData->m_biasFactor = float(m_limit.getBiasFactor());
hingeData->m_relaxationFactor = float(m_limit.getRelaxationFactor());
#else
hingeData->m_lowerLimit = float(m_lowerLimit);
hingeData->m_upperLimit = float(m_upperLimit);
hingeData->m_limitSoftness = float(m_limitSoftness);
hingeData->m_biasFactor = float(m_biasFactor);
hingeData->m_relaxationFactor = float(m_relaxationFactor);
#endif
return btHingeConstraintDataName;
}
#endif //BT_HINGECONSTRAINT_H

9
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
View file

@ -13,11 +13,10 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef JACOBIAN_ENTRY_H
#define JACOBIAN_ENTRY_H
#ifndef BT_JACOBIAN_ENTRY_H
#define BT_JACOBIAN_ENTRY_H
#include "LinearMath/btVector3.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btMatrix3x3.h"
//notes:
@ -153,4 +152,4 @@ public:
};
#endif //JACOBIAN_ENTRY_H
#endif //BT_JACOBIAN_ENTRY_H

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

@ -20,14 +20,11 @@ subject to the following restrictions:
btPoint2PointConstraint::btPoint2PointConstraint()
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
m_useSolveConstraintObsolete(false)
{
}
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
m_flags(0),
m_useSolveConstraintObsolete(false)
{
@ -36,6 +33,7 @@ m_useSolveConstraintObsolete(false)
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
m_flags(0),
m_useSolveConstraintObsolete(false)
{
@ -126,7 +124,7 @@ void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const
btVector3 a2 = body1_trans.getBasis()*getPivotInB();
{
btVector3 a2n = -a2;
// 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);
@ -136,14 +134,21 @@ void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const
// set right hand side
btScalar k = info->fps * info->erp;
btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
btScalar k = info->fps * currERP;
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
if(m_flags & BT_P2P_FLAGS_CFM)
{
for (j=0; j<3; j++)
{
info->cfm[j*info->rowskip] = m_cfm;
}
}
btScalar impulseClamp = m_setting.m_impulseClamp;//
for (j=0; j<3; j++)
@ -154,85 +159,11 @@ void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const
info->m_upperLimit[j*info->rowskip] = impulseClamp;
}
}
info->m_damping = m_setting.m_damping;
}
void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
if (m_useSolveConstraintObsolete)
{
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
btVector3 normal(0,0,0);
// btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
// btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
for (int i=0;i<3;i++)
{
normal[i] = 1;
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
btVector3 vel1,vel2;
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
btVector3 vel = vel1 - vel2;
btScalar rel_vel;
rel_vel = normal.dot(vel);
/*
//velocity error (first order error)
btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);
*/
//positional error (zeroth order error)
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
btScalar deltaImpulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
btScalar impulseClamp = m_setting.m_impulseClamp;
const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse;
if (sum < -impulseClamp)
{
deltaImpulse = -impulseClamp-m_appliedImpulse;
m_appliedImpulse = -impulseClamp;
}
else if (sum > impulseClamp)
{
deltaImpulse = impulseClamp-m_appliedImpulse;
m_appliedImpulse = impulseClamp;
}
else
{
m_appliedImpulse = sum;
}
btVector3 impulse_vector = normal * deltaImpulse;
btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse);
bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse);
normal[i] = 0;
}
}
}
void btPoint2PointConstraint::updateRHS(btScalar timeStep)
{
@ -240,3 +171,60 @@ void btPoint2PointConstraint::updateRHS(btScalar timeStep)
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
{
if(axis != -1)
{
btAssertConstrParams(0);
}
else
{
switch(num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
m_erp = value;
m_flags |= BT_P2P_FLAGS_ERP;
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
m_cfm = value;
m_flags |= BT_P2P_FLAGS_CFM;
break;
default:
btAssertConstrParams(0);
}
}
}
///return the local value of parameter
btScalar btPoint2PointConstraint::getParam(int num, int axis) const
{
btScalar retVal(SIMD_INFINITY);
if(axis != -1)
{
btAssertConstrParams(0);
}
else
{
switch(num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
retVal = m_erp;
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
retVal = m_cfm;
break;
default:
btAssertConstrParams(0);
}
}
return retVal;
}

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

@ -13,8 +13,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef POINT2POINTCONSTRAINT_H
#define POINT2POINTCONSTRAINT_H
#ifndef BT_POINT2POINTCONSTRAINT_H
#define BT_POINT2POINTCONSTRAINT_H
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
@ -22,6 +22,15 @@ subject to the following restrictions:
class btRigidBody;
#ifdef BT_USE_DOUBLE_PRECISION
#define btPoint2PointConstraintData btPoint2PointConstraintDoubleData
#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData"
#else
#define btPoint2PointConstraintData btPoint2PointConstraintFloatData
#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
struct btConstraintSetting
{
btConstraintSetting() :
@ -35,6 +44,12 @@ struct btConstraintSetting
btScalar m_impulseClamp;
};
enum btPoint2PointFlags
{
BT_P2P_FLAGS_ERP = 1,
BT_P2P_FLAGS_CFM = 2
};
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
{
@ -46,10 +61,14 @@ public:
btVector3 m_pivotInA;
btVector3 m_pivotInB;
int m_flags;
btScalar m_erp;
btScalar m_cfm;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
@ -59,7 +78,6 @@ public:
btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
btPoint2PointConstraint();
virtual void buildJacobian();
@ -71,8 +89,6 @@ public:
void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans);
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
void updateRHS(btScalar timeStep);
void setPivotA(const btVector3& pivotA)
@ -95,7 +111,53 @@ public:
return m_pivotInB;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
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;
};
#endif //POINT2POINTCONSTRAINT_H
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btPoint2PointConstraintFloatData
{
btTypedConstraintData m_typeConstraintData;
btVector3FloatData m_pivotInA;
btVector3FloatData m_pivotInB;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btPoint2PointConstraintDoubleData
{
btTypedConstraintData m_typeConstraintData;
btVector3DoubleData m_pivotInA;
btVector3DoubleData m_pivotInB;
};
SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const
{
return sizeof(btPoint2PointConstraintData);
}
///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;
btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
m_pivotInA.serialize(p2pData->m_pivotInA);
m_pivotInB.serialize(p2pData->m_pivotInB);
return btPoint2PointConstraintDataName;
}
#endif //BT_POINT2POINTCONSTRAINT_H

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

File diff suppressed because it is too large Load diff

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

@ -13,73 +13,109 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
#ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
#define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
#include "btConstraintSolver.h"
class btIDebugDraw;
#include "btContactConstraint.h"
#include "btSolverBody.h"
#include "btSolverConstraint.h"
#include "btTypedConstraint.h"
class btPersistentManifold;
class btStackAlloc;
class btDispatcher;
class btCollisionObject;
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
class btSequentialImpulseConstraintSolver : public btConstraintSolver
ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver
{
protected:
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
btConstraintArray m_tmpSolverContactConstraintPool;
btConstraintArray m_tmpSolverNonContactConstraintPool;
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
btAlignedObjectArray<int> m_orderTmpConstraintPool;
btAlignedObjectArray<int> m_orderNonContactConstraintPool;
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
int m_maxOverrideNumSolverIterations;
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,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
void setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 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);
void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
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);
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
unsigned long m_btSeed2;
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
void resolveSplitPenetrationSIMD(
btSolverBody& body1,
btSolverBody& body2,
btSolverBody& bodyA,btSolverBody& bodyB,
const btSolverConstraint& contactConstraint);
void resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& body1,
btSolverBody& body2,
btSolverBody& bodyA,btSolverBody& bodyB,
const btSolverConstraint& contactConstraint);
//internal method
int getOrInitSolverBody(btCollisionObject& body);
int getOrInitSolverBody(btCollisionObject& body);
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
void resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
protected:
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
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 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);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
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);
btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
///clear internal cached data and reset random seed
virtual void reset();
@ -98,10 +134,8 @@ public:
};
#ifndef BT_PREFER_SIMD
typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered;
#endif
#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H

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

File diff suppressed because it is too large Load diff

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

@ -22,8 +22,8 @@ TODO:
- add conversion for ODE constraint solver
*/
#ifndef SLIDER_CONSTRAINT_H
#define SLIDER_CONSTRAINT_H
#ifndef BT_SLIDER_CONSTRAINT_H
#define BT_SLIDER_CONSTRAINT_H
@ -40,14 +40,32 @@ class btRigidBody;
#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0))
#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0))
#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7))
#define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f))
enum btSliderFlags
{
BT_SLIDER_FLAGS_CFM_DIRLIN = (1 << 0),
BT_SLIDER_FLAGS_ERP_DIRLIN = (1 << 1),
BT_SLIDER_FLAGS_CFM_DIRANG = (1 << 2),
BT_SLIDER_FLAGS_ERP_DIRANG = (1 << 3),
BT_SLIDER_FLAGS_CFM_ORTLIN = (1 << 4),
BT_SLIDER_FLAGS_ERP_ORTLIN = (1 << 5),
BT_SLIDER_FLAGS_CFM_ORTANG = (1 << 6),
BT_SLIDER_FLAGS_ERP_ORTANG = (1 << 7),
BT_SLIDER_FLAGS_CFM_LIMLIN = (1 << 8),
BT_SLIDER_FLAGS_ERP_LIMLIN = (1 << 9),
BT_SLIDER_FLAGS_CFM_LIMANG = (1 << 10),
BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11)
};
class btSliderConstraint : public btTypedConstraint
ATTRIBUTE_ALIGNED16(class) btSliderConstraint : public btTypedConstraint
{
protected:
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
bool m_useOffsetForConstraintFrame;
btTransform m_frameInA;
btTransform m_frameInB;
// use frameA fo define limits, if true
@ -67,26 +85,39 @@ protected:
btScalar m_softnessDirLin;
btScalar m_restitutionDirLin;
btScalar m_dampingDirLin;
btScalar m_cfmDirLin;
btScalar m_softnessDirAng;
btScalar m_restitutionDirAng;
btScalar m_dampingDirAng;
btScalar m_cfmDirAng;
btScalar m_softnessLimLin;
btScalar m_restitutionLimLin;
btScalar m_dampingLimLin;
btScalar m_cfmLimLin;
btScalar m_softnessLimAng;
btScalar m_restitutionLimAng;
btScalar m_dampingLimAng;
btScalar m_cfmLimAng;
btScalar m_softnessOrthoLin;
btScalar m_restitutionOrthoLin;
btScalar m_dampingOrthoLin;
btScalar m_cfmOrthoLin;
btScalar m_softnessOrthoAng;
btScalar m_restitutionOrthoAng;
btScalar m_dampingOrthoAng;
btScalar m_cfmOrthoAng;
// for interlal use
bool m_solveLinLim;
bool m_solveAngLim;
int m_flags;
btJacobianEntry m_jacLin[3];
btScalar m_jacLinDiagABInv[3];
@ -124,12 +155,14 @@ protected:
//------------------------
void initParams();
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
// constructors
btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
btSliderConstraint();
btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
// overrides
virtual void buildJacobian();
virtual void getInfo1 (btConstraintInfo1* info);
void getInfo1NonVirtual(btConstraintInfo1* info);
@ -138,8 +171,6 @@ public:
void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
// access
const btRigidBody& getRigidBodyA() const { return m_rbA; }
@ -207,7 +238,10 @@ public:
btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
btScalar getLinearPos() { return m_linPos; }
btScalar getLinearPos() const { return m_linPos; }
btScalar getAngularPos() const { return m_angPos; }
// access for ODE solver
@ -215,20 +249,87 @@ public:
btScalar getLinDepth() { return m_depth[0]; }
bool getSolveAngLimit() { return m_solveAngLim; }
btScalar getAngDepth() { return m_angDepth; }
// internal
void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
// shared code used by ODE solver
void calculateTransforms(const btTransform& transA,const btTransform& transB);
void testLinLimits();
void testLinLimits2(btConstraintInfo2* info);
void testAngLimits();
// access for PE Solver
btVector3 getAncorInA();
btVector3 getAncorInB();
// access for UseFrameOffset
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
void setFrames(const btTransform& frameA, const btTransform& frameB)
{
m_frameInA=frameA;
m_frameInB=frameB;
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
buildJacobian();
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
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 btSliderConstraintData
{
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformFloatData m_rbBFrame;
float m_linearUpperLimit;
float m_linearLowerLimit;
float m_angularUpperLimit;
float m_angularLowerLimit;
int m_useLinearReferenceFrameA;
int m_useOffsetForConstraintFrame;
};
SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const
{
return sizeof(btSliderConstraintData);
}
#endif //SLIDER_CONSTRAINT_H
///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;
btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
m_frameInA.serializeFloat(sliderData->m_rbAFrame);
m_frameInB.serializeFloat(sliderData->m_rbBFrame);
sliderData->m_linearUpperLimit = float(m_upperLinLimit);
sliderData->m_linearLowerLimit = float(m_lowerLinLimit);
sliderData->m_angularUpperLimit = float(m_upperAngLimit);
sliderData->m_angularLowerLimit = float(m_lowerAngLimit);
sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA;
sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame;
return "btSliderConstraintData";
}
#endif //BT_SLIDER_CONSTRAINT_H

6
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
View file

@ -13,8 +13,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SOLVE_2LINEAR_CONSTRAINT_H
#define SOLVE_2LINEAR_CONSTRAINT_H
#ifndef BT_SOLVE_2LINEAR_CONSTRAINT_H
#define BT_SOLVE_2LINEAR_CONSTRAINT_H
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btVector3.h"
@ -104,4 +104,4 @@ public:
};
#endif //SOLVE_2LINEAR_CONSTRAINT_H
#endif //BT_SOLVE_2LINEAR_CONSTRAINT_H

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

@ -19,7 +19,7 @@ subject to the following restrictions:
class btRigidBody;
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btAlignedAllocator.h"
#include "LinearMath/btTransformUtil.h"
@ -105,23 +105,35 @@ 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_ALIGNED16 (struct) btSolverBody
ATTRIBUTE_ALIGNED64 (struct) btSolverBody
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btTransform m_worldTransform;
btVector3 m_deltaLinearVelocity;
btVector3 m_deltaAngularVelocity;
btVector3 m_angularFactor;
btVector3 m_linearFactor;
btVector3 m_invMass;
btScalar m_friction;
btRigidBody* m_originalBody;
btVector3 m_pushVelocity;
btVector3 m_turnVelocity;
btVector3 m_linearVelocity;
btVector3 m_angularVelocity;
btRigidBody* m_originalBody;
void setWorldTransform(const btTransform& worldTransform)
{
m_worldTransform = worldTransform;
}
const btTransform& getWorldTransform() const
{
return m_worldTransform;
}
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
{
if (m_originalBody)
velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
else
velocity.setValue(0,0,0);
}
@ -129,7 +141,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
{
if (m_originalBody)
angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
angVel =m_angularVelocity+m_deltaAngularVelocity;
else
angVel.setValue(0,0,0);
}
@ -138,9 +150,9 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
{
//if (m_invMass)
if (m_originalBody)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude;
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
}
@ -149,35 +161,125 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
{
if (m_originalBody)
{
m_pushVelocity += linearComponent*impulseMagnitude;
m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
}
const btVector3& getDeltaLinearVelocity() const
{
return m_deltaLinearVelocity;
}
const btVector3& getDeltaAngularVelocity() const
{
return m_deltaAngularVelocity;
}
const btVector3& getPushVelocity() const
{
return m_pushVelocity;
}
const btVector3& getTurnVelocity() const
{
return m_turnVelocity;
}
////////////////////////////////////////////////
///some internal methods, don't use them
btVector3& internalGetDeltaLinearVelocity()
{
return m_deltaLinearVelocity;
}
btVector3& internalGetDeltaAngularVelocity()
{
return m_deltaAngularVelocity;
}
const btVector3& internalGetAngularFactor() const
{
return m_angularFactor;
}
const btVector3& internalGetInvMass() const
{
return m_invMass;
}
void internalSetInvMass(const btVector3& invMass)
{
m_invMass = invMass;
}
btVector3& internalGetPushVelocity()
{
return m_pushVelocity;
}
btVector3& internalGetTurnVelocity()
{
return m_turnVelocity;
}
SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
{
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
}
SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const
{
angVel = m_angularVelocity+m_deltaAngularVelocity;
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
{
if (m_originalBody)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
}
void writebackVelocity()
{
if (m_originalBody)
{
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
m_linearVelocity +=m_deltaLinearVelocity;
m_angularVelocity += m_deltaAngularVelocity;
//m_originalBody->setCompanionId(-1);
}
}
void writebackVelocity(btScalar timeStep)
void writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
{
(void) timeStep;
if (m_originalBody)
{
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
m_linearVelocity += m_deltaLinearVelocity;
m_angularVelocity += m_deltaAngularVelocity;
//correct the position/orientation based on push/turn recovery
btTransform newTransform;
btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
m_originalBody->setWorldTransform(newTransform);
if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
{
// btQuaternion orn = m_worldTransform.getRotation();
btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
m_worldTransform = newTransform;
}
//m_worldTransform.setRotation(orn);
//m_originalBody->setCompanionId(-1);
}
}

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

@ -20,6 +20,7 @@ class btRigidBody;
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "btJacobianEntry.h"
#include "LinearMath/btAlignedObjectArray.h"
//#define NO_FRICTION_TANGENTIALS 1
#include "btSolverBody.h"
@ -41,45 +42,27 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
mutable btSimdScalar m_appliedPushImpulse;
mutable btSimdScalar m_appliedImpulse;
btScalar m_friction;
btScalar m_jacDiagABInv;
union
{
int m_numConsecutiveRowsPerKernel;
btScalar m_unusedPadding0;
};
union
{
int m_frictionIndex;
btScalar m_unusedPadding1;
};
union
{
int m_solverBodyIdA;
btScalar m_unusedPadding2;
};
union
{
int m_solverBodyIdB;
btScalar m_unusedPadding3;
};
btScalar m_rhs;
btScalar m_cfm;
union
btScalar m_lowerLimit;
btScalar m_upperLimit;
btScalar m_rhsPenetration;
union
{
void* m_originalContactPoint;
btScalar m_unusedPadding4;
};
btScalar m_rhs;
btScalar m_cfm;
btScalar m_lowerLimit;
btScalar m_upperLimit;
btScalar m_rhsPenetration;
int m_overrideNumSolverIterations;
int m_frictionIndex;
int m_solverBodyIdA;
int m_solverBodyIdB;
enum btSolverConstraintType
{
BT_SOLVER_CONTACT_1D = 0,

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

@ -16,34 +16,25 @@ subject to the following restrictions:
#include "btTypedConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btSerializer.h"
static btRigidBody s_fixed(0, 0,0);
#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
:btTypedObject(type),
m_userConstraintType(-1),
m_userConstraintId(-1),
m_needsFeedback(false),
m_rbA(s_fixed),
m_rbB(s_fixed),
m_appliedImpulse(btScalar(0.)),
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
{
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
}
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
:btTypedObject(type),
m_userConstraintType(-1),
m_userConstraintId(-1),
m_breakingImpulseThreshold(SIMD_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),
m_overrideNumSolverIterations(-1),
m_rbA(rbA),
m_rbB(s_fixed),
m_rbB(getFixedBody()),
m_appliedImpulse(btScalar(0.)),
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
m_jointFeedback(0)
{
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
}
@ -51,14 +42,16 @@ btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rb
:btTypedObject(type),
m_userConstraintType(-1),
m_userConstraintId(-1),
m_breakingImpulseThreshold(SIMD_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),
m_overrideNumSolverIterations(-1),
m_rbA(rbA),
m_rbB(rbB),
m_appliedImpulse(btScalar(0.)),
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
m_jointFeedback(0)
{
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
}
@ -113,4 +106,117 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
return lim_fact;
}
///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;
tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
char* name = (char*) serializer->findNameForPointer(this);
tcd->m_name = (char*)serializer->getUniquePointer(name);
if (tcd->m_name)
{
serializer->serializeName(name);
}
tcd->m_objectType = m_objectType;
tcd->m_needsFeedback = m_needsFeedback;
tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations;
tcd->m_breakingImpulseThreshold = float(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_disableCollisionsBetweenLinkedBodies = false;
int i;
for (i=0;i<m_rbA.getNumConstraintRefs();i++)
if (m_rbA.getConstraintRef(i) == this)
tcd->m_disableCollisionsBetweenLinkedBodies = true;
for (i=0;i<m_rbB.getNumConstraintRefs();i++)
if (m_rbB.getConstraintRef(i) == this)
tcd->m_disableCollisionsBetweenLinkedBodies = true;
return "btTypedConstraintData";
}
btRigidBody& btTypedConstraint::getFixedBody()
{
static btRigidBody s_fixed(0, 0,0);
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
return s_fixed;
}
void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
{
m_halfRange = (high - low) / 2.0f;
m_center = btNormalizeAngle(low + m_halfRange);
m_softness = _softness;
m_biasFactor = _biasFactor;
m_relaxationFactor = _relaxationFactor;
}
void btAngularLimit::test(const btScalar angle)
{
m_correction = 0.0f;
m_sign = 0.0f;
m_solveLimit = false;
if (m_halfRange >= 0.0f)
{
btScalar deviation = btNormalizeAngle(angle - m_center);
if (deviation < -m_halfRange)
{
m_solveLimit = true;
m_correction = - (deviation + m_halfRange);
m_sign = +1.0f;
}
else if (deviation > m_halfRange)
{
m_solveLimit = true;
m_correction = m_halfRange - deviation;
m_sign = -1.0f;
}
}
}
btScalar btAngularLimit::getError() const
{
return m_correction * m_sign;
}
void btAngularLimit::fit(btScalar& angle) const
{
if (m_halfRange > 0.0f)
{
btScalar relativeAngle = btNormalizeAngle(angle - m_center);
if (!btEqual(relativeAngle, m_halfRange))
{
if (relativeAngle > 0.0f)
{
angle = getHigh();
}
else
{
angle = getLow();
}
}
}
}
btScalar btAngularLimit::getLow() const
{
return btNormalizeAngle(m_center - m_halfRange);
}
btScalar btAngularLimit::getHigh() const
{
return btNormalizeAngle(m_center + m_halfRange);
}

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

@ -1,6 +1,6 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
Copyright (c) 2003-2010 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
@ -13,31 +13,71 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef TYPED_CONSTRAINT_H
#define TYPED_CONSTRAINT_H
#ifndef BT_TYPED_CONSTRAINT_H
#define BT_TYPED_CONSTRAINT_H
class btRigidBody;
#include "LinearMath/btScalar.h"
#include "btSolverConstraint.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
struct btSolverBody;
#include "BulletDynamics/Dynamics/btRigidBody.h"
class btSerializer;
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
enum btTypedConstraintType
{
POINT2POINT_CONSTRAINT_TYPE=MAX_CONTACT_MANIFOLD_TYPE+1,
POINT2POINT_CONSTRAINT_TYPE=3,
HINGE_CONSTRAINT_TYPE,
CONETWIST_CONSTRAINT_TYPE,
D6_CONSTRAINT_TYPE,
SLIDER_CONSTRAINT_TYPE,
CONTACT_CONSTRAINT_TYPE
CONTACT_CONSTRAINT_TYPE,
D6_SPRING_CONSTRAINT_TYPE,
GEAR_CONSTRAINT_TYPE,
MAX_CONSTRAINT_TYPE
};
enum btConstraintParams
{
BT_CONSTRAINT_ERP=1,
BT_CONSTRAINT_STOP_ERP,
BT_CONSTRAINT_CFM,
BT_CONSTRAINT_STOP_CFM
};
#if 1
#define btAssertConstrParams(_par) btAssert(_par)
#else
#define btAssertConstrParams(_par)
#endif
ATTRIBUTE_ALIGNED16(struct) btJointFeedback
{
btVector3 m_appliedForceBodyA;
btVector3 m_appliedTorqueBodyA;
btVector3 m_appliedForceBodyB;
btVector3 m_appliedTorqueBodyB;
};
///TypedConstraint is the baseclass for Bullet constraints and vehicles
class btTypedConstraint : public btTypedObject
ATTRIBUTE_ALIGNED16(class) btTypedConstraint : public btTypedObject
{
int m_userConstraintType;
int m_userConstraintId;
bool m_needsFeedback;
union
{
int m_userConstraintId;
void* m_userConstraintPtr;
};
btScalar m_breakingImpulseThreshold;
bool m_isEnabled;
bool m_needsFeedback;
int m_overrideNumSolverIterations;
btTypedConstraint& operator=(btTypedConstraint& other)
{
@ -51,14 +91,16 @@ protected:
btRigidBody& m_rbB;
btScalar m_appliedImpulse;
btScalar m_dbgDrawSize;
btJointFeedback* m_jointFeedback;
btVector3 m_appliedLinearImpulse;
btVector3 m_appliedAngularImpulseA;
btVector3 m_appliedAngularImpulseB;
///internal method used by the constraint solver, don't use them directly
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
public:
btTypedConstraint(btTypedConstraintType type);
BT_DECLARE_ALIGNED_ALLOCATOR();
virtual ~btTypedConstraint() {};
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
@ -67,6 +109,8 @@ public:
int m_numConstraintRows,nub;
};
static btRigidBody& getFixedBody();
struct btConstraintInfo2 {
// integrator parameters: frames per second (1/stepsize), default error
// reduction parameter (0..1).
@ -96,14 +140,33 @@ public:
int *findex;
// number of solver iterations
int m_numIterations;
//damping of the velocity
btScalar m_damping;
};
int getOverrideNumSolverIterations() const
{
return m_overrideNumSolverIterations;
}
///override the number of constraint solver iterations used to solve this constraint
///-1 will use the default number of iterations, as specified in SolverInfo.m_numIterations
void setOverrideNumSolverIterations(int overideNumIterations)
{
m_overrideNumSolverIterations = overideNumIterations;
}
///internal method used by the constraint solver, don't use them directly
virtual void buildJacobian() = 0;
virtual void buildJacobian() {};
///internal method used by the constraint solver, don't use them directly
virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
{
(void)ca;
(void)solverBodyA;
(void)solverBodyB;
(void)timeStep;
}
///internal method used by the constraint solver, don't use them directly
@ -117,12 +180,37 @@ public:
{
m_appliedImpulse = appliedImpulse;
}
///internal method used by the constraint solver, don't use them directly
btScalar internalGetAppliedImpulse()
{
return m_appliedImpulse;
}
btScalar getBreakingImpulseThreshold() const
{
return m_breakingImpulseThreshold;
}
void setBreakingImpulseThreshold(btScalar threshold)
{
m_breakingImpulseThreshold = threshold;
}
bool isEnabled() const
{
return m_isEnabled;
}
void setEnabled(bool enabled)
{
m_isEnabled=enabled;
}
///internal method used by the constraint solver, don't use them directly
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) = 0;
virtual void solveConstraintObsolete(btSolverBody& /*bodyA*/,btSolverBody& /*bodyB*/,btScalar /*timeStep*/) {};
///internal method used by the constraint solver, don't use them directly
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
const btRigidBody& getRigidBodyA() const
{
@ -162,6 +250,32 @@ public:
return m_userConstraintId;
}
void setUserConstraintPtr(void* ptr)
{
m_userConstraintPtr = ptr;
}
void* getUserConstraintPtr()
{
return m_userConstraintPtr;
}
void setJointFeedback(btJointFeedback* jointFeedback)
{
m_jointFeedback = jointFeedback;
}
const btJointFeedback* getJointFeedback() const
{
return m_jointFeedback;
}
btJointFeedback* getJointFeedback()
{
return m_jointFeedback;
}
int getUid() const
{
return m_userConstraintId;
@ -187,44 +301,6 @@ public:
return m_appliedImpulse;
}
const btVector3& getAppliedLinearImpulse() const
{
btAssert(m_needsFeedback);
return m_appliedLinearImpulse;
}
btVector3& getAppliedLinearImpulse()
{
btAssert(m_needsFeedback);
return m_appliedLinearImpulse;
}
const btVector3& getAppliedAngularImpulseA() const
{
btAssert(m_needsFeedback);
return m_appliedAngularImpulseA;
}
btVector3& getAppliedAngularImpulseA()
{
btAssert(m_needsFeedback);
return m_appliedAngularImpulseA;
}
const btVector3& getAppliedAngularImpulseB() const
{
btAssert(m_needsFeedback);
return m_appliedAngularImpulseB;
}
btVector3& getAppliedAngularImpulseB()
{
btAssert(m_needsFeedback);
return m_appliedAngularImpulseB;
}
btTypedConstraintType getConstraintType () const
{
return btTypedConstraintType(m_objectType);
@ -238,7 +314,19 @@ public:
{
return m_dbgDrawSize;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1) = 0;
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const = 0;
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;
};
// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
@ -251,13 +339,13 @@ SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScal
}
else if(angleInRadians < angleLowerLimitInRadians)
{
btScalar diffLo = btNormalizeAngle(angleLowerLimitInRadians - angleInRadians); // this is positive
btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
}
else if(angleInRadians > angleUpperLimitInRadians)
{
btScalar diffHi = btNormalizeAngle(angleInRadians - angleUpperLimitInRadians); // this is positive
btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
}
@ -267,5 +355,128 @@ SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScal
}
}
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btTypedConstraintData
{
btRigidBodyData *m_rbA;
btRigidBodyData *m_rbB;
char *m_name;
#endif //TYPED_CONSTRAINT_H
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;
};
SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
{
return sizeof(btTypedConstraintData);
}
class btAngularLimit
{
private:
btScalar
m_center,
m_halfRange,
m_softness,
m_biasFactor,
m_relaxationFactor,
m_correction,
m_sign;
bool
m_solveLimit;
public:
/// Default constructor initializes limit as inactive, allowing free constraint movement
btAngularLimit()
:m_center(0.0f),
m_halfRange(-1.0f),
m_softness(0.9f),
m_biasFactor(0.3f),
m_relaxationFactor(1.0f),
m_correction(0.0f),
m_sign(0.0f),
m_solveLimit(false)
{}
/// Sets all limit's parameters.
/// When low > high limit becomes inactive.
/// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit
void set(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f);
/// Checks conastaint angle against limit. If limit is active and the angle violates the limit
/// correction is calculated.
void test(const btScalar angle);
/// Returns limit's softness
inline btScalar getSoftness() const
{
return m_softness;
}
/// Returns limit's bias factor
inline btScalar getBiasFactor() const
{
return m_biasFactor;
}
/// Returns limit's relaxation factor
inline btScalar getRelaxationFactor() const
{
return m_relaxationFactor;
}
/// Returns correction value evaluated when test() was invoked
inline btScalar getCorrection() const
{
return m_correction;
}
/// Returns sign value evaluated when test() was invoked
inline btScalar getSign() const
{
return m_sign;
}
/// Gives half of the distance between min and max limit angle
inline btScalar getHalfRange() const
{
return m_halfRange;
}
/// Returns true when the last test() invocation recognized limit violation
inline bool isLimit() const
{
return m_solveLimit;
}
/// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle
/// returned is modified so it equals to the limit closest to given angle.
void fit(btScalar& angle) const;
/// Returns correction value multiplied by sign value
btScalar getError() const;
btScalar getLow() const;
btScalar getHigh() const;
};
#endif //BT_TYPED_CONSTRAINT_H

30
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
View file

@ -27,7 +27,7 @@ subject to the following restrictions:
// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2)
: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
m_anchor(anchor),
m_axis1(axis1),
@ -42,8 +42,8 @@ btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB,
// new position of X, allowed limits are (-PI,PI);
// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
// Build the frame in world coordinate system first
btVector3 zAxis = axis1.normalize();
btVector3 yAxis = axis2.normalize();
btVector3 zAxis = m_axis1.normalize();
btVector3 yAxis = m_axis2.normalize();
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
btTransform frameInW;
frameInW.setIdentity();
@ -61,3 +61,27 @@ btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB,
setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS));
}
void btUniversalConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
{
m_axis1 = axis1;
m_axis2 = axis2;
btVector3 zAxis = axis1.normalized();
btVector3 yAxis = axis2.normalized();
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
btTransform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
frameInW.setOrigin(m_anchor);
// now get constraint frame in local coordinate systems
m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
calculateTransforms();
}

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

@ -13,8 +13,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef UNIVERSAL_CONSTRAINT_H
#define UNIVERSAL_CONSTRAINT_H
#ifndef BT_UNIVERSAL_CONSTRAINT_H
#define BT_UNIVERSAL_CONSTRAINT_H
@ -31,17 +31,20 @@ subject to the following restrictions:
/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular.
/// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal."
class btUniversalConstraint : public btGeneric6DofConstraint
ATTRIBUTE_ALIGNED16(class) btUniversalConstraint : public btGeneric6DofConstraint
{
protected:
btVector3 m_anchor;
btVector3 m_axis1;
btVector3 m_axis2;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2);
btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2);
// access
const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
@ -52,9 +55,11 @@ public:
// limits
void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); }
void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); }
void setAxis( const btVector3& axis1, const btVector3& axis2);
};
#endif // UNIVERSAL_CONSTRAINT_H
#endif // BT_UNIVERSAL_CONSTRAINT_H

7
Engine/lib/bullet/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
View file

@ -43,7 +43,7 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
#include "LinearMath/btStackAlloc.h"
/*
Create and Delete a Physics SDK
@ -373,10 +373,7 @@ double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
btStackAlloc gStackAlloc(1024*1024*2);
input.m_stackAlloc = &gStackAlloc;
btTransform tr;
tr.setIdentity();

8
Engine/lib/bullet/src/BulletDynamics/Dynamics/btActionInterface.h
View file

@ -20,11 +20,17 @@ class btIDebugDraw;
class btCollisionWorld;
#include "LinearMath/btScalar.h"
#include "btRigidBody.h"
///Basic interface to allow actions such as vehicles and characters to be updated inside a btDynamicsWorld
class btActionInterface
{
public:
protected:
static btRigidBody& getFixedBody();
public:
virtual ~btActionInterface()
{

196
Engine/lib/bullet/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
View file

@ -1,196 +0,0 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btContinuousDynamicsWorld.h"
#include "LinearMath/btQuickprof.h"
//collision detection
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
//rigidbody & constraints
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#include <stdio.h>
btContinuousDynamicsWorld::btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
{
}
btContinuousDynamicsWorld::~btContinuousDynamicsWorld()
{
}
void btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
{
startProfiling(timeStep);
if(0 != m_internalPreTickCallback) {
(*m_internalPreTickCallback)(this, timeStep);
}
///update aabbs information
updateAabbs();
//static int frame=0;
// printf("frame %d\n",frame++);
///apply gravity, predict motion
predictUnconstraintMotion(timeStep);
btDispatcherInfo& dispatchInfo = getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = getDebugDrawer();
///perform collision detection
performDiscreteCollisionDetection();
calculateSimulationIslands();
getSolverInfo().m_timeStep = timeStep;
///solve contact and other joint constraints
solveConstraints(getSolverInfo());
///CallbackTriggers();
calculateTimeOfImpacts(timeStep);
btScalar toi = dispatchInfo.m_timeOfImpact;
// if (toi < 1.f)
// printf("toi = %f\n",toi);
if (toi < 0.f)
printf("toi = %f\n",toi);
///integrate transforms
integrateTransforms(timeStep * toi);
///update vehicle simulation
updateActions(timeStep);
updateActivationState( timeStep );
if(0 != m_internalTickCallback) {
(*m_internalTickCallback)(this, timeStep);
}
}
void btContinuousDynamicsWorld::calculateTimeOfImpacts(btScalar timeStep)
{
///these should be 'temporal' aabbs!
updateTemporalAabbs(timeStep);
///'toi' is the global smallest time of impact. However, we just calculate the time of impact for each object individually.
///so we handle the case moving versus static properly, and we cheat for moving versus moving
btScalar toi = 1.f;
btDispatcherInfo& dispatchInfo = getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_timeOfImpact = 1.f;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS;
///calculate time of impact for overlapping pairs
btDispatcher* dispatcher = getDispatcher();
if (dispatcher)
dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
toi = dispatchInfo.m_timeOfImpact;
dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_DISCRETE;
}
void btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep)
{
btVector3 temporalAabbMin,temporalAabbMax;
for ( int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
if (body)
{
body->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),temporalAabbMin,temporalAabbMax);
const btVector3& linvel = body->getLinearVelocity();
//make the AABB temporal
btScalar temporalAabbMaxx = temporalAabbMax.getX();
btScalar temporalAabbMaxy = temporalAabbMax.getY();
btScalar temporalAabbMaxz = temporalAabbMax.getZ();
btScalar temporalAabbMinx = temporalAabbMin.getX();
btScalar temporalAabbMiny = temporalAabbMin.getY();
btScalar temporalAabbMinz = temporalAabbMin.getZ();
// add linear motion
btVector3 linMotion = linvel*timeStep;
if (linMotion.x() > 0.f)
temporalAabbMaxx += linMotion.x();
else
temporalAabbMinx += linMotion.x();
if (linMotion.y() > 0.f)
temporalAabbMaxy += linMotion.y();
else
temporalAabbMiny += linMotion.y();
if (linMotion.z() > 0.f)
temporalAabbMaxz += linMotion.z();
else
temporalAabbMinz += linMotion.z();
//add conservative angular motion
btScalar angularMotion(0);// = angvel.length() * GetAngularMotionDisc() * timeStep;
btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
temporalAabbMin -= angularMotion3d;
temporalAabbMax += angularMotion3d;
m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax,m_dispatcher1);
}
}
//update aabb (of all moved objects)
m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
}

46
Engine/lib/bullet/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h
View file

@ -1,46 +0,0 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_CONTINUOUS_DYNAMICS_WORLD_H
#define BT_CONTINUOUS_DYNAMICS_WORLD_H
#include "btDiscreteDynamicsWorld.h"
///btContinuousDynamicsWorld adds optional (per object) continuous collision detection for fast moving objects to the btDiscreteDynamicsWorld.
///This copes with fast moving objects that otherwise would tunnel/miss collisions.
///Under construction, don't use yet! Please use btDiscreteDynamicsWorld instead.
class btContinuousDynamicsWorld : public btDiscreteDynamicsWorld
{
void updateTemporalAabbs(btScalar timeStep);
public:
btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
virtual ~btContinuousDynamicsWorld();
///time stepping with calculation of time of impact for selected fast moving objects
virtual void internalSingleStepSimulation( btScalar timeStep);
virtual void calculateTimeOfImpacts(btScalar timeStep);
virtual btDynamicsWorldType getWorldType() const
{
return BT_CONTINUOUS_DYNAMICS_WORLD;
}
};
#endif //BT_CONTINUOUS_DYNAMICS_WORLD_H

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

File diff suppressed because it is too large Load diff

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

@ -25,16 +25,21 @@ class btConstraintSolver;
class btSimulationIslandManager;
class btTypedConstraint;
class btActionInterface;
class btPersistentManifold;
class btIDebugDraw;
struct InplaceSolverIslandCallback;
#include "LinearMath/btAlignedObjectArray.h"
///btDiscreteDynamicsWorld provides discrete rigid body simulation
///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
class btDiscreteDynamicsWorld : public btDynamicsWorld
ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorld : public btDynamicsWorld
{
protected:
btAlignedObjectArray<btTypedConstraint*> m_sortedConstraints;
InplaceSolverIslandCallback* m_solverIslandCallback;
btConstraintSolver* m_constraintSolver;
@ -53,11 +58,14 @@ protected:
bool m_ownsIslandManager;
bool m_ownsConstraintSolver;
bool m_synchronizeAllMotionStates;
bool m_applySpeculativeContactRestitution;
btAlignedObjectArray<btActionInterface*> m_actions;
int m_profileTimings;
btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds;
virtual void predictUnconstraintMotion(btScalar timeStep);
virtual void integrateTransforms(btScalar timeStep);
@ -74,15 +82,19 @@ protected:
virtual void internalSingleStepSimulation( btScalar timeStep);
void createPredictiveContacts(btScalar timeStep);
virtual void saveKinematicState(btScalar timeStep);
void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
void serializeRigidBodies(btSerializer* serializer);
void serializeDynamicsWorldInfo(btSerializer* serializer);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
@ -135,7 +147,6 @@ public:
///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
virtual void removeCollisionObject(btCollisionObject* collisionObject);
void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
void debugDrawConstraint(btTypedConstraint* constraint);
@ -192,6 +203,19 @@ public:
return m_synchronizeAllMotionStates;
}
void setApplySpeculativeContactRestitution(bool enable)
{
m_applySpeculativeContactRestitution = enable;
}
bool getApplySpeculativeContactRestitution() const
{
return m_applySpeculativeContactRestitution;
}
///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo)
virtual void serialize(btSerializer* serializer);
};
#endif //BT_DISCRETE_DYNAMICS_WORLD_H

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

@ -32,7 +32,8 @@ enum btDynamicsWorldType
{
BT_SIMPLE_DYNAMICS_WORLD=1,
BT_DISCRETE_DYNAMICS_WORLD=2,
BT_CONTINUOUS_DYNAMICS_WORLD=3
BT_CONTINUOUS_DYNAMICS_WORLD=3,
BT_SOFT_RIGID_DYNAMICS_WORLD=4
};
///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
@ -86,6 +87,8 @@ public:
virtual void addRigidBody(btRigidBody* body) = 0;
virtual void addRigidBody(btRigidBody* body, short group, short mask) = 0;
virtual void removeRigidBody(btRigidBody* body) = 0;
virtual void setConstraintSolver(btConstraintSolver* solver) = 0;
@ -143,6 +146,21 @@ public:
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btDynamicsWorldDoubleData
{
btContactSolverInfoDoubleData m_solverInfo;
btVector3DoubleData m_gravity;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btDynamicsWorldFloatData
{
btContactSolverInfoFloatData m_solverInfo;
btVector3FloatData m_gravity;
};
#endif //BT_DYNAMICS_WORLD_H

107
Engine/lib/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp
View file

@ -19,6 +19,7 @@ subject to the following restrictions:
#include "LinearMath/btTransformUtil.h"
#include "LinearMath/btMotionState.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#include "LinearMath/btSerializer.h"
//'temporarily' global variables
btScalar gDeactivationTime = btScalar(2.);
@ -50,8 +51,8 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
m_linearDamping = btScalar(0.);
m_angularDamping = btScalar(0.5);
setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
m_optionalMotionState = constructionInfo.m_motionState;
@ -77,15 +78,26 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
//moved to btCollisionObject
m_friction = constructionInfo.m_friction;
m_rollingFriction = constructionInfo.m_rollingFriction;
m_restitution = constructionInfo.m_restitution;
setCollisionShape( constructionInfo.m_collisionShape );
m_debugBodyId = uniqueId++;
setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
updateInertiaTensor();
m_rigidbodyFlags = 0;
m_deltaLinearVelocity.setZero();
m_deltaAngularVelocity.setZero();
m_invMass = m_inverseMass*m_linearFactor;
m_pushVelocity.setZero();
m_turnVelocity.setZero();
}
@ -136,8 +148,8 @@ void btRigidBody::setGravity(const btVector3& acceleration)
void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
{
m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
}
@ -227,21 +239,41 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
m_inverseMass = btScalar(1.0) / mass;
}
//Fg = m * a
m_gravity = mass * m_gravity_acceleration;
m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
m_invMass = m_linearFactor*m_inverseMass;
}
void btRigidBody::updateInertiaTensor()
{
m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
}
btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
{
btVector3 inertiaLocal;
inertiaLocal[0] = 1.f/getInvInertiaDiagLocal()[0];
inertiaLocal[1] = 1.f/getInvInertiaDiagLocal()[1];
inertiaLocal[2] = 1.f/getInvInertiaDiagLocal()[2];
btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
btVector3 tmp = inertiaTensorWorld*getAngularVelocity();
btVector3 gf = getAngularVelocity().cross(tmp);
btScalar l2 = gf.length2();
if (l2>maxGyroscopicForce*maxGyroscopicForce)
{
gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce;
}
return gf;
}
void btRigidBody::integrateVelocities(btScalar step)
{
if (isStaticOrKinematicObject())
@ -271,7 +303,7 @@ btQuaternion btRigidBody::getOrientation() const
void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
{
if (isStaticOrKinematicObject())
if (isKinematicObject())
{
m_interpolationWorldTransform = m_worldTransform;
} else
@ -285,22 +317,25 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
}
bool btRigidBody::checkCollideWithOverride(btCollisionObject* co)
bool btRigidBody::checkCollideWithOverride(const btCollisionObject* co) const
{
btRigidBody* otherRb = btRigidBody::upcast(co);
const btRigidBody* otherRb = btRigidBody::upcast(co);
if (!otherRb)
return true;
for (int i = 0; i < m_constraintRefs.size(); ++i)
{
btTypedConstraint* c = m_constraintRefs[i];
if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
return false;
const btTypedConstraint* c = m_constraintRefs[i];
if (c->isEnabled())
if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
return false;
}
return true;
}
void btRigidBody::addConstraintRef(btTypedConstraint* c)
{
int index = m_constraintRefs.findLinearSearch(c);
@ -315,3 +350,51 @@ void btRigidBody::removeConstraintRef(btTypedConstraint* c)
m_constraintRefs.remove(c);
m_checkCollideWith = m_constraintRefs.size() > 0;
}
int btRigidBody::calculateSerializeBufferSize() const
{
int sz = sizeof(btRigidBodyData);
return sz;
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
{
btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer;
btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld);
m_linearVelocity.serialize(rbd->m_linearVelocity);
m_angularVelocity.serialize(rbd->m_angularVelocity);
rbd->m_inverseMass = m_inverseMass;
m_angularFactor.serialize(rbd->m_angularFactor);
m_linearFactor.serialize(rbd->m_linearFactor);
m_gravity.serialize(rbd->m_gravity);
m_gravity_acceleration.serialize(rbd->m_gravity_acceleration);
m_invInertiaLocal.serialize(rbd->m_invInertiaLocal);
m_totalForce.serialize(rbd->m_totalForce);
m_totalTorque.serialize(rbd->m_totalTorque);
rbd->m_linearDamping = m_linearDamping;
rbd->m_angularDamping = m_angularDamping;
rbd->m_additionalDamping = m_additionalDamping;
rbd->m_additionalDampingFactor = m_additionalDampingFactor;
rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
return btRigidBodyDataName;
}
void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
{
btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1);
const char* structType = serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this);
}

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

@ -13,8 +13,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef RIGIDBODY_H
#define RIGIDBODY_H
#ifndef BT_RIGIDBODY_H
#define BT_RIGIDBODY_H
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btTransform.h"
@ -29,6 +29,24 @@ class btTypedConstraint;
extern btScalar gDeactivationTime;
extern bool gDisableDeactivation;
#ifdef BT_USE_DOUBLE_PRECISION
#define btRigidBodyData btRigidBodyDoubleData
#define btRigidBodyDataName "btRigidBodyDoubleData"
#else
#define btRigidBodyData btRigidBodyFloatData
#define btRigidBodyDataName "btRigidBodyFloatData"
#endif //BT_USE_DOUBLE_PRECISION
enum btRigidBodyFlags
{
BT_DISABLE_WORLD_GRAVITY = 1,
///The BT_ENABLE_GYROPSCOPIC_FORCE can easily introduce instability
///So generally it is best to not enable it.
///If really needed, run at a high frequency like 1000 Hertz: ///See Demos/GyroscopicDemo for an example use
BT_ENABLE_GYROPSCOPIC_FORCE = 2
};
///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
///It is recommended for performance and memory use to share btCollisionShape objects whenever possible.
@ -45,7 +63,6 @@ class btRigidBody : public btCollisionObject
btVector3 m_linearVelocity;
btVector3 m_angularVelocity;
btScalar m_inverseMass;
btVector3 m_angularFactor;
btVector3 m_linearFactor;
btVector3 m_gravity;
@ -73,6 +90,21 @@ class btRigidBody : public btCollisionObject
//keep track of typed constraints referencing this rigid body
btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
int m_rigidbodyFlags;
int m_debugBodyId;
protected:
ATTRIBUTE_ALIGNED64(btVector3 m_deltaLinearVelocity);
btVector3 m_deltaAngularVelocity;
btVector3 m_angularFactor;
btVector3 m_invMass;
btVector3 m_pushVelocity;
btVector3 m_turnVelocity;
public:
@ -97,6 +129,9 @@ public:
///best simulation results when friction is non-zero
btScalar m_friction;
///the m_rollingFriction prevents rounded shapes, such as spheres, cylinders and capsules from rolling forever.
///See Bullet/Demos/RollingFrictionDemo for usage
btScalar m_rollingFriction;
///best simulation results using zero restitution.
btScalar m_restitution;
@ -111,7 +146,6 @@ public:
btScalar m_additionalAngularDampingThresholdSqr;
btScalar m_additionalAngularDampingFactor;
btRigidBodyConstructionInfo( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
m_mass(mass),
m_motionState(motionState),
@ -120,6 +154,7 @@ public:
m_linearDamping(btScalar(0.)),
m_angularDamping(btScalar(0.)),
m_friction(btScalar(0.5)),
m_rollingFriction(btScalar(0)),
m_restitution(btScalar(0.)),
m_linearSleepingThreshold(btScalar(0.8)),
m_angularSleepingThreshold(btScalar(1.f)),
@ -161,13 +196,13 @@ public:
///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
static const btRigidBody* upcast(const btCollisionObject* colObj)
{
if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
return (const btRigidBody*)colObj;
return 0;
}
static btRigidBody* upcast(btCollisionObject* colObj)
{
if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
return (btRigidBody*)colObj;
return 0;
}
@ -227,6 +262,7 @@ public:
void setLinearFactor(const btVector3& linearFactor)
{
m_linearFactor = linearFactor;
m_invMass = m_linearFactor*m_inverseMass;
}
btScalar getInvMass() const { return m_inverseMass; }
const btMatrix3x3& getInvInertiaTensorWorld() const {
@ -242,12 +278,12 @@ public:
m_totalForce += force*m_linearFactor;
}
const btVector3& getTotalForce()
const btVector3& getTotalForce() const
{
return m_totalForce;
};
const btVector3& getTotalTorque()
const btVector3& getTotalTorque() const
{
return m_totalTorque;
};
@ -301,19 +337,6 @@ public:
}
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
{
if (m_inverseMass != btScalar(0.))
{
m_linearVelocity += linearComponent*m_linearFactor*impulseMagnitude;
if (m_angularFactor)
{
m_angularVelocity += angularComponent*m_angularFactor*impulseMagnitude;
}
}
}
void clearForces()
{
m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
@ -479,7 +502,7 @@ public:
return (getBroadphaseProxy() != 0);
}
virtual bool checkCollideWithOverride(btCollisionObject* co);
virtual bool checkCollideWithOverride(const btCollisionObject* co) const;
void addConstraintRef(btTypedConstraint* c);
void removeConstraintRef(btTypedConstraint* c);
@ -489,15 +512,89 @@ public:
return m_constraintRefs[index];
}
int getNumConstraintRefs()
int getNumConstraintRefs() const
{
return m_constraintRefs.size();
}
int m_debugBodyId;
void setFlags(int flags)
{
m_rigidbodyFlags = flags;
}
int getFlags() const
{
return m_rigidbodyFlags;
}
btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;
///////////////////////////////////////////////
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
virtual void serializeSingleObject(class btSerializer* serializer) const;
};
//@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btRigidBodyFloatData
{
btCollisionObjectFloatData m_collisionObjectData;
btMatrix3x3FloatData m_invInertiaTensorWorld;
btVector3FloatData m_linearVelocity;
btVector3FloatData m_angularVelocity;
btVector3FloatData m_angularFactor;
btVector3FloatData m_linearFactor;
btVector3FloatData m_gravity;
btVector3FloatData m_gravity_acceleration;
btVector3FloatData m_invInertiaLocal;
btVector3FloatData m_totalForce;
btVector3FloatData m_totalTorque;
float m_inverseMass;
float m_linearDamping;
float m_angularDamping;
float m_additionalDampingFactor;
float m_additionalLinearDampingThresholdSqr;
float m_additionalAngularDampingThresholdSqr;
float m_additionalAngularDampingFactor;
float m_linearSleepingThreshold;
float m_angularSleepingThreshold;
int m_additionalDamping;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btRigidBodyDoubleData
{
btCollisionObjectDoubleData m_collisionObjectData;
btMatrix3x3DoubleData m_invInertiaTensorWorld;
btVector3DoubleData m_linearVelocity;
btVector3DoubleData m_angularVelocity;
btVector3DoubleData m_angularFactor;
btVector3DoubleData m_linearFactor;
btVector3DoubleData m_gravity;
btVector3DoubleData m_gravity_acceleration;
btVector3DoubleData m_invInertiaLocal;
btVector3DoubleData m_totalForce;
btVector3DoubleData m_totalTorque;
double m_inverseMass;
double m_linearDamping;
double m_angularDamping;
double m_additionalDampingFactor;
double m_additionalLinearDampingThresholdSqr;
double m_additionalAngularDampingThresholdSqr;
double m_additionalAngularDampingFactor;
double m_linearSleepingThreshold;
double m_angularSleepingThreshold;
int m_additionalDamping;
char m_padding[4];
};
#endif
#endif //BT_RIGIDBODY_H

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

@ -78,7 +78,7 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
btContactSolverInfo infoGlobal;
infoGlobal.m_timeStep = timeStep;
m_constraintSolver->prepareSolve(0,numManifolds);
m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1);
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);
}
@ -155,6 +155,33 @@ void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
}
}
void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
{
body->setGravity(m_gravity);
if (body->getCollisionShape())
{
addCollisionObject(body,group,mask);
}
}
void btSimpleDynamicsWorld::debugDrawWorld()
{
}
void btSimpleDynamicsWorld::addAction(btActionInterface* action)
{
}
void btSimpleDynamicsWorld::removeAction(btActionInterface* action)
{
}
void btSimpleDynamicsWorld::updateAabbs()
{
btTransform predictedTrans;

10
Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
View file

@ -23,7 +23,7 @@ class btOverlappingPairCache;
class btConstraintSolver;
///The btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds.
///Please use btDiscreteDynamicsWorld instead (or btContinuousDynamicsWorld once it is finished).
///Please use btDiscreteDynamicsWorld instead
class btSimpleDynamicsWorld : public btDynamicsWorld
{
protected:
@ -56,8 +56,16 @@ public:
virtual void addRigidBody(btRigidBody* body);
virtual void addRigidBody(btRigidBody* body, short group, short mask);
virtual void removeRigidBody(btRigidBody* body);
virtual void debugDrawWorld();
virtual void addAction(btActionInterface* action);
virtual void removeAction(btActionInterface* action);
///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
virtual void removeCollisionObject(btCollisionObject* collisionObject);

13
Engine/lib/bullet/src/BulletDynamics/Jamfile
View file

@ -1,13 +0,0 @@
SubDir TOP src BulletDynamics ;
Description bulletdynamics : "Bullet Rigidbody Dynamics" ;
Library bulletdynamics :
[ Wildcard ConstraintSolver : *.h *.cpp ]
[ Wildcard Dynamics : *.h *.cpp ]
[ Wildcard Vehicle : *.h *.cpp ]
[ Wildcard Character : *.h *.cpp ]
;
LibDepends bulletdynamics : bulletcollision ;

33
Engine/lib/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
View file

@ -22,7 +22,15 @@
#include "LinearMath/btIDebugDraw.h"
#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
static btRigidBody s_fixedObject( 0,0,0);
#define ROLLING_INFLUENCE_FIX
btRigidBody& btActionInterface::getFixedBody()
{
static btRigidBody s_fixed(0, 0,0);
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
return s_fixed;
}
btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster )
:m_vehicleRaycaster(raycaster),
@ -70,6 +78,7 @@ btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, con
ci.m_frictionSlip = tuning.m_frictionSlip;
ci.m_bIsFrontWheel = isFrontWheel;
ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce;
m_wheelInfo.push_back( btWheelInfo(ci));
@ -186,7 +195,7 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
wheel.m_raycastInfo.m_isInContact = true;
wheel.m_raycastInfo.m_groundObject = &s_fixedObject;///@todo for driving on dynamic/movable objects!;
wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!;
//wheel.m_raycastInfo.m_groundObject = object;
@ -301,10 +310,9 @@ void btRaycastVehicle::updateVehicle( btScalar step )
btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
btScalar gMaxSuspensionForce = btScalar(6000.);
if (suspensionForce > gMaxSuspensionForce)
if (suspensionForce > wheel.m_maxSuspensionForce)
{
suspensionForce = gMaxSuspensionForce;
suspensionForce = wheel.m_maxSuspensionForce;
}
btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
@ -689,7 +697,12 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence));
#else
rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
#endif
m_chassisBody->applyImpulse(sideImp,rel_pos);
//apply friction impulse on the ground
@ -708,13 +721,13 @@ void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
for (int v=0;v<this->getNumWheels();v++)
{
btVector3 wheelColor(0,255,255);
btVector3 wheelColor(0,1,1);
if (getWheelInfo(v).m_raycastInfo.m_isInContact)
{
wheelColor.setValue(0,0,255);
wheelColor.setValue(0,0,1);
} else
{
wheelColor.setValue(255,0,255);
wheelColor.setValue(1,0,1);
}
btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();
@ -743,14 +756,14 @@ void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3&
if (rayCallback.hasHit())
{
btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
const btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
if (body && body->hasContactResponse())
{
result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
result.m_hitNormalInWorld.normalize();
result.m_distFraction = rayCallback.m_closestHitFraction;
return body;
return (void*)body;
}
}
return 0;

37
Engine/lib/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h
View file

@ -8,8 +8,8 @@
* of this software for any purpose.
* It is provided "as is" without express or implied warranty.
*/
#ifndef RAYCASTVEHICLE_H
#define RAYCASTVEHICLE_H
#ifndef BT_RAYCASTVEHICLE_H
#define BT_RAYCASTVEHICLE_H
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
@ -29,6 +29,10 @@ class btRaycastVehicle : public btActionInterface
btAlignedObjectArray<btVector3> m_axle;
btAlignedObjectArray<btScalar> m_forwardImpulse;
btAlignedObjectArray<btScalar> m_sideImpulse;
///backwards compatibility
int m_userConstraintType;
int m_userConstraintId;
public:
class btVehicleTuning
@ -40,7 +44,8 @@ public:
m_suspensionCompression(btScalar(0.83)),
m_suspensionDamping(btScalar(0.88)),
m_maxSuspensionTravelCm(btScalar(500.)),
m_frictionSlip(btScalar(10.5))
m_frictionSlip(btScalar(10.5)),
m_maxSuspensionForce(btScalar(6000.))
{
}
btScalar m_suspensionStiffness;
@ -48,6 +53,7 @@ public:
btScalar m_suspensionDamping;
btScalar m_maxSuspensionTravelCm;
btScalar m_frictionSlip;
btScalar m_maxSuspensionForce;
};
private:
@ -78,6 +84,7 @@ public:
///btActionInterface interface
virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step)
{
(void) collisionWorld;
updateVehicle(step);
}
@ -105,7 +112,7 @@ public:
void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true );
void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
// void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
btWheelInfo& addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel);
@ -188,6 +195,26 @@ public:
}
///backwards compatibility
int getUserConstraintType() const
{
return m_userConstraintType ;
}
void setUserConstraintType(int userConstraintType)
{
m_userConstraintType = userConstraintType;
};
void setUserConstraintId(int uid)
{
m_userConstraintId = uid;
}
int getUserConstraintId() const
{
return m_userConstraintId;
}
};
@ -205,5 +232,5 @@ public:
};
#endif //RAYCASTVEHICLE_H
#endif //BT_RAYCASTVEHICLE_H

8
Engine/lib/bullet/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
View file

@ -1,5 +1,5 @@
/*
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
* Copyright (c) 2005 Erwin Coumans http://bulletphysics.org
*
* Permission to use, copy, modify, distribute and sell this software
* and its documentation for any purpose is hereby granted without fee,
@ -8,8 +8,8 @@
* of this software for any purpose.
* It is provided "as is" without express or implied warranty.
*/
#ifndef VEHICLE_RAYCASTER_H
#define VEHICLE_RAYCASTER_H
#ifndef BT_VEHICLE_RAYCASTER_H
#define BT_VEHICLE_RAYCASTER_H
#include "LinearMath/btVector3.h"
@ -31,5 +31,5 @@ virtual ~btVehicleRaycaster()
};
#endif //VEHICLE_RAYCASTER_H
#endif //BT_VEHICLE_RAYCASTER_H

9
Engine/lib/bullet/src/BulletDynamics/Vehicle/btWheelInfo.h
View file

@ -8,8 +8,8 @@
* of this software for any purpose.
* It is provided "as is" without express or implied warranty.
*/
#ifndef WHEEL_INFO_H
#define WHEEL_INFO_H
#ifndef BT_WHEEL_INFO_H
#define BT_WHEEL_INFO_H
#include "LinearMath/btVector3.h"
#include "LinearMath/btTransform.h"
@ -29,6 +29,7 @@ struct btWheelInfoConstructionInfo
btScalar m_wheelsDampingCompression;
btScalar m_wheelsDampingRelaxation;
btScalar m_frictionSlip;
btScalar m_maxSuspensionForce;
bool m_bIsFrontWheel;
};
@ -68,6 +69,7 @@ struct btWheelInfo
btScalar m_rotation;
btScalar m_deltaRotation;
btScalar m_rollInfluence;
btScalar m_maxSuspensionForce;
btScalar m_engineForce;
@ -99,6 +101,7 @@ struct btWheelInfo
m_brake = btScalar(0.);
m_rollInfluence = btScalar(0.1);
m_bIsFrontWheel = ci.m_bIsFrontWheel;
m_maxSuspensionForce = ci.m_maxSuspensionForce;
}
@ -112,5 +115,5 @@ struct btWheelInfo
};
#endif //WHEEL_INFO_H
#endif //BT_WHEEL_INFO_H

53
Engine/lib/bullet/src/BulletDynamics/ibmsdk/Makefile
View file

@ -1,53 +0,0 @@
#### Source code Dirs
VPATH = \
../ConstraintSolver \
../Dynamics \
../Vehicle
ROOT = ../../..
#### Library
LIBRARY_ppu = bulletdynamics.a
#### Compiler flags
CPPFLAGS = \
-DUSE_LIBSPE2 \
-I../ConstraintSolver \
-I../Dynamics \
-I../Vehicle \
-I$(ROOT)/src \
-I$(SDKINC)
#### Optimization level flags
#CC_OPT_LEVEL = $(CC_OPT_LEVEL_DEBUG)
CC_OPT_LEVEL = -O3
##### Objects to be archived in lib
OBJS = \
btContactConstraint.o \
btGeneric6DofConstraint.o \
btHingeConstraint.o \
btPoint2PointConstraint.o \
btSequentialImpulseConstraintSolver.o \
btSolve2LinearConstraint.o \
btTypedConstraint.o \
btDiscreteDynamicsWorld.o \
btRigidBody.o \
btSimpleDynamicsWorld.o \
btRaycastVehicle.o \
btWheelInfo.o
#### Install directories
INSTALL_DIR = $(ROOT)/lib/ibmsdk
INSTALL_FILES = $(LIBRARY_ppu)
IBM_CELLSDK_VERSION := $(shell if [ -d /opt/cell ]; then echo "3.0"; fi)
ifeq ("$(IBM_CELLSDK_VERSION)","3.0")
CELL_TOP ?= /opt/cell/sdk
include $(CELL_TOP)/buildutils/make.footer
else
CELL_TOP ?= /opt/ibm/cell-sdk/prototype
include $(CELL_TOP)/make.footer
endif

11
Engine/lib/bullet/src/BulletDynamics/premake4.lua Normal file
View file

@ -0,0 +1,11 @@
project "BulletDynamics"
kind "StaticLib"
targetdir "../../lib"
includedirs {
"..",
}
files {
"**.cpp",
"**.h"
}