diff options
author | Guillermo S. Romero <gsr.b3d@infernal-iceberg.com> | 2010-06-17 07:29:16 +0400 |
---|---|---|
committer | Guillermo S. Romero <gsr.b3d@infernal-iceberg.com> | 2010-06-17 07:29:16 +0400 |
commit | 3398d901a17fbb093f593830b3ee40f32ada993c (patch) | |
tree | e5c9946e6c3acaa197a32804049c2ba71d4a5b13 /extern/bullet2/BulletDynamics | |
parent | ed59822857de7e7b41b33b79c306f5e9b8755c62 (diff) |
Revert changes back to original state, before 29506.
Diffstat (limited to 'extern/bullet2/BulletDynamics')
49 files changed, 0 insertions, 14945 deletions
diff --git a/extern/bullet2/BulletDynamics/CMakeLists.txt b/extern/bullet2/BulletDynamics/CMakeLists.txt deleted file mode 100644 index 7516e2b957f..00000000000 --- a/extern/bullet2/BulletDynamics/CMakeLists.txt +++ /dev/null @@ -1,112 +0,0 @@ -INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src ) - - - -SET(BulletDynamics_SRCS - Character/btKinematicCharacterController.cpp - ConstraintSolver/btConeTwistConstraint.cpp - ConstraintSolver/btContactConstraint.cpp - ConstraintSolver/btGeneric6DofConstraint.cpp - ConstraintSolver/btGeneric6DofSpringConstraint.cpp - ConstraintSolver/btHinge2Constraint.cpp - ConstraintSolver/btHingeConstraint.cpp - ConstraintSolver/btPoint2PointConstraint.cpp - ConstraintSolver/btSequentialImpulseConstraintSolver.cpp - ConstraintSolver/btSliderConstraint.cpp - ConstraintSolver/btSolve2LinearConstraint.cpp - ConstraintSolver/btTypedConstraint.cpp - ConstraintSolver/btUniversalConstraint.cpp - Dynamics/btContinuousDynamicsWorld.cpp - Dynamics/btDiscreteDynamicsWorld.cpp - Dynamics/btRigidBody.cpp - Dynamics/btSimpleDynamicsWorld.cpp - Dynamics/Bullet-C-API.cpp - Vehicle/btRaycastVehicle.cpp - Vehicle/btWheelInfo.cpp -) - -SET(Root_HDRS - ../btBulletDynamicsCommon.h - ../btBulletCollisionCommon.h -) -SET(ConstraintSolver_HDRS - ConstraintSolver/btConeTwistConstraint.h - ConstraintSolver/btConstraintSolver.h - ConstraintSolver/btContactConstraint.h - ConstraintSolver/btContactSolverInfo.h - ConstraintSolver/btGeneric6DofConstraint.h - ConstraintSolver/btGeneric6DofSpringConstraint.h - ConstraintSolver/btHinge2Constraint.h - ConstraintSolver/btHingeConstraint.h - ConstraintSolver/btJacobianEntry.h - ConstraintSolver/btPoint2PointConstraint.h - ConstraintSolver/btSequentialImpulseConstraintSolver.h - ConstraintSolver/btSliderConstraint.h - ConstraintSolver/btSolve2LinearConstraint.h - ConstraintSolver/btSolverBody.h - ConstraintSolver/btSolverConstraint.h - ConstraintSolver/btTypedConstraint.h - ConstraintSolver/btUniversalConstraint.h -) -SET(Dynamics_HDRS - Dynamics/btActionInterface.h - Dynamics/btContinuousDynamicsWorld.h - Dynamics/btDiscreteDynamicsWorld.h - Dynamics/btDynamicsWorld.h - Dynamics/btSimpleDynamicsWorld.h - Dynamics/btRigidBody.h -) -SET(Vehicle_HDRS - Vehicle/btRaycastVehicle.h - Vehicle/btVehicleRaycaster.h - Vehicle/btWheelInfo.h -) - -SET(Character_HDRS - Character/btCharacterControllerInterface.h - Character/btKinematicCharacterController.h -) - - - -SET(BulletDynamics_HDRS - ${Root_HDRS} - ${ConstraintSolver_HDRS} - ${Dynamics_HDRS} - ${Vehicle_HDRS} - ${Character_HDRS} -) - - -ADD_LIBRARY(BulletDynamics ${BulletDynamics_SRCS} ${BulletDynamics_HDRS}) -SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES VERSION ${BULLET_VERSION}) -SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES SOVERSION ${BULLET_VERSION}) -IF (BUILD_SHARED_LIBS) - TARGET_LINK_LIBRARIES(BulletDynamics BulletCollision LinearMath) -ENDIF (BUILD_SHARED_LIBS) - -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 FILES_MATCHING PATTERN "*.h") - INSTALL(FILES ../btBulletDynamicsCommon.h DESTINATION include/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) - ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) -ENDIF (INSTALL_LIBS)
\ No newline at end of file diff --git a/extern/bullet2/BulletDynamics/Character/btCharacterControllerInterface.h b/extern/bullet2/BulletDynamics/Character/btCharacterControllerInterface.h deleted file mode 100644 index 19373daa241..00000000000 --- a/extern/bullet2/BulletDynamics/Character/btCharacterControllerInterface.h +++ /dev/null @@ -1,45 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -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 CHARACTER_CONTROLLER_INTERFACE_H -#define CHARACTER_CONTROLLER_INTERFACE_H - -#include "LinearMath/btVector3.h" -#include "BulletDynamics/Dynamics/btActionInterface.h" - -class btCollisionShape; -class btRigidBody; -class btCollisionWorld; - -class btCharacterControllerInterface : public btActionInterface -{ -public: - btCharacterControllerInterface () {}; - virtual ~btCharacterControllerInterface () {}; - - virtual void setWalkDirection(const btVector3& walkDirection) = 0; - virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0; - virtual void reset () = 0; - virtual void warp (const btVector3& origin) = 0; - - virtual void preStep ( btCollisionWorld* collisionWorld) = 0; - virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0; - virtual bool canJump () const = 0; - virtual void jump () = 0; - - virtual bool onGround () const = 0; -}; - -#endif diff --git a/extern/bullet2/BulletDynamics/Character/btKinematicCharacterController.cpp b/extern/bullet2/BulletDynamics/Character/btKinematicCharacterController.cpp deleted file mode 100644 index d551218272e..00000000000 --- a/extern/bullet2/BulletDynamics/Character/btKinematicCharacterController.cpp +++ /dev/null @@ -1,553 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -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 "LinearMath/btIDebugDraw.h" -#include "BulletCollision/CollisionDispatch/btGhostObject.h" -#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include "LinearMath/btDefaultMotionState.h" -#include "btKinematicCharacterController.h" - - -// static helper method -static btVector3 -getNormalizedVector(const btVector3& v) -{ - btVector3 n = v.normalized(); - if (n.length() < SIMD_EPSILON) { - n.setValue(0, 0, 0); - } - return n; -} - - -///@todo Interact with dynamic objects, -///Ride kinematicly animated platforms properly -///More realistic (or maybe just a config option) falling -/// -> Should integrate falling velocity manually and use that in stepDown() -///Support jumping -///Support ducking -class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback -{ -public: - btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) - { - m_me = me; - } - - virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) - { - if (rayResult.m_collisionObject == m_me) - return 1.0; - - return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace); - } -protected: - btCollisionObject* m_me; -}; - -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)) - { - m_me = me; - } - - virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) - { - if (convexResult.m_hitCollisionObject == m_me) - return 1.0; - - return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); - } -protected: - btCollisionObject* m_me; -}; - -/* - * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal' - * - * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html - */ -btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal) -{ - return direction - (btScalar(2.0) * direction.dot(normal)) * normal; -} - -/* - * Returns the portion of 'direction' that is parallel to 'normal' - */ -btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal) -{ - btScalar magnitude = direction.dot(normal); - return normal * magnitude; -} - -/* - * Returns the portion of 'direction' that is perpindicular to 'normal' - */ -btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal) -{ - return direction - parallelComponent(direction, normal); -} - -btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis) -{ - m_upAxis = upAxis; - m_addedMargin = 0.02f; - m_walkDirection.setValue(0,0,0); - m_useGhostObjectSweepTest = true; - m_ghostObject = ghostObject; - m_stepHeight = stepHeight; - m_turnAngle = btScalar(0.0); - m_convexShape=convexShape; - m_useWalkDirection = true; // use walk direction by default, legacy behavior - m_velocityTimeInterval = 0.0; -} - -btKinematicCharacterController::~btKinematicCharacterController () -{ -} - -btPairCachingGhostObject* btKinematicCharacterController::getGhostObject() -{ - return m_ghostObject; -} - -bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld) -{ - - bool penetration = false; - - collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher()); - - m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); - - btScalar maxPen = btScalar(0.0); - for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) - { - m_manifoldArray.resize(0); - - btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i]; - - if (collisionPair->m_algorithm) - collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray); - - - for (int j=0;j<m_manifoldArray.size();j++) - { - btPersistentManifold* manifold = m_manifoldArray[j]; - btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0); - for (int p=0;p<manifold->getNumContacts();p++) - { - const btManifoldPoint&pt = manifold->getContactPoint(p); - - if (pt.getDistance() < 0.0) - { - if (pt.getDistance() < maxPen) - { - maxPen = pt.getDistance(); - m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? - - } - m_currentPosition += pt.m_normalWorldOnB * directionSign * pt.getDistance() * btScalar(0.2); - penetration = true; - } else { - //printf("touching %f\n", pt.getDistance()); - } - } - - //manifold->clearManifold(); - } - } - btTransform newTrans = m_ghostObject->getWorldTransform(); - newTrans.setOrigin(m_currentPosition); - m_ghostObject->setWorldTransform(newTrans); -// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]); - return penetration; -} - -void btKinematicCharacterController::stepUp ( btCollisionWorld* world) -{ - // phase 1: up - btTransform start, end; - m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * m_stepHeight; - - start.setIdentity (); - end.setIdentity (); - - /* FIXME: Handle penetration properly */ - start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * btScalar(0.1f)); - end.setOrigin (m_targetPosition); - - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); - callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - - if (m_useGhostObjectSweepTest) - { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); - } - else - { - world->convexSweepTest (m_convexShape, start, end, callback); - } - - 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); - } else { - m_currentStepOffset = m_stepHeight; - m_currentPosition = m_targetPosition; - } -} - -void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) -{ - btVector3 movementDirection = m_targetPosition - m_currentPosition; - btScalar movementLength = movementDirection.length(); - if (movementLength>SIMD_EPSILON) - { - movementDirection.normalize(); - - btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal); - reflectDir.normalize(); - - btVector3 parallelDir, perpindicularDir; - - parallelDir = parallelComponent (reflectDir, hitNormal); - perpindicularDir = perpindicularComponent (reflectDir, hitNormal); - - m_targetPosition = m_currentPosition; - if (0)//tangentMag != 0.0) - { - btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength); -// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]); - m_targetPosition += parComponent; - } - - if (normalMag != 0.0) - { - btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength); -// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]); - m_targetPosition += perpComponent; - } - } else - { -// printf("movementLength don't normalize a zero vector\n"); - } -} - -void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove) -{ - // printf("m_normalizedDirection=%f,%f,%f\n", - // m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]); - // phase 2: forward and strafe - btTransform start, end; - m_targetPosition = m_currentPosition + walkMove; - start.setIdentity (); - end.setIdentity (); - - btScalar fraction = 1.0; - btScalar distance2 = (m_currentPosition-m_targetPosition).length2(); -// printf("distance2=%f\n",distance2); - - if (m_touchingContact) - { - if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) - updateTargetPositionBasedOnCollision (m_touchingNormal); - } - - int maxIter = 10; - - while (fraction > btScalar(0.01) && maxIter-- > 0) - { - start.setOrigin (m_currentPosition); - end.setOrigin (m_targetPosition); - - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); - callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - - - btScalar margin = m_convexShape->getMargin(); - m_convexShape->setMargin(margin + m_addedMargin); - - - if (m_useGhostObjectSweepTest) - { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } else - { - collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } - - m_convexShape->setMargin(margin); - - - fraction -= callback.m_closestHitFraction; - - if (callback.hasHit()) - { - // we moved only a fraction - btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length(); - if (hitDistance<0.f) - { -// printf("neg dist?\n"); - } - - /* 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); - } - - updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld); - btVector3 currentDir = m_targetPosition - m_currentPosition; - distance2 = currentDir.length2(); - if (distance2 > SIMD_EPSILON) - { - currentDir.normalize(); - /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */ - if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0)) - { - break; - } - } else - { -// printf("currentDir: don't normalize a zero vector\n"); - break; - } - } else { - // we moved whole way - m_currentPosition = m_targetPosition; - } - - // if (callback.m_closestHitFraction == 0.f) - // break; - - } -} - -void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt) -{ - btTransform start, end; - - // phase 3: down - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * m_currentStepOffset; - btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * m_stepHeight; - m_targetPosition -= (step_drop + gravity_drop); - - start.setIdentity (); - end.setIdentity (); - - start.setOrigin (m_currentPosition); - end.setOrigin (m_targetPosition); - - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); - callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - - if (m_useGhostObjectSweepTest) - { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } else - { - collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } - - if (callback.hasHit()) - { - // we dropped a fraction of the height -> hit floor - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); - } else { - // we dropped the full height - - m_currentPosition = m_targetPosition; - } -} - - - -void btKinematicCharacterController::setWalkDirection -( -const btVector3& walkDirection -) -{ - m_useWalkDirection = true; - m_walkDirection = walkDirection; - m_normalizedDirection = getNormalizedVector(m_walkDirection); -} - - - -void btKinematicCharacterController::setVelocityForTimeInterval -( -const btVector3& velocity, -btScalar timeInterval -) -{ -// printf("setVelocity!\n"); -// printf(" interval: %f\n", timeInterval); -// printf(" velocity: (%f, %f, %f)\n", -// velocity.x(), velocity.y(), velocity.z()); - - m_useWalkDirection = false; - m_walkDirection = velocity; - m_normalizedDirection = getNormalizedVector(m_walkDirection); - m_velocityTimeInterval = timeInterval; -} - - - -void btKinematicCharacterController::reset () -{ -} - -void btKinematicCharacterController::warp (const btVector3& origin) -{ - btTransform xform; - xform.setIdentity(); - xform.setOrigin (origin); - m_ghostObject->setWorldTransform (xform); -} - - -void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld) -{ - - int numPenetrationLoops = 0; - m_touchingContact = false; - while (recoverFromPenetration (collisionWorld)) - { - numPenetrationLoops++; - m_touchingContact = true; - if (numPenetrationLoops > 4) - { -// printf("character could not recover from penetration = %d\n", numPenetrationLoops); - break; - } - } - - m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); - m_targetPosition = m_currentPosition; -// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]); - - -} - -void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt) -{ -// printf("playerStep(): "); -// printf(" dt = %f", dt); - - // quick check... - if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) { -// printf("\n"); - return; // no motion - } - - btTransform xform; - xform = m_ghostObject->getWorldTransform (); - -// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]); -// printf("walkSpeed=%f\n",walkSpeed); - - stepUp (collisionWorld); - if (m_useWalkDirection) { - stepForwardAndStrafe (collisionWorld, m_walkDirection); - } else { - //printf(" time: %f", m_velocityTimeInterval); - // still have some time left for moving! - btScalar dtMoving = - (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); - - // okay, step - stepForwardAndStrafe(collisionWorld, move); - } - stepDown (collisionWorld, dt); - - // printf("\n"); - - xform.setOrigin (m_currentPosition); - m_ghostObject->setWorldTransform (xform); -} - -void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) -{ - m_fallSpeed = fallSpeed; -} - -void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed) -{ - m_jumpSpeed = jumpSpeed; -} - -void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight) -{ - m_maxJumpHeight = maxJumpHeight; -} - -bool btKinematicCharacterController::canJump () const -{ - return onGround(); -} - -void btKinematicCharacterController::jump () -{ - if (!canJump()) - return; - -#if 0 - currently no jumping. - btTransform xform; - m_rigidBody->getMotionState()->getWorldTransform (xform); - btVector3 up = xform.getBasis()[1]; - up.normalize (); - btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0); - m_rigidBody->applyCentralImpulse (up * magnitude); -#endif -} - -bool btKinematicCharacterController::onGround () const -{ - return true; -} - - -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; -}
\ No newline at end of file diff --git a/extern/bullet2/BulletDynamics/Character/btKinematicCharacterController.h b/extern/bullet2/BulletDynamics/Character/btKinematicCharacterController.h deleted file mode 100644 index 16bf5df7e70..00000000000 --- a/extern/bullet2/BulletDynamics/Character/btKinematicCharacterController.h +++ /dev/null @@ -1,145 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -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 KINEMATIC_CHARACTER_CONTROLLER_H -#define KINEMATIC_CHARACTER_CONTROLLER_H - -#include "LinearMath/btVector3.h" - -#include "btCharacterControllerInterface.h" - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" - - -class btCollisionShape; -class btRigidBody; -class btCollisionWorld; -class btCollisionDispatcher; -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 -{ -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_fallSpeed; - btScalar m_jumpSpeed; - btScalar m_maxJumpHeight; - - btScalar m_turnAngle; - - btScalar m_stepHeight; - - btScalar m_addedMargin;//@todo: remove this and fix the code - - ///this is the desired walk direction, set by the user - btVector3 m_walkDirection; - btVector3 m_normalizedDirection; - - //some internal variables - btVector3 m_currentPosition; - btScalar m_currentStepOffset; - btVector3 m_targetPosition; - - ///keep track of the contact manifolds - btManifoldArray m_manifoldArray; - - bool m_touchingContact; - btVector3 m_touchingNormal; - - bool m_useGhostObjectSweepTest; - bool m_useWalkDirection; - 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); - - bool recoverFromPenetration ( btCollisionWorld* collisionWorld); - void stepUp (btCollisionWorld* collisionWorld); - void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0)); - void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove); - void stepDown (btCollisionWorld* collisionWorld, btScalar dt); -public: - btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1); - ~btKinematicCharacterController (); - - - ///btActionInterface interface - virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime) - { - preStep ( collisionWorld); - playerStep (collisionWorld, deltaTime); - } - - ///btActionInterface interface - void debugDraw(btIDebugDraw* debugDrawer); - - void setUpAxis (int axis) - { - if (axis < 0) - axis = 0; - if (axis > 2) - axis = 2; - m_upAxis = axis; - } - - /// 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. - /// 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. - /// This call will reset any walk direction set by setWalkDirection(). - /// Negative time intervals will result in no motion. - virtual void setVelocityForTimeInterval(const btVector3& velocity, - btScalar timeInterval); - - void reset (); - void warp (const btVector3& origin); - - void preStep ( btCollisionWorld* collisionWorld); - void playerStep ( btCollisionWorld* collisionWorld, btScalar dt); - - void setFallSpeed (btScalar fallSpeed); - void setJumpSpeed (btScalar jumpSpeed); - void setMaxJumpHeight (btScalar maxJumpHeight); - bool canJump () const; - void jump (); - - btPairCachingGhostObject* getGhostObject(); - void setUseGhostSweepTest(bool useGhostObjectSweepTest) - { - m_useGhostObjectSweepTest = useGhostObjectSweepTest; - } - - bool onGround () const; -}; - -#endif // KINEMATIC_CHARACTER_CONTROLLER_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp deleted file mode 100644 index bf77c495404..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ /dev/null @@ -1,1117 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios - -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. - -Written by: Marcus Hennix -*/ - - -#include "btConeTwistConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btMinMax.h" -#include <new> - - - -//#define CONETWIST_USE_OBSOLETE_SOLVER true -#define CONETWIST_USE_OBSOLETE_SOLVER false -#define CONETWIST_DEF_FIX_THRESH btScalar(.05f) - - -SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld) -{ - btVector3 vec = axis * invInertiaWorld; - return axis.dot(vec); -} - - - - -btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, - const btTransform& rbAFrame,const btTransform& rbBFrame) - :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), - m_angularOnly(false), - m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) -{ - init(); -} - -btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) - :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), - m_angularOnly(false), - m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) -{ - m_rbBFrame = m_rbAFrame; - init(); -} - - -void btConeTwistConstraint::init() -{ - m_angularOnly = false; - m_solveTwistLimit = false; - m_solveSwingLimit = false; - m_bMotorEnabled = false; - m_maxMotorImpulse = btScalar(-1); - - 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); -} - - -void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } - else - { - info->m_numConstraintRows = 3; - info->nub = 3; - calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); - if(m_solveSwingLimit) - { - info->m_numConstraintRows++; - info->nub--; - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) - { - info->m_numConstraintRows++; - info->nub--; - } - } - if(m_solveTwistLimit) - { - info->m_numConstraintRows++; - info->nub--; - } - } -} - -void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info) -{ - //always reserve 6 rows: object transform is not available on SPU - info->m_numConstraintRows = 6; - info->nub = 0; - -} - - -void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) -{ - getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); -} - -void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) -{ - calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB); - - btAssert(!m_useSolveConstraintObsolete); - // set jacobian - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; - btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); - btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - // set right hand side - 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; - btVector3 ax1; - // angular limits - if(m_solveSwingLimit) - { - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) - { - btTransform trA = transA*m_rbAFrame; - btVector3 p = trA.getBasis().getColumn(1); - btVector3 q = trA.getBasis().getColumn(2); - int srow1 = srow + info->rowskip; - J1[srow+0] = p[0]; - J1[srow+1] = p[1]; - J1[srow+2] = p[2]; - J1[srow1+0] = q[0]; - J1[srow1+1] = q[1]; - J1[srow1+2] = q[2]; - J2[srow+0] = -p[0]; - J2[srow+1] = -p[1]; - J2[srow+2] = -p[2]; - J2[srow1+0] = -q[0]; - J2[srow1+1] = -q[1]; - J2[srow1+2] = -q[2]; - btScalar fact = info->fps * m_relaxationFactor; - info->m_constraintError[srow] = fact * m_swingAxis.dot(p); - info->m_constraintError[srow1] = fact * m_swingAxis.dot(q); - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - info->m_lowerLimit[srow1] = -SIMD_INFINITY; - info->m_upperLimit[srow1] = SIMD_INFINITY; - srow = srow1 + info->rowskip; - } - else - { - ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - btScalar k = info->fps * m_biasFactor; - - info->m_constraintError[srow] = k * m_swingCorrection; - 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; - srow += info->rowskip; - } - } - if(m_solveTwistLimit) - { - ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor; - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - btScalar k = info->fps * m_biasFactor; - info->m_constraintError[srow] = k * m_twistCorrection; - if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) - { - info->cfm[srow] = m_angCFM; - } - if(m_twistSpan > 0.0f) - { - - if(m_twistCorrection > 0.0f) - { - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - } - else - { - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - srow += info->rowskip; - } -} - - - -void btConeTwistConstraint::buildJacobian() -{ - if (m_useSolveConstraintObsolete) - { - m_appliedImpulse = btScalar(0.); - m_accTwistLimitImpulse = btScalar(0.); - m_accSwingLimitImpulse = btScalar(0.); - m_accMotorImpulse = btVector3(0.,0.,0.); - - if (!m_angularOnly) - { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 relPos = pivotBInW - pivotAInW; - - btVector3 normal[3]; - if (relPos.length2() > SIMD_EPSILON) - { - normal[0] = relPos.normalized(); - } - else - { - normal[0].setValue(btScalar(1.0),0,0); - } - - btPlaneSpace1(normal[0], normal[1], normal[2]); - - for (int i=0;i<3;i++) - { - new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normal[i], - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - } - } - - calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); - } -} - - - -void btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep) -{ - #ifndef __SPU__ - 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; - bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); - btVector3 vel2; - bodyB.internalGetVelocityInLocalPointObsolete(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 ftorqueAxis1 = rel_pos1.cross(normal); - btVector3 ftorqueAxis2 = rel_pos2.cross(normal); - bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); - bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); - - } - } - - // apply motor - if (m_bMotorEnabled) - { - // compute current and predicted transforms - btTransform trACur = m_rbA.getCenterOfMassTransform(); - btTransform trBCur = m_rbB.getCenterOfMassTransform(); - btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA); - btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB); - btTransform trAPred; trAPred.setIdentity(); - btVector3 zerovec(0,0,0); - btTransformUtil::integrateTransform( - trACur, zerovec, omegaA, timeStep, trAPred); - btTransform trBPred; trBPred.setIdentity(); - btTransformUtil::integrateTransform( - trBCur, zerovec, omegaB, timeStep, trBPred); - - // compute desired transforms in world - btTransform trPose(m_qTarget); - btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse(); - btTransform trADes = trBPred * trABDes; - btTransform trBDes = trAPred * trABDes.inverse(); - - // compute desired omegas in world - btVector3 omegaADes, omegaBDes; - - btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes); - btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes); - - // compute delta omegas - btVector3 dOmegaA = omegaADes - omegaA; - btVector3 dOmegaB = omegaBDes - omegaB; - - // compute weighted avg axis of dOmega (weighting based on inertias) - btVector3 axisA, axisB; - btScalar kAxisAInv = 0, kAxisBInv = 0; - - if (dOmegaA.length2() > SIMD_EPSILON) - { - axisA = dOmegaA.normalized(); - kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA); - } - - if (dOmegaB.length2() > SIMD_EPSILON) - { - axisB = dOmegaB.normalized(); - kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB); - } - - btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB; - - static bool bDoTorque = true; - if (bDoTorque && avgAxis.length2() > SIMD_EPSILON) - { - avgAxis.normalize(); - kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis); - kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis); - btScalar kInvCombined = kAxisAInv + kAxisBInv; - - btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) / - (kInvCombined * kInvCombined); - - if (m_maxMotorImpulse >= 0) - { - btScalar fMaxImpulse = m_maxMotorImpulse; - if (m_bNormalizedMotorStrength) - fMaxImpulse = fMaxImpulse/kAxisAInv; - - btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse; - btScalar newUnclampedMag = newUnclampedAccImpulse.length(); - if (newUnclampedMag > fMaxImpulse) - { - newUnclampedAccImpulse.normalize(); - newUnclampedAccImpulse *= fMaxImpulse; - impulse = newUnclampedAccImpulse - m_accMotorImpulse; - } - m_accMotorImpulse += impulse; - } - - btScalar impulseMag = impulse.length(); - btVector3 impulseAxis = impulse / 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.internalGetAngularVelocity(angVelA); - btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB); - btVector3 relVel = angVelB - angVelA; - if (relVel.length2() > SIMD_EPSILON) - { - btVector3 relVelAxis = relVel.normalized(); - btScalar m_kDamping = btScalar(1.) / - (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) + - getRigidBodyB().computeAngularImpulseDenominator(relVelAxis)); - btVector3 impulse = m_damping * m_kDamping * relVel; - - btScalar impulseMag = impulse.length(); - btVector3 impulseAxis = impulse / impulseMag; - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); - } - } - - // joint limits - { - ///solve angular part - btVector3 angVelA; - bodyA.internalGetAngularVelocity(angVelA); - btVector3 angVelB; - bodyB.internalGetAngularVelocity(angVelB); - - // solve swing limit - if (m_solveSwingLimit) - { - btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep; - btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis); - if (relSwingVel > 0) - amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor; - btScalar impulseMag = amplitude * m_kSwing; - - // Clamp the accumulated impulse - btScalar temp = m_accSwingLimitImpulse; - m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); - impulseMag = m_accSwingLimitImpulse - temp; - - btVector3 impulse = m_swingAxis * impulseMag; - - // don't let cone response affect twist - // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit) - { - btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA; - btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple; - impulse = impulseNoTwistCouple; - } - - impulseMag = impulse.length(); - btVector3 noTwistSwingAxis = impulse / impulseMag; - - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); - } - - - // solve twist limit - if (m_solveTwistLimit) - { - btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep; - btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis ); - if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important) - amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor; - btScalar impulseMag = amplitude * m_kTwist; - - // Clamp the accumulated impulse - btScalar temp = m_accTwistLimitImpulse; - m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); - impulseMag = m_accTwistLimitImpulse - temp; - - btVector3 impulse = 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); - } - } - } -#else -btAssert(0); -#endif //__SPU__ -} - - - - -void btConeTwistConstraint::updateRHS(btScalar timeStep) -{ - (void)timeStep; - -} - - -#ifndef __SPU__ -void btConeTwistConstraint::calcAngleInfo() -{ - m_swingCorrection = btScalar(0.); - m_twistLimitSign = btScalar(0.); - m_solveTwistLimit = false; - m_solveSwingLimit = false; - - btVector3 b1Axis1,b1Axis2,b1Axis3; - btVector3 b2Axis1,b2Axis2; - - b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); - b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); - - btScalar swing1=btScalar(0.),swing2 = btScalar(0.); - - btScalar swx=btScalar(0.),swy = btScalar(0.); - btScalar thresh = btScalar(10.); - btScalar fact; - - // Get Frame into world space - if (m_swingSpan1 >= btScalar(0.05f)) - { - b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); - swx = b2Axis1.dot(b1Axis1); - swy = b2Axis1.dot(b1Axis2); - swing1 = btAtan2Fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; - fact = fact / (fact + btScalar(1.0)); - swing1 *= fact; - } - - if (m_swingSpan2 >= btScalar(0.05f)) - { - b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); - swx = b2Axis1.dot(b1Axis1); - swy = b2Axis1.dot(b1Axis3); - swing2 = btAtan2Fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; - fact = fact / (fact + btScalar(1.0)); - swing2 *= fact; - } - - btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); - btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); - btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq; - - if (EllipseAngle > 1.0f) - { - m_swingCorrection = EllipseAngle-1.0f; - m_solveSwingLimit = true; - // Calculate necessary axis & factors - m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); - m_swingAxis.normalize(); - btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; - m_swingAxis *= swingAxisSign; - } - - // Twist limits - if (m_twistSpan >= btScalar(0.)) - { - btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1); - btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); - btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); - btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); - m_twistAngle = twist; - -// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); - btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.); - if (twist <= -m_twistSpan*lockedFreeFactor) - { - m_twistCorrection = -(twist + m_twistSpan); - m_solveTwistLimit = true; - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); - m_twistAxis *= -1.0f; - } - else if (twist > m_twistSpan*lockedFreeFactor) - { - m_twistCorrection = (twist - m_twistSpan); - m_solveTwistLimit = true; - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); - } - } -} -#endif //__SPU__ - -static btVector3 vTwist(1,0,0); // twist axis in constraint's space - - - -void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) -{ - m_swingCorrection = btScalar(0.); - m_twistLimitSign = btScalar(0.); - m_solveTwistLimit = false; - m_solveSwingLimit = false; - // compute rotation of A wrt B (in constraint space) - if (m_bMotorEnabled && (!m_useSolveConstraintObsolete)) - { // it is assumed that setMotorTarget() was alredy called - // and motor target m_qTarget is within constraint limits - // TODO : split rotation to pure swing and pure twist - // compute desired transforms in world - btTransform trPose(m_qTarget); - btTransform trA = transA * m_rbAFrame; - btTransform trB = transB * m_rbBFrame; - btTransform trDeltaAB = trB * trPose * trA.inverse(); - btQuaternion qDeltaAB = trDeltaAB.getRotation(); - btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); - m_swingAxis = swingAxis; - m_swingAxis.normalize(); - m_swingCorrection = qDeltaAB.getAngle(); - if(!btFuzzyZero(m_swingCorrection)) - { - m_solveSwingLimit = true; - } - return; - } - - - { - // compute rotation of A wrt B (in constraint space) - btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation(); - btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation(); - btQuaternion qAB = qB.inverse() * qA; - // split rotation into cone and twist - // (all this is done from B's perspective. Maybe I should be averaging axes...) - btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); - btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); - btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); - - if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh) - { - btScalar swingAngle, swingLimit = 0; btVector3 swingAxis; - computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit); - - if (swingAngle > swingLimit * m_limitSoftness) - { - m_solveSwingLimit = true; - - // compute limit ratio: 0->1, where - // 0 == beginning of soft limit - // 1 == hard/real limit - m_swingLimitRatio = 1.f; - if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON) - { - m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/ - (swingLimit - swingLimit * m_limitSoftness); - } - - // swing correction tries to get back to soft limit - m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness); - - // adjustment of swing axis (based on ellipse normal) - adjustSwingAxisToUseEllipseNormal(swingAxis); - - // Calculate necessary axis & factors - m_swingAxis = quatRotate(qB, -swingAxis); - - m_twistAxisA.setValue(0,0,0); - - m_kSwing = btScalar(1.) / - (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) + - computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB)); - } - } - else - { - // you haven't set any limits; - // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?) - // anyway, we have either hinge or fixed joint - btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); - btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); - btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0); - btVector3 target; - btScalar x = ivB.dot(ivA); - btScalar y = ivB.dot(jvA); - btScalar z = ivB.dot(kvA); - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) - { // fixed. We'll need to add one more row to constraint - if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) - { - m_solveSwingLimit = true; - m_swingAxis = -ivB.cross(ivA); - } - } - else - { - if(m_swingSpan1 < m_fixThresh) - { // hinge around Y axis - if(!(btFuzzyZero(y))) - { - m_solveSwingLimit = true; - if(m_swingSpan2 >= m_fixThresh) - { - y = btScalar(0.f); - btScalar span2 = btAtan2(z, x); - if(span2 > m_swingSpan2) - { - x = btCos(m_swingSpan2); - z = btSin(m_swingSpan2); - } - else if(span2 < -m_swingSpan2) - { - x = btCos(m_swingSpan2); - z = -btSin(m_swingSpan2); - } - } - } - } - else - { // hinge around Z axis - if(!btFuzzyZero(z)) - { - m_solveSwingLimit = true; - if(m_swingSpan1 >= m_fixThresh) - { - z = btScalar(0.f); - btScalar span1 = btAtan2(y, x); - if(span1 > m_swingSpan1) - { - x = btCos(m_swingSpan1); - y = btSin(m_swingSpan1); - } - else if(span1 < -m_swingSpan1) - { - x = btCos(m_swingSpan1); - y = -btSin(m_swingSpan1); - } - } - } - } - target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0]; - target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1]; - target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2]; - target.normalize(); - m_swingAxis = -ivB.cross(target); - m_swingCorrection = m_swingAxis.length(); - m_swingAxis.normalize(); - } - } - - if (m_twistSpan >= btScalar(0.f)) - { - btVector3 twistAxis; - computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis); - - if (m_twistAngle > m_twistSpan*m_limitSoftness) - { - m_solveTwistLimit = true; - - m_twistLimitRatio = 1.f; - if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON) - { - m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/ - (m_twistSpan - m_twistSpan * m_limitSoftness); - } - - // twist correction tries to get back to soft limit - m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness); - - m_twistAxis = quatRotate(qB, -twistAxis); - - m_kTwist = btScalar(1.) / - (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) + - computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB)); - } - - if (m_solveSwingLimit) - m_twistAxisA = quatRotate(qA, -twistAxis); - } - else - { - m_twistAngle = btScalar(0.f); - } - } -} - - - -// given a cone rotation in constraint space, (pre: twist must already be removed) -// this method computes its corresponding swing angle and axis. -// more interestingly, it computes the cone/swing limit (angle) for this cone "pose". -void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, - btScalar& swingAngle, // out - btVector3& vSwingAxis, // out - btScalar& swingLimit) // out -{ - swingAngle = qCone.getAngle(); - if (swingAngle > SIMD_EPSILON) - { - 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; - } - - // 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.) - - // For starters, compute the direction from center to surface of ellipse. - // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis. - // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.) - btScalar xEllipse = vSwingAxis.y(); - btScalar yEllipse = -vSwingAxis.z(); - - // Now, we use the slope of the vector (using x/yEllipse) and find the length - // of the line that intersects the ellipse: - // x^2 y^2 - // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) - // a^2 b^2 - // Do the math and it should be clear. - - swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1 - if (fabs(xEllipse) > SIMD_EPSILON) - { - btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); - btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); - norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); - btScalar swingLimit2 = (1 + surfaceSlope2) / norm; - swingLimit = sqrt(swingLimit2); - } - - // test! - /*swingLimit = m_swingSpan2; - if (fabs(vSwingAxis.z()) > SIMD_EPSILON) - { - btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2; - btScalar sinphi = m_swingSpan2 / sqrt(mag_2); - btScalar phi = asin(sinphi); - btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z())); - btScalar alpha = 3.14159f - theta - phi; - btScalar sinalpha = sin(alpha); - swingLimit = m_swingSpan1 * sinphi/sinalpha; - }*/ - } - else if (swingAngle < 0) - { - // this should never happen! - int wtf = 0; wtf = wtf; - } -} - -btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const -{ - // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone) - btScalar xEllipse = btCos(fAngleInRadians); - btScalar yEllipse = btSin(fAngleInRadians); - - // Use the slope of the vector (using x/yEllipse) and find the length - // of the line that intersects the ellipse: - // x^2 y^2 - // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) - // a^2 b^2 - // Do the math and it should be clear. - - float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) - if (fabs(xEllipse) > SIMD_EPSILON) - { - btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); - btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); - norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); - btScalar swingLimit2 = (1 + surfaceSlope2) / norm; - swingLimit = sqrt(swingLimit2); - } - - // convert into point in constraint space: - // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively - btVector3 vSwingAxis(0, xEllipse, -yEllipse); - btQuaternion qSwing(vSwingAxis, swingLimit); - btVector3 vPointInConstraintSpace(fLength,0,0); - return quatRotate(qSwing, vPointInConstraintSpace); -} - -// given a twist rotation in constraint space, (pre: cone must already be removed) -// this method computes its corresponding angle and axis. -void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, - btScalar& twistAngle, // out - btVector3& vTwistAxis) // out -{ - btQuaternion qMinTwist = qTwist; - twistAngle = qTwist.getAngle(); - - if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. - { - qMinTwist = operator-(qTwist); - twistAngle = qMinTwist.getAngle(); - } - if (twistAngle < 0) - { - // this should never happen - int wtf = 0; wtf = wtf; - } - - vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); - if (twistAngle > SIMD_EPSILON) - vTwistAxis.normalize(); -} - - -void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const -{ - // the swing axis is computed as the "twist-free" cone rotation, - // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2). - // so, if we're outside the limits, the closest way back inside the cone isn't - // along the vector back to the center. better (and more stable) to use the ellipse normal. - - // convert swing axis to direction from center to surface of ellipse - // (ie. rotate 2D vector by PI/2) - btScalar y = -vSwingAxis.z(); - btScalar z = vSwingAxis.y(); - - // do the math... - if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0. - { - // compute gradient/normal of ellipse surface at current "point" - btScalar grad = y/z; - grad *= m_swingSpan2 / m_swingSpan1; - - // adjust y/z to represent normal at point (instead of vector to point) - if (y > 0) - y = fabs(grad * z); - else - y = -fabs(grad * z); - - // convert ellipse direction back to swing axis - vSwingAxis.setZ(-y); - vSwingAxis.setY( z); - vSwingAxis.normalize(); - } -} - - - -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(); - - btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation(); - setMotorTargetInConstraintSpace(qConstraint); -} - - -void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q) -{ - m_qTarget = q; - - // clamp motor target to within limits - { - btScalar softness = 1.f;//m_limitSoftness; - - // split into twist and cone - btVector3 vTwisted = quatRotate(m_qTarget, vTwist); - btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize(); - btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize(); - - // clamp cone - if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f)) - { - btScalar swingAngle, swingLimit; btVector3 swingAxis; - computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit); - - if (fabs(swingAngle) > SIMD_EPSILON) - { - if (swingAngle > swingLimit*softness) - swingAngle = swingLimit*softness; - else if (swingAngle < -swingLimit*softness) - swingAngle = -swingLimit*softness; - qTargetCone = btQuaternion(swingAxis, swingAngle); - } - } - - // clamp twist - if (m_twistSpan >= btScalar(0.05f)) - { - btScalar twistAngle; btVector3 twistAxis; - computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis); - - if (fabs(twistAngle) > SIMD_EPSILON) - { - // eddy todo: limitSoftness used here??? - if (twistAngle > m_twistSpan*softness) - twistAngle = m_twistSpan*softness; - else if (twistAngle < -m_twistSpan*softness) - twistAngle = -m_twistSpan*softness; - qTargetTwist = btQuaternion(twistAxis, twistAngle); - } - } - - m_qTarget = qTargetCone * qTargetTwist; - } -} - -///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; -} - - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h deleted file mode 100644 index f310d474e7a..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ /dev/null @@ -1,332 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios - -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. - -Written by: Marcus Hennix -*/ - - - -/* -Overview: - -btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc). -It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint". -It divides the 3 rotational DOFs into swing (movement within a cone) and twist. -Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape. -(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.) - -In the contraint's frame of reference: -twist is along the x-axis, -and swing 1 and 2 are along the z and y axes respectively. -*/ - - - -#ifndef CONETWISTCONSTRAINT_H -#define CONETWISTCONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -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 -{ -#ifdef IN_PARALLELL_SOLVER -public: -#endif - btJacobianEntry m_jac[3]; //3 orthogonal linear constraints - - btTransform m_rbAFrame; - btTransform m_rbBFrame; - - btScalar m_limitSoftness; - btScalar m_biasFactor; - btScalar m_relaxationFactor; - - btScalar m_damping; - - btScalar m_swingSpan1; - btScalar m_swingSpan2; - btScalar m_twistSpan; - - btScalar m_fixThresh; - - btVector3 m_swingAxis; - btVector3 m_twistAxis; - - btScalar m_kSwing; - btScalar m_kTwist; - - btScalar m_twistLimitSign; - btScalar m_swingCorrection; - btScalar m_twistCorrection; - - btScalar m_twistAngle; - - btScalar m_accSwingLimitImpulse; - btScalar m_accTwistLimitImpulse; - - bool m_angularOnly; - bool m_solveTwistLimit; - bool m_solveSwingLimit; - - bool m_useSolveConstraintObsolete; - - // not yet used... - btScalar m_swingLimitRatio; - btScalar m_twistLimitRatio; - btVector3 m_twistAxisA; - - // motor - bool m_bMotorEnabled; - bool m_bNormalizedMotorStrength; - btQuaternion m_qTarget; - 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: - - btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame); - - btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame); - - virtual void buildJacobian(); - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual(btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); - - virtual void solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep); - - void updateRHS(btScalar timeStep); - - const btRigidBody& getRigidBodyA() const - { - return m_rbA; - } - const btRigidBody& getRigidBodyB() const - { - return m_rbB; - } - - void setAngularOnly(bool angularOnly) - { - m_angularOnly = angularOnly; - } - - void setLimit(int limitIndex,btScalar limitValue) - { - switch (limitIndex) - { - case 3: - { - m_twistSpan = limitValue; - break; - } - case 4: - { - m_swingSpan2 = limitValue; - break; - } - case 5: - { - m_swingSpan1 = limitValue; - break; - } - default: - { - } - }; - } - - // setLimit(), a few notes: - // _softness: - // 0->1, recommend ~0.8->1. - // describes % of limits where movement is free. - // beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached. - // _biasFactor: - // 0->1?, recommend 0.3 +/-0.3 or so. - // strength with which constraint resists zeroth order (angular, not angular velocity) limit violation. - // __relaxationFactor: - // 0->1, recommend to stay near 1. - // the lower the value, the less the constraint will fight velocities which violate the angular limits. - void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) - { - m_swingSpan1 = _swingSpan1; - m_swingSpan2 = _swingSpan2; - m_twistSpan = _twistSpan; - - m_limitSoftness = _softness; - m_biasFactor = _biasFactor; - m_relaxationFactor = _relaxationFactor; - } - - const btTransform& getAFrame() { return m_rbAFrame; }; - const btTransform& getBFrame() { return m_rbBFrame; }; - - inline int getSolveTwistLimit() - { - return m_solveTwistLimit; - } - - inline int getSolveSwingLimit() - { - return m_solveTwistLimit; - } - - inline btScalar getTwistLimitSign() - { - return m_twistLimitSign; - } - - void calcAngleInfo(); - void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); - - inline btScalar getSwingSpan1() - { - return m_swingSpan1; - } - inline btScalar getSwingSpan2() - { - return m_swingSpan2; - } - inline btScalar getTwistSpan() - { - return m_twistSpan; - } - inline btScalar getTwistAngle() - { - return m_twistAngle; - } - bool isPastSwingLimit() { return m_solveSwingLimit; } - - - void setDamping(btScalar damping) { m_damping = damping; } - - void enableMotor(bool b) { m_bMotorEnabled = b; } - void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; } - void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; } - - btScalar getFixThresh() { return m_fixThresh; } - void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; } - - // setMotorTarget: - // q: the desired rotation of bodyA wrt bodyB. - // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability) - // note: don't forget to enableMotor() - void setMotorTarget(const btQuaternion &q); - - // same as above, but q is the desired rotation of frameA wrt frameB in constraint space - void setMotorTargetInConstraintSpace(const btQuaternion &q); - - 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); - ///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 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 //CONETWISTCONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btConstraintSolver.h deleted file mode 100644 index 7a8e9c1953d..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ /dev/null @@ -1,52 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 CONSTRAINT_SOLVER_H -#define CONSTRAINT_SOLVER_H - -#include "LinearMath/btScalar.h" - -class btPersistentManifold; -class btRigidBody; -class btCollisionObject; -class btTypedConstraint; -struct btContactSolverInfo; -struct btBroadphaseProxy; -class btIDebugDraw; -class btStackAlloc; -class btDispatcher; -/// btConstraintSolver provides solver interface -class btConstraintSolver -{ - -public: - - virtual ~btConstraintSolver() {} - - virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;} - - ///solve a group of constraints - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0; - - virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */, btStackAlloc* /* stackAlloc */) {;} - - ///clear internal cached data and reset random seed - virtual void reset() = 0; -}; - - - - -#endif //CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.cpp deleted file mode 100644 index d97096d9f26..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 "btContactConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btContactSolverInfo.h" -#include "LinearMath/btMinMax.h" -#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" - - - -btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB) -:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB), - m_contactManifold(*contactManifold) -{ - -} - -btContactConstraint::~btContactConstraint() -{ - -} - -void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold) -{ - m_contactManifold = *contactManifold; -} - -void btContactConstraint::getInfo1 (btConstraintInfo1* info) -{ - -} - -void btContactConstraint::getInfo2 (btConstraintInfo2* info) -{ - -} - -void btContactConstraint::buildJacobian() -{ - -} - - - - - -#include "btContactConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btContactSolverInfo.h" -#include "LinearMath/btMinMax.h" -#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" - -#define ASSERT2 btAssert - -#define USE_INTERNAL_APPLY_IMPULSE 1 - - -//bilateral constraint between two dynamic objects -void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, - btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep) -{ - (void)timeStep; - (void)distance; - - - btScalar normalLenSqr = normal.length2(); - ASSERT2(btFabs(normalLenSqr) < btScalar(1.1)); - if (normalLenSqr > btScalar(1.1)) - { - impulse = btScalar(0.); - return; - } - btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); - //this jacobian entry could be re-used for all iterations - - btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - - btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), - body2.getCenterOfMassTransform().getBasis().transpose(), - rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), - body2.getInvInertiaDiagLocal(),body2.getInvMass()); - - btScalar jacDiagAB = jac.getDiagonal(); - btScalar jacDiagABInv = btScalar(1.) / jacDiagAB; - - btScalar rel_vel = jac.getRelativeVelocity( - body1.getLinearVelocity(), - body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(), - body2.getLinearVelocity(), - body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); - btScalar a; - a=jacDiagABInv; - - - rel_vel = normal.dot(vel); - - //todo: move this into proper structure - btScalar contactDamping = btScalar(0.2); - -#ifdef ONLY_USE_LINEAR_MASS - btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass()); - impulse = - contactDamping * rel_vel * massTerm; -#else - btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; - impulse = velocityImpulse; -#endif -} - - - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.h deleted file mode 100644 index 63c1a417bc1..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ /dev/null @@ -1,68 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 CONTACT_CONSTRAINT_H -#define CONTACT_CONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" - -///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface -ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint -{ -protected: - - btPersistentManifold m_contactManifold; - -public: - - - btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB); - - void setContactManifold(btPersistentManifold* contactManifold); - - btPersistentManifold* getContactManifold() - { - return &m_contactManifold; - } - - const btPersistentManifold* getContactManifold() const - { - return &m_contactManifold; - } - - virtual ~btContactConstraint(); - - virtual void getInfo1 (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - ///obsolete methods - virtual void buildJacobian(); - - -}; - - -///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects -void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, - btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep); - - - -#endif //CONTACT_CONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btContactSolverInfo.h deleted file mode 100644 index 1025da42f5e..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ /dev/null @@ -1,87 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 CONTACT_SOLVER_INFO -#define CONTACT_SOLVER_INFO - -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. -}; - -struct btContactSolverInfoData -{ - - - btScalar m_tau; - btScalar m_damping; - btScalar m_friction; - btScalar m_timeStep; - btScalar m_restitution; - int m_numIterations; - btScalar m_maxErrorReduction; - btScalar m_sor; - btScalar m_erp;//used as Baumgarte factor - btScalar m_erp2;//used in Split Impulse - btScalar m_globalCfm;//constraint force mixing - int m_splitImpulse; - btScalar m_splitImpulsePenetrationThreshold; - btScalar m_linearSlop; - btScalar m_warmstartingFactor; - - int m_solverMode; - int m_restingContactRestitutionThreshold; - int m_minimumSolverBatchSize; - - -}; - -struct btContactSolverInfo : public btContactSolverInfoData -{ - - - - inline btContactSolverInfo() - { - m_tau = btScalar(0.6); - m_damping = btScalar(1.0); - m_friction = btScalar(0.3); - m_restitution = btScalar(0.); - m_maxErrorReduction = btScalar(20.); - m_numIterations = 10; - m_erp = btScalar(0.2); - m_erp2 = btScalar(0.1); - m_globalCfm = btScalar(0.); - m_sor = btScalar(1.); - m_splitImpulse = false; - m_splitImpulsePenetrationThreshold = -0.02f; - m_linearSlop = btScalar(0.0); - m_warmstartingFactor=btScalar(0.85); - m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER; - m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution - m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit - } -}; - -#endif //CONTACT_SOLVER_INFO diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp deleted file mode 100644 index a970d706284..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ /dev/null @@ -1,1012 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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. -*/ -/* -2007-09-09 -Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - -#include "btGeneric6DofConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btTransformUtil.h" -#include <new> - - - -#define D6_USE_OBSOLETE_METHOD false -#define D6_USE_FRAME_OFFSET true - - - - - - -btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) -: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) -, 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 - - - -btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); -btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) -{ - int i = index%3; - int j = index/3; - return mat[i][j]; -} - - - -///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html -bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); -bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) -{ - // // rot = cy*cz -cy*sz sy - // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx - // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy - // - - btScalar fi = btGetMatrixElem(mat,2); - if (fi < btScalar(1.0f)) - { - if (fi > btScalar(-1.0f)) - { - xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); - xyz[1] = btAsin(btGetMatrixElem(mat,2)); - xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); - return true; - } - else - { - // WARNING. Not unique. XA - ZA = -atan2(r10,r11) - xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); - xyz[1] = -SIMD_HALF_PI; - xyz[2] = btScalar(0.0); - return false; - } - } - else - { - // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) - xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); - xyz[1] = SIMD_HALF_PI; - xyz[2] = 0.0; - } - return false; -} - -//////////////////////////// btRotationalLimitMotor //////////////////////////////////// - -int btRotationalLimitMotor::testLimitValue(btScalar test_value) -{ - if(m_loLimit>m_hiLimit) - { - m_currentLimit = 0;//Free from violation - return 0; - } - if (test_value < m_loLimit) - { - m_currentLimit = 1;//low limit violation - m_currentLimitError = test_value - m_loLimit; - return 1; - } - else if (test_value> m_hiLimit) - { - m_currentLimit = 2;//High limit violation - m_currentLimitError = test_value - m_hiLimit; - return 2; - }; - - m_currentLimit = 0;//Free from violation - return 0; - -} - - - -btScalar btRotationalLimitMotor::solveAngularLimits( - btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, - btRigidBody * body0, btRigidBody * body1 ) -{ - if (needApplyTorques()==false) return 0.0f; - - btScalar target_velocity = m_targetVelocity; - btScalar maxMotorForce = m_maxMotorForce; - - //current error correction - if (m_currentLimit!=0) - { - target_velocity = -m_stopERP*m_currentLimitError/(timeStep); - maxMotorForce = m_maxLimitForce; - } - - maxMotorForce *= timeStep; - - // current velocity difference - - btVector3 angVelA; - body0->internalGetAngularVelocity(angVelA); - btVector3 angVelB; - body1->internalGetAngularVelocity(angVelB); - - btVector3 vel_diff; - vel_diff = angVelA-angVelB; - - - - btScalar rel_vel = axis.dot(vel_diff); - - // correction velocity - btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); - - - if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) - { - return 0.0f;//no need for applying force - } - - - // correction impulse - btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; - - // clip correction impulse - btScalar clippedMotorImpulse; - - ///@todo: should clip against accumulated impulse - if (unclippedMotorImpulse>0.0f) - { - clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; - } - else - { - clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; - } - - - // sort with accumulated impulses - btScalar lo = btScalar(-BT_LARGE_FLOAT); - btScalar hi = btScalar(BT_LARGE_FLOAT); - - btScalar oldaccumImpulse = m_accumulatedImpulse; - btScalar sum = oldaccumImpulse + clippedMotorImpulse; - m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; - - clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; - - btVector3 motorImp = clippedMotorImpulse * axis; - - //body0->applyTorqueImpulse(motorImp); - //body1->applyTorqueImpulse(-motorImp); - - body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); - body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); - - - return clippedMotorImpulse; - - -} - -//////////////////////////// End btRotationalLimitMotor //////////////////////////////////// - - - - -//////////////////////////// btTranslationalLimitMotor //////////////////////////////////// - - -int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value) -{ - btScalar loLimit = m_lowerLimit[limitIndex]; - btScalar hiLimit = m_upperLimit[limitIndex]; - if(loLimit > hiLimit) - { - m_currentLimit[limitIndex] = 0;//Free from violation - m_currentLimitError[limitIndex] = btScalar(0.f); - return 0; - } - - if (test_value < loLimit) - { - m_currentLimit[limitIndex] = 2;//low limit violation - m_currentLimitError[limitIndex] = test_value - loLimit; - return 2; - } - else if (test_value> hiLimit) - { - m_currentLimit[limitIndex] = 1;//High limit violation - m_currentLimitError[limitIndex] = test_value - hiLimit; - return 1; - }; - - m_currentLimit[limitIndex] = 0;//Free from violation - m_currentLimitError[limitIndex] = btScalar(0.f); - return 0; -} - - - -btScalar btTranslationalLimitMotor::solveLinearAxis( - btScalar timeStep, - btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, - int limit_index, - const btVector3 & axis_normal_on_a, - const btVector3 & anchorPos) -{ - - ///find relative velocity - // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); - // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); - btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); - - btVector3 vel1; - body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); - btVector3 vel2; - body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); - btVector3 vel = vel1 - vel2; - - btScalar rel_vel = axis_normal_on_a.dot(vel); - - - - /// apply displacement correction - - //positional error (zeroth order error) - btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); - btScalar lo = btScalar(-BT_LARGE_FLOAT); - btScalar hi = btScalar(BT_LARGE_FLOAT); - - btScalar minLimit = m_lowerLimit[limit_index]; - btScalar maxLimit = m_upperLimit[limit_index]; - - //handle the limits - if (minLimit < maxLimit) - { - { - if (depth > maxLimit) - { - depth -= maxLimit; - lo = btScalar(0.); - - } - else - { - if (depth < minLimit) - { - depth -= minLimit; - hi = btScalar(0.); - } - else - { - return 0.0f; - } - } - } - } - - btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; - - - - - btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; - btScalar sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; - 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); - body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); - body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); - - - - - return normalImpulse; -} - -//////////////////////////// btTranslationalLimitMotor //////////////////////////////////// - -void btGeneric6DofConstraint::calculateAngleInfo() -{ - btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); - matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); - // in euler angle mode we do not actually constrain the angular velocity - // along the axes axis[0] and axis[2] (although we do use axis[1]) : - // - // to get constrain w2-w1 along ...not - // ------ --------------------- ------ - // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] - // d(angle[1])/dt = 0 ax[1] - // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] - // - // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. - // to prove the result for angle[0], write the expression for angle[0] from - // GetInfo1 then take the derivative. to prove this for angle[2] it is - // easier to take the euler rate expression for d(angle[2])/dt with respect - // to the components of w and set that to 0. - btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); - btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); - - m_calculatedAxis[1] = axis2.cross(axis0); - m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); - m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); - - m_calculatedAxis[0].normalize(); - m_calculatedAxis[1].normalize(); - m_calculatedAxis[2].normalize(); - -} - -void btGeneric6DofConstraint::calculateTransforms() -{ - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); -} - -void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) -{ - m_calculatedTransformA = transA * m_frameInA; - 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; - } -} - - - -void btGeneric6DofConstraint::buildLinearJacobian( - btJacobianEntry & jacLinear,const btVector3 & normalWorld, - const btVector3 & pivotAInW,const btVector3 & pivotBInW) -{ - new (&jacLinear) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normalWorld, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); -} - - - -void btGeneric6DofConstraint::buildAngularJacobian( - btJacobianEntry & jacAngular,const btVector3 & jointAxisW) -{ - new (&jacAngular) btJacobianEntry(jointAxisW, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - -} - - - -bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) -{ - btScalar angle = m_calculatedAxisAngleDiff[axis_index]; - angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit); - m_angularLimits[axis_index].m_currentPosition = angle; - //test limits - m_angularLimits[axis_index].testLimitValue(angle); - return m_angularLimits[axis_index].needApplyTorques(); -} - - - -void btGeneric6DofConstraint::buildJacobian() -{ -#ifndef __SPU__ - if (m_useSolveConstraintObsolete) - { - - // Clear accumulated impulses for the next simulation step - m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); - int i; - for(i = 0; i < 3; i++) - { - m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); - } - //calculates transform - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - - // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); - // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); - calcAnchorPos(); - btVector3 pivotAInW = m_AnchorPos; - btVector3 pivotBInW = m_AnchorPos; - - // not used here - // btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - // btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 normalWorld; - //linear part - for (i=0;i<3;i++) - { - if (m_linearLimits.isLimited(i)) - { - if (m_useLinearReferenceFrameA) - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - else - normalWorld = m_calculatedTransformB.getBasis().getColumn(i); - - buildLinearJacobian( - m_jacLinear[i],normalWorld , - pivotAInW,pivotBInW); - - } - } - - // angular part - for (i=0;i<3;i++) - { - //calculates error angle - if (testAngularLimitMotor(i)) - { - normalWorld = this->getAxis(i); - // Create angular atom - buildAngularJacobian(m_jacAng[i],normalWorld); - } - } - - } -#endif //__SPU__ - -} - - -void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } else - { - //prepare constraint - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - info->m_numConstraintRows = 0; - info->nub = 6; - int i; - //test linear limits - for(i = 0; i < 3; i++) - { - if(m_linearLimits.needApplyForce(i)) - { - info->m_numConstraintRows++; - info->nub--; - } - } - //test angular limits - for (i=0;i<3 ;i++ ) - { - if(testAngularLimitMotor(i)) - { - info->m_numConstraintRows++; - info->nub--; - } - } - } -} - -void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } else - { - //pre-allocate all 6 - info->m_numConstraintRows = 6; - info->nub = 0; - } -} - - -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()); -} - -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); - 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, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) -{ -// int row = 0; - //solve linear limits - btRotationalLimitMotor limot; - for (int i=0;i<3 ;i++ ) - { - if(m_linearLimits.needApplyForce(i)) - { // re-use rotational motor code - limot.m_bounce = btScalar(0.f); - limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; - limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i]; - 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_hiLimit = m_linearLimits.m_upperLimit[i]; - limot.m_limitSoftness = m_linearLimits.m_limitSoftness; - limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; - limot.m_maxLimitForce = btScalar(0.f); - limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; - limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; - btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); - 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; -} - - - -int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) -{ - btGeneric6DofConstraint * d6constraint = this; - int row = row_offset; - //solve angular limits - for (int i=0;i<3 ;i++ ) - { - if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) - { - btVector3 axis = d6constraint->getAxis(i); - 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); - } - } - - return row; -} - - - - -void btGeneric6DofConstraint::updateRHS(btScalar timeStep) -{ - (void)timeStep; - -} - - - -btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const -{ - return m_calculatedAxis[axis_index]; -} - - -btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const -{ - return m_calculatedLinearDiff[axisIndex]; -} - - -btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const -{ - return m_calculatedAxisAngleDiff[axisIndex]; -} - - - -void btGeneric6DofConstraint::calcAnchorPos(void) -{ - btScalar imA = m_rbA.getInvMass(); - btScalar imB = m_rbB.getInvMass(); - btScalar weight; - if(imB == btScalar(0.0)) - { - weight = btScalar(1.0); - } - else - { - weight = imA / (imA + imB); - } - const btVector3& pA = m_calculatedTransformA.getOrigin(); - const btVector3& pB = m_calculatedTransformB.getOrigin(); - m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight); - return; -} - - - -void btGeneric6DofConstraint::calculateLinearInfo() -{ - m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin(); - m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff; - for(int i = 0; i < 3; i++) - { - m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i]; - m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]); - } -} - - - -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,int rotAllowed) -{ - int srow = row * info->rowskip; - int powered = limot->m_enableMotor; - int limit = limot->m_currentLimit; - if (powered || limit) - { // if the joint is powered, or has joint limits, add in the extra row - btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; - btScalar *J2 = rotational ? info->m_J2angularAxis : 0; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - if(rotational) - { - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - } - if((!rotational)) - { - 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]; - } - } - // if we're limited low and high simultaneously, the joint motor is - // ineffective - if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0; - info->m_constraintError[srow] = btScalar(0.f); - if (powered) - { - info->cfm[srow] = limot->m_normalCFM; - if(!limit) - { - btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; - - btScalar mot_fact = getMotorFactor( limot->m_currentPosition, - limot->m_loLimit, - limot->m_hiLimit, - tag_vel, - 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; - } - } - if(limit) - { - btScalar k = info->fps * limot->m_stopERP; - if(!rotational) - { - info->m_constraintError[srow] += k * limot->m_currentLimitError; - } - else - { - info->m_constraintError[srow] += -k * limot->m_currentLimitError; - } - info->cfm[srow] = limot->m_stopCFM; - if (limot->m_loLimit == limot->m_hiLimit) - { // limited low and high simultaneously - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - if (limit == 1) - { - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - // deal with bounce - if (limot->m_bounce > 0) - { - // calculate joint velocity - btScalar vel; - if (rotational) - { - vel = angVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= angVelB.dot(ax1); - } - else - { - vel = linVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= linVelB.dot(ax1); - } - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if (limit == 1) - { - if (vel < 0) - { - btScalar newc = -limot->m_bounce* vel; - if (newc > info->m_constraintError[srow]) - info->m_constraintError[srow] = newc; - } - } - else - { - if (vel > 0) - { - btScalar newc = -limot->m_bounce * vel; - if (newc < info->m_constraintError[srow]) - info->m_constraintError[srow] = newc; - } - } - } - } - } - return 1; - } - else return 0; -} - - - - - - - ///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; -} diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h deleted file mode 100644 index 2653d26dc3f..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ /dev/null @@ -1,588 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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. -*/ - -/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev -/// Added support for generic constraint solver through getInfo1/getInfo2 methods - -/* -2007-09-09 -btGeneric6DofConstraint Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - - -#ifndef GENERIC_6DOF_CONSTRAINT_H -#define GENERIC_6DOF_CONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -class btRigidBody; - - - - -//! Rotation Limit structure for generic joints -class btRotationalLimitMotor -{ -public: - //! limit_parameters - //!@{ - btScalar m_loLimit;//!< joint limit - btScalar m_hiLimit;//!< joint limit - btScalar m_targetVelocity;//!< target motor velocity - btScalar m_maxMotorForce;//!< max force on motor - btScalar m_maxLimitForce;//!< max force on limit - btScalar m_damping;//!< Damping. - btScalar m_limitSoftness;//! Relaxation factor - 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; - - //!@} - - //! temp_variables - //!@{ - btScalar m_currentLimitError;//! How much is violated this limit - btScalar m_currentPosition; //! current value of angle - int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit - btScalar m_accumulatedImpulse; - //!@} - - btRotationalLimitMotor() - { - m_accumulatedImpulse = 0.f; - m_targetVelocity = 0; - m_maxMotorForce = 0.1f; - m_maxLimitForce = 300.0f; - m_loLimit = 1.0f; - m_hiLimit = -1.0f; - m_normalCFM = 0.f; - m_stopERP = 0.2f; - m_stopCFM = 0.f; - m_bounce = 0.0f; - m_damping = 1.0f; - m_limitSoftness = 0.5f; - m_currentLimit = 0; - m_currentLimitError = 0; - m_enableMotor = false; - } - - btRotationalLimitMotor(const btRotationalLimitMotor & limot) - { - m_targetVelocity = limot.m_targetVelocity; - m_maxMotorForce = limot.m_maxMotorForce; - m_limitSoftness = limot.m_limitSoftness; - m_loLimit = limot.m_loLimit; - m_hiLimit = limot.m_hiLimit; - 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; - m_enableMotor = limot.m_enableMotor; - } - - - - //! Is limited - bool isLimited() - { - if(m_loLimit > m_hiLimit) return false; - return true; - } - - //! Need apply correction - bool needApplyTorques() - { - if(m_currentLimit == 0 && m_enableMotor == false) return false; - return true; - } - - //! calculates error - /*! - calculates m_currentLimit and m_currentLimitError. - */ - int testLimitValue(btScalar test_value); - - //! apply the correction impulses for two bodies - btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); - -}; - - - -class btTranslationalLimitMotor -{ -public: - btVector3 m_lowerLimit;//!< the constraint lower limits - btVector3 m_upperLimit;//!< the constraint upper limits - btVector3 m_accumulatedImpulse; - //! Linear_Limit_parameters - //!@{ - 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 - btVector3 m_maxMotorForce;//!< max force on motor - btVector3 m_currentLimitError;//! How much is violated this limit - btVector3 m_currentLinearDiff;//! Current relative offset of constraint frames - int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit - - btTranslationalLimitMotor() - { - 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); - m_restitution = btScalar(0.5f); - for(int i=0; i < 3; i++) - { - m_enableMotor[i] = false; - m_targetVelocity[i] = btScalar(0.f); - m_maxMotorForce[i] = btScalar(0.f); - } - } - - btTranslationalLimitMotor(const btTranslationalLimitMotor & other ) - { - m_lowerLimit = other.m_lowerLimit; - m_upperLimit = other.m_upperLimit; - m_accumulatedImpulse = other.m_accumulatedImpulse; - - 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]; - m_targetVelocity[i] = other.m_targetVelocity[i]; - m_maxMotorForce[i] = other.m_maxMotorForce[i]; - } - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - inline bool isLimited(int limitIndex) - { - return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); - } - inline bool needApplyForce(int limitIndex) - { - if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false; - return true; - } - int testLimitValue(int limitIndex, btScalar test_value); - - - btScalar solveLinearAxis( - btScalar timeStep, - btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, - int limit_index, - const btVector3 & axis_normal_on_a, - const btVector3 & anchorPos); - - -}; - -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'. -currently this limit supports rotational motors<br> -<ul> -<li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method. -At this moment translational motors are not supported. May be in the future. </li> - -<li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit. -This is accessible through btGeneric6DofConstraint.getLimitMotor method, -This brings support for limit parameters and motors. </li> - -<li> Angulars limits have these possible ranges: -<table border=1 > -<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> -</tr><tr> - <td>Y</td> - <td>-PI/2</td> - <td>PI/2</td> -</tr><tr> - <td>Z</td> - <td>-PI</td> - <td>PI</td> -</tr> -</table> -</li> -</ul> - -*/ -class btGeneric6DofConstraint : public btTypedConstraint -{ -protected: - - //! relative_frames - //!@{ - btTransform m_frameInA;//!< the constraint space w.r.t body A - btTransform m_frameInB;//!< the constraint space w.r.t body B - //!@} - - //! Jacobians - //!@{ - btJacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints - btJacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints - //!@} - - //! Linear_Limit_parameters - //!@{ - btTranslationalLimitMotor m_linearLimits; - //!@} - - - //! hinge_parameters - //!@{ - btRotationalLimitMotor m_angularLimits[3]; - //!@} - - -protected: - //! temporal variables - //!@{ - btScalar m_timeStep; - btTransform m_calculatedTransformA; - btTransform m_calculatedTransformB; - 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) - { - btAssert(0); - (void) other; - return *this; - } - - - 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, 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, - const btVector3 & pivotAInW,const btVector3 & pivotBInW); - - void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW); - - // tests linear limits - void calculateLinearInfo(); - - //! calcs the euler angles between the two bodies. - void calculateAngleInfo(); - - - -public: - - ///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(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. - \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo - */ - void calculateTransforms(const btTransform& transA,const btTransform& transB); - - void calculateTransforms(); - - //! Gets the global transform of the offset for body A - /*! - \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo. - */ - const btTransform & getCalculatedTransformA() const - { - return m_calculatedTransformA; - } - - //! Gets the global transform of the offset for body B - /*! - \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo. - */ - const btTransform & getCalculatedTransformB() const - { - return m_calculatedTransformB; - } - - const btTransform & getFrameOffsetA() const - { - return m_frameInA; - } - - const btTransform & getFrameOffsetB() const - { - return m_frameInB; - } - - - btTransform & getFrameOffsetA() - { - return m_frameInA; - } - - btTransform & getFrameOffsetB() - { - return m_frameInB; - } - - - //! performs Jacobian calculation, and also calculates angle differences and axis - virtual void buildJacobian(); - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); - - - void updateRHS(btScalar timeStep); - - //! Get the rotation axis in global coordinates - /*! - \pre btGeneric6DofConstraint.buildJacobian must be called previously. - */ - btVector3 getAxis(int axis_index) const; - - //! Get the relative Euler angle - /*! - \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. - */ - btScalar getAngle(int axis_index) const; - - //! Get the relative position of the constraint pivot - /*! - \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. - */ - btScalar getRelativePivotPosition(int axis_index) const; - - - //! Test angular limit. - /*! - Calculates angular correction and returns true if limit needs to be corrected. - \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. - */ - bool testAngularLimitMotor(int axis_index); - - void setLinearLowerLimit(const btVector3& linearLower) - { - m_linearLimits.m_lowerLimit = linearLower; - } - - void setLinearUpperLimit(const btVector3& linearUpper) - { - m_linearLimits.m_upperLimit = linearUpper; - } - - void setAngularLowerLimit(const btVector3& angularLower) - { - for(int i = 0; i < 3; i++) - m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); - } - - void setAngularUpperLimit(const btVector3& angularUpper) - { - for(int i = 0; i < 3; i++) - m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); - } - - //! Retrieves the angular limit informacion - btRotationalLimitMotor * getRotationalLimitMotor(int index) - { - return &m_angularLimits[index]; - } - - //! Retrieves the limit informacion - btTranslationalLimitMotor * getTranslationalLimitMotor() - { - return &m_linearLimits; - } - - //first 3 are linear, next 3 are angular - void setLimit(int axis, btScalar lo, btScalar hi) - { - if(axis<3) - { - m_linearLimits.m_lowerLimit[axis] = lo; - m_linearLimits.m_upperLimit[axis] = hi; - } - else - { - lo = btNormalizeAngle(lo); - hi = btNormalizeAngle(hi); - m_angularLimits[axis-3].m_loLimit = lo; - m_angularLimits[axis-3].m_hiLimit = hi; - } - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - bool isLimited(int limitIndex) - { - if(limitIndex<3) - { - return m_linearLimits.isLimited(limitIndex); - - } - return m_angularLimits[limitIndex-3].isLimited(); - } - - virtual void calcAnchorPos(void); // overridable - - 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, 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; - - 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; - - 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 //GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp deleted file mode 100644 index 3fa7de4ddb8..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -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 "btGeneric6DofSpringConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" - - -btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA) - : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA) -{ - for(int i = 0; i < 6; i++) - { - m_springEnabled[i] = false; - m_equilibriumPoint[i] = btScalar(0.f); - m_springStiffness[i] = btScalar(0.f); - m_springDamping[i] = btScalar(1.f); - } -} - - -void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff) -{ - btAssert((index >= 0) && (index < 6)); - m_springEnabled[index] = onOff; - if(index < 3) - { - m_linearLimits.m_enableMotor[index] = onOff; - } - else - { - m_angularLimits[index - 3].m_enableMotor = onOff; - } -} - - - -void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness) -{ - btAssert((index >= 0) && (index < 6)); - m_springStiffness[index] = stiffness; -} - - -void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping) -{ - btAssert((index >= 0) && (index < 6)); - m_springDamping[index] = damping; -} - - -void btGeneric6DofSpringConstraint::setEquilibriumPoint() -{ - calculateTransforms(); - int i; - - for( i = 0; i < 3; i++) - { - m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; - } - for(i = 0; i < 3; i++) - { - m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i]; - } -} - - - -void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index) -{ - btAssert((index >= 0) && (index < 6)); - calculateTransforms(); - if(index < 3) - { - m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; - } - else - { - m_equilibriumPoint[index] = m_calculatedAxisAngleDiff[index - 3]; - } -} - - - -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(); - for(i = 0; i < 3; i++) - { - if(m_springEnabled[i]) - { - // get current position of constraint - btScalar currPos = m_calculatedLinearDiff[i]; - // calculate difference - btScalar delta = currPos - m_equilibriumPoint[i]; - // spring force is (delta * m_stiffness) according to Hooke's Law - btScalar force = delta * m_springStiffness[i]; - btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations); - m_linearLimits.m_targetVelocity[i] = velFactor * force; - m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps; - } - } - for(i = 0; i < 3; i++) - { - if(m_springEnabled[i + 3]) - { - // get current position of constraint - btScalar currPos = m_calculatedAxisAngleDiff[i]; - // calculate difference - btScalar delta = currPos - m_equilibriumPoint[i+3]; - // spring force is (-delta * m_stiffness) according to Hooke's Law - btScalar force = -delta * m_springStiffness[i+3]; - btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations); - m_angularLimits[i].m_targetVelocity = velFactor * force; - m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps; - } - } -} - - -void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info) -{ - // this will be called by constraint solver at the constraint setup stage - // set current motor parameters - internalUpdateSprings(info); - // do the rest of job for constraint setup - btGeneric6DofConstraint::getInfo2(info); -} - - - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h deleted file mode 100644 index e0c1fc9ae39..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h +++ /dev/null @@ -1,54 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -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 GENERIC_6DOF_SPRING_CONSTRAINT_H -#define GENERIC_6DOF_SPRING_CONSTRAINT_H - - -#include "LinearMath/btVector3.h" -#include "btTypedConstraint.h" -#include "btGeneric6DofConstraint.h" - - -/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF - -/// DOF index used in enableSpring() and setStiffness() means: -/// 0 : translation X -/// 1 : translation Y -/// 2 : translation Z -/// 3 : rotation X (3rd Euler rotational around new position of X axis, range [-PI+epsilon, PI-epsilon] ) -/// 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 -{ -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 internalUpdateSprings(btConstraintInfo2* info); -public: - btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - 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 - virtual void getInfo2 (btConstraintInfo2* info); -}; - -#endif // GENERIC_6DOF_SPRING_CONSTRAINT_H - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp deleted file mode 100644 index 29123d526b4..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -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 "btHinge2Constraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" - - - -// constructor -// anchor, axis1 and axis2 are in world coordinate system -// axis1 must be orthogonal to axis2 -btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) -: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), - m_anchor(anchor), - m_axis1(axis1), - m_axis2(axis2) -{ - // build frame basis - // 6DOF constraint uses Euler angles and to define limits - // it is assumed that rotational order is : - // Z - first, allowed limits are (-PI,PI); - // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number - // used to prevent constraint from instability on poles; - // 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 xAxis = axis2.normalize(); - btVector3 yAxis = zAxis.cross(xAxis); // 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(anchor); - // now get constraint frame in local coordinate systems - m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; - m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; - // sei limits - setLinearLowerLimit(btVector3(0.f, 0.f, -1.f)); - setLinearUpperLimit(btVector3(0.f, 0.f, 1.f)); - // like front wheels of a car - setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f)); - setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f)); - // enable suspension - enableSpring(2, true); - setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-) - setDamping(2, 0.01f); - setEquilibriumPoint(); -} - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.h deleted file mode 100644 index 15fd4a014cc..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.h +++ /dev/null @@ -1,58 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -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 HINGE2_CONSTRAINT_H -#define HINGE2_CONSTRAINT_H - - - -#include "LinearMath/btVector3.h" -#include "btTypedConstraint.h" -#include "btGeneric6DofSpringConstraint.h" - - - -// Constraint similar to ODE Hinge2 Joint -// has 3 degrees of frredom: -// 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 -{ -protected: - btVector3 m_anchor; - btVector3 m_axis1; - btVector3 m_axis2; -public: - // constructor - // anchor, axis1 and axis2 are in world coordinate system - // axis1 must be orthogonal to axis2 - btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2); - // access - const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); } - const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); } - const btVector3& getAxis1() { return m_axis1; } - const btVector3& getAxis2() { return m_axis2; } - btScalar getAngle1() { return getAngle(2); } - btScalar getAngle2() { return getAngle(0); } - // limits - void setUpperLimit(btScalar ang1max) { setAngularUpperLimit(btVector3(-1.f, 0.f, ang1max)); } - void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3( 1.f, 0.f, ang1min)); } -}; - - - -#endif // HINGE2_CONSTRAINT_H - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp deleted file mode 100644 index 2bfa55a2933..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ /dev/null @@ -1,992 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 "btHingeConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btMinMax.h" -#include <new> -#include "btSolverBody.h" - - - -//#define HINGE_USE_OBSOLETE_SOLVER false -#define HINGE_USE_OBSOLETE_SOLVER false - -#define HINGE_USE_FRAME_OFFSET true - -#ifndef __SPU__ - - - - - -btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, - btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA) - :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), - m_angularOnly(false), - m_enableAngularMotor(false), - m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), - m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), - m_useReferenceFrameA(useReferenceFrameA), - m_flags(0) -{ - m_rbAFrame.getOrigin() = pivotInA; - - // since no frame is given, assume this to be zero angle and just pick rb transform axis - btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); - - btVector3 rbAxisA2; - btScalar projection = axisInA.dot(rbAxisA1); - if (projection >= 1.0f - SIMD_EPSILON) { - rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); - rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); - } else if (projection <= -1.0f + SIMD_EPSILON) { - rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); - rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); - } else { - rbAxisA2 = axisInA.cross(rbAxisA1); - rbAxisA1 = rbAxisA2.cross(axisInA); - } - - m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), - rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), - rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); - - btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); - btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); - btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); - - m_rbBFrame.getOrigin() = pivotInB; - m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), - rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), - rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); - - //start with free - m_lowerLimit = btScalar(1.0f); - m_upperLimit = btScalar(-1.0f); - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; - 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), -m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), -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 - // fixed axis in worldspace - btVector3 rbAxisA1, rbAxisA2; - btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); - - m_rbAFrame.getOrigin() = pivotInA; - m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), - rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), - rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); - - btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; - - btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); - btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); - btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); - - - m_rbBFrame.getOrigin() = 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() ); - - //start with free - m_lowerLimit = btScalar(1.0f); - m_upperLimit = btScalar(-1.0f); - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; - m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); -} - - - -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), -m_angularOnly(false), -m_enableAngularMotor(false), -m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), -m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), -m_useReferenceFrameA(useReferenceFrameA), -m_flags(0) -{ - //start with free - m_lowerLimit = btScalar(1.0f); - m_upperLimit = btScalar(-1.0f); - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; - m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); -} - - - -btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) -:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), -m_angularOnly(false), -m_enableAngularMotor(false), -m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), -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()); - - //start with free - m_lowerLimit = btScalar(1.0f); - m_upperLimit = btScalar(-1.0f); - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; - m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); -} - - - -void btHingeConstraint::buildJacobian() -{ - if (m_useSolveConstraintObsolete) - { - m_appliedImpulse = btScalar(0.); - m_accMotorImpulse = btScalar(0.); - - if (!m_angularOnly) - { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 relPos = pivotBInW - pivotAInW; - - btVector3 normal[3]; - if (relPos.length2() > SIMD_EPSILON) - { - normal[0] = relPos.normalized(); - } - else - { - normal[0].setValue(btScalar(1.0),0,0); - } - - btPlaneSpace1(normal[0], normal[1], normal[2]); - - for (int i=0;i<3;i++) - { - new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normal[i], - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - } - } - - //calculate two perpendicular jointAxis, orthogonal to hingeAxis - //these two jointAxis require equal angular velocities for both bodies - - //this is unused for now, it's a todo - btVector3 jointAxis0local; - btVector3 jointAxis1local; - - btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); - - btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; - btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; - btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - - new (&m_jacAng[0]) btJacobianEntry(jointAxis0, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - new (&m_jacAng[1]) btJacobianEntry(jointAxis1, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - // clear accumulator - m_accLimitImpulse = btScalar(0.); - - // test angular limit - testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - - //Compute K = J*W*J' for hinge axis - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + - getRigidBodyB().computeAngularImpulseDenominator(axisA)); - - } -} - - -#endif //__SPU__ - - -void btHingeConstraint::getInfo1(btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } - else - { - info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular - info->nub = 1; - //always add the row, to avoid computation (data is not available yet) - //prepare constraint - testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - if(getSolveLimit() || getEnableAngularMotor()) - { - info->m_numConstraintRows++; // limit 3rd anguar as well - info->nub--; - } - - } -} - -void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } - else - { - //always add the 'limit' row, to avoid computation (data is not available yet) - info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular - info->nub = 0; - } -} - -void btHingeConstraint::getInfo2 (btConstraintInfo2* info) -{ - 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()); - } -} - - -void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) -{ - ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now - testLimit(transA,transB); - - getInfo2Internal(info,transA,transB,angVelA,angVelB); -} - - -void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) -{ - - btAssert(!m_useSolveConstraintObsolete); - int i, skip = 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 0 - if (0) - { - for (i=0;i<6;i++) - { - info->m_J1linearAxis[i*skip]=0; - info->m_J1linearAxis[i*skip+1]=0; - info->m_J1linearAxis[i*skip+2]=0; - - info->m_J1angularAxis[i*skip]=0; - info->m_J1angularAxis[i*skip+1]=0; - info->m_J1angularAxis[i*skip+2]=0; - - info->m_J2angularAxis[i*skip]=0; - info->m_J2angularAxis[i*skip+1]=0; - info->m_J2angularAxis[i*skip+2]=0; - - info->m_constraintError[i*skip]=0.f; - } - } -#endif //#if 0 - // linear (all fixed) - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[skip + 1] = 1; - info->m_J1linearAxis[2 * skip + 2] = 1; - - - - - - btVector3 a1 = pivotAInW - transA.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip); - btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - btVector3 a2 = pivotBInW - transB.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - // linear RHS - btScalar k = info->fps * info->erp; - 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 - // 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. - // get hinge axis (Z) - btVector3 ax1 = trA.getBasis().getColumn(2); - // get 2 orthos to hinge axis (X, Y) - btVector3 p = trA.getBasis().getColumn(0); - btVector3 q = trA.getBasis().getColumn(1); - // set the two hinge angular rows - int s3 = 3 * info->rowskip; - int s4 = 4 * info->rowskip; - - 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 ax1,ax2 are the unit length hinge axes as computed from body1 and - // body2, 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. - btVector3 ax2 = trB.getBasis().getColumn(2); - btVector3 u = ax1.cross(ax2); - info->m_constraintError[s3] = k * u.dot(p); - info->m_constraintError[s4] = k * u.dot(q); - // check angular limits - int nrow = 4; // last filled row - int srow; - btScalar limit_err = btScalar(0.0); - int limit = 0; - if(getSolveLimit()) - { - limit_err = m_correction * m_referenceSign; - 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) - btScalar bounce = m_relaxationFactor; - 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; - } - } - } - } - info->m_constraintError[srow] *= m_biasFactor; - } // if(limit) - } // if angular limit or powered -} - - - - - - -void btHingeConstraint::updateRHS(btScalar timeStep) -{ - (void)timeStep; - -} - - -btScalar btHingeConstraint::getHingeAngle() -{ - return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); -} - -btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB) -{ - 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 = 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); - m_correction = btScalar(0.); - m_limitSign = btScalar(0.); - m_solveLimit = false; - if (m_lowerLimit <= m_upperLimit) - { - m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, 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; -} -#endif - -static btVector3 vHinge(0, 0, btScalar(1)); - -void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) -{ - // convert target from body to constraint space - btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation(); - qConstraint.normalize(); - - // extract "pure" hinge component - btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize(); - btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge); - btQuaternion qHinge = qNoHinge.inverse() * qConstraint; - qHinge.normalize(); - - // compute angular target, clamped to limits - btScalar targetAngle = qHinge.getAngle(); - if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. - { - qHinge = operator-(qHinge); - targetAngle = qHinge.getAngle(); - } - if (qHinge.getZ() < 0) - targetAngle = -targetAngle; - - setMotorTarget(targetAngle, dt); -} - -void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) -{ - if (m_lowerLimit < m_upperLimit) - { - if (targetAngle < m_lowerLimit) - targetAngle = m_lowerLimit; - else if (targetAngle > m_upperLimit) - targetAngle = m_upperLimit; - } - - // compute angular velocity - btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - btScalar dAngle = targetAngle - curAngle; - m_motorTargetVelocity = dAngle / 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]; - - 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 k = info->fps * info->erp; - 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()) - { - limit_err = m_correction * m_referenceSign; - 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) - btScalar bounce = m_relaxationFactor; - 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; - } - } - } - } - info->m_constraintError[srow] *= m_biasFactor; - } // 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; -} - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.h deleted file mode 100644 index a65f3638ed5..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ /dev/null @@ -1,332 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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. -*/ - -/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */ - -#ifndef HINGECONSTRAINT_H -#define HINGECONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -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 -{ -#ifdef IN_PARALLELL_SOLVER -public: -#endif - btJacobianEntry m_jac[3]; //3 orthogonal linear constraints - btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor - - btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis. - btTransform m_rbBFrame; - - btScalar m_motorTargetVelocity; - btScalar m_maxMotorImpulse; - - btScalar m_limitSoftness; - btScalar m_biasFactor; - btScalar m_relaxationFactor; - - btScalar m_lowerLimit; - btScalar m_upperLimit; - - btScalar m_kHinge; - - btScalar m_limitSign; - btScalar m_correction; - - btScalar m_accLimitImpulse; - btScalar m_hingeAngle; - 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); - - btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,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); - - - virtual void buildJacobian(); - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual(btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - 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); - - - void updateRHS(btScalar timeStep); - - const btRigidBody& getRigidBodyA() const - { - return m_rbA; - } - const btRigidBody& getRigidBodyB() const - { - return m_rbB; - } - - btRigidBody& getRigidBodyA() - { - return m_rbA; - } - - btRigidBody& getRigidBodyB() - { - return m_rbB; - } - - void setAngularOnly(bool angularOnly) - { - m_angularOnly = angularOnly; - } - - void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse) - { - m_enableAngularMotor = enableMotor; - m_motorTargetVelocity = targetVelocity; - m_maxMotorImpulse = maxMotorImpulse; - } - - // extra motor API, including ability to set a target rotation (as opposed to angular velocity) - // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to - // maintain a given angular target. - void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; } - void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; } - void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B. - void setMotorTarget(btScalar targetAngle, btScalar dt); - - - void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) - { - m_lowerLimit = btNormalizeAngle(low); - m_upperLimit = btNormalizeAngle(high); - - m_limitSoftness = _softness; - m_biasFactor = _biasFactor; - m_relaxationFactor = _relaxationFactor; - - } - - void setAxis(btVector3& axisInA) - { - btVector3 rbAxisA1, rbAxisA2; - btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); - btVector3 pivotInA = m_rbAFrame.getOrigin(); -// m_rbAFrame.getOrigin() = pivotInA; - m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), - rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), - rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); - - btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA; - - btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); - btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); - btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); - - - 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() ); - } - - btScalar getLowerLimit() const - { - return m_lowerLimit; - } - - btScalar getUpperLimit() const - { - return m_upperLimit; - } - - - btScalar getHingeAngle(); - - btScalar getHingeAngle(const btTransform& transA,const btTransform& transB); - - void testLimit(const btTransform& transA,const btTransform& transB); - - - const btTransform& getAFrame() const { return m_rbAFrame; }; - const btTransform& getBFrame() const { return m_rbBFrame; }; - - btTransform& getAFrame() { return m_rbAFrame; }; - btTransform& getBFrame() { return m_rbBFrame; }; - - inline int getSolveLimit() - { - return m_solveLimit; - } - - inline btScalar getLimitSign() - { - return m_limitSign; - } - - inline bool getAngularOnly() - { - return m_angularOnly; - } - inline bool getEnableAngularMotor() - { - return m_enableAngularMotor; - } - inline btScalar getMotorTargetVelosity() - { - return m_motorTargetVelocity; - } - inline btScalar getMaxMotorImpulse() - { - 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; - - -}; - -///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; - - 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); - - return btHingeConstraintDataName; -} - -#endif //HINGECONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btJacobianEntry.h deleted file mode 100644 index 22a8af66b8e..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ /dev/null @@ -1,156 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 JACOBIAN_ENTRY_H -#define JACOBIAN_ENTRY_H - -#include "LinearMath/btVector3.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" - - -//notes: -// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components -// which makes the btJacobianEntry memory layout 16 bytes -// if you only are interested in angular part, just feed massInvA and massInvB zero - -/// Jacobian entry is an abstraction that allows to describe constraints -/// it can be used in combination with a constraint solver -/// Can be used to relate the effect of an impulse to the constraint error -ATTRIBUTE_ALIGNED16(class) btJacobianEntry -{ -public: - btJacobianEntry() {}; - //constraint between two different rigidbodies - btJacobianEntry( - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - const btVector3& rel_pos1,const btVector3& rel_pos2, - const btVector3& jointAxis, - const btVector3& inertiaInvA, - const btScalar massInvA, - const btVector3& inertiaInvB, - const btScalar massInvB) - :m_linearJointAxis(jointAxis) - { - m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis)); - m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); - - btAssert(m_Adiag > btScalar(0.0)); - } - - //angular constraint between two different rigidbodies - btJacobianEntry(const btVector3& jointAxis, - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - const btVector3& inertiaInvA, - const btVector3& inertiaInvB) - :m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) - { - m_aJ= world2A*jointAxis; - m_bJ = world2B*-jointAxis; - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - - btAssert(m_Adiag > btScalar(0.0)); - } - - //angular constraint between two different rigidbodies - btJacobianEntry(const btVector3& axisInA, - const btVector3& axisInB, - const btVector3& inertiaInvA, - const btVector3& inertiaInvB) - : m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) - , m_aJ(axisInA) - , m_bJ(-axisInB) - { - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - - btAssert(m_Adiag > btScalar(0.0)); - } - - //constraint on one rigidbody - btJacobianEntry( - const btMatrix3x3& world2A, - const btVector3& rel_pos1,const btVector3& rel_pos2, - const btVector3& jointAxis, - const btVector3& inertiaInvA, - const btScalar massInvA) - :m_linearJointAxis(jointAxis) - { - m_aJ= world2A*(rel_pos1.cross(jointAxis)); - m_bJ = world2A*(rel_pos2.cross(-jointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); - m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); - - btAssert(m_Adiag > btScalar(0.0)); - } - - btScalar getDiagonal() const { return m_Adiag; } - - // for two constraints on the same rigidbody (for example vehicle friction) - btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const - { - const btJacobianEntry& jacA = *this; - btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); - btScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ); - return lin + ang; - } - - - - // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) - btScalar getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const - { - const btJacobianEntry& jacA = *this; - btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; - btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; - btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; - btVector3 lin0 = massInvA * lin ; - btVector3 lin1 = massInvB * lin; - btVector3 sum = ang0+ang1+lin0+lin1; - return sum[0]+sum[1]+sum[2]; - } - - btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB) - { - btVector3 linrel = linvelA - linvelB; - btVector3 angvela = angvelA * m_aJ; - btVector3 angvelb = angvelB * m_bJ; - linrel *= m_linearJointAxis; - angvela += angvelb; - angvela += linrel; - btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2]; - return rel_vel2 + SIMD_EPSILON; - } -//private: - - btVector3 m_linearJointAxis; - btVector3 m_aJ; - btVector3 m_bJ; - btVector3 m_0MinvJt; - btVector3 m_1MinvJt; - //Optimization: can be stored in the w/last component of one of the vectors - btScalar m_Adiag; - -}; - -#endif //JACOBIAN_ENTRY_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp deleted file mode 100644 index 50ad71ab505..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ /dev/null @@ -1,229 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 "btPoint2PointConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include <new> - - - - - -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) -{ - -} - - -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) -{ - -} - -void btPoint2PointConstraint::buildJacobian() -{ - - ///we need it for both methods - { - m_appliedImpulse = btScalar(0.); - - btVector3 normal(0,0,0); - - for (int i=0;i<3;i++) - { - normal[i] = 1; - new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), - m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), - normal, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - normal[i] = 0; - } - } - - -} - -void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info) -{ - getInfo1NonVirtual(info); -} - -void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } else - { - info->m_numConstraintRows = 3; - info->nub = 3; - } -} - - - - -void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info) -{ - getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); -} - -void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans) -{ - btAssert(!m_useSolveConstraintObsolete); - - //retrieve matrices - - // anchor points in global coordinates with respect to body PORs. - - // set jacobian - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; - - btVector3 a1 = body0_trans.getBasis()*getPivotInA(); - { - btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); - btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - - /*info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[s+1] = -1; - info->m_J2linearAxis[2*s+2] = -1; - */ - - btVector3 a2 = body1_trans.getBasis()*getPivotInB(); - - { - btVector3 a2n = -a2; - btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - - - - // set right hand side - 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]); - //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++) - { - if (m_setting.m_impulseClamp > 0) - { - info->m_lowerLimit[j*info->rowskip] = -impulseClamp; - info->m_upperLimit[j*info->rowskip] = impulseClamp; - } - } - -} - - - -void btPoint2PointConstraint::updateRHS(btScalar timeStep) -{ - (void)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; -} - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h deleted file mode 100644 index b589ee68254..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ /dev/null @@ -1,161 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 POINT2POINTCONSTRAINT_H -#define POINT2POINTCONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -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() : - m_tau(btScalar(0.3)), - m_damping(btScalar(1.)), - m_impulseClamp(btScalar(0.)) - { - } - btScalar m_tau; - btScalar m_damping; - 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 -{ -#ifdef IN_PARALLELL_SOLVER -public: -#endif - btJacobianEntry m_jac[3]; //3 orthogonal linear constraints - - btVector3 m_pivotInA; - btVector3 m_pivotInB; - - int m_flags; - btScalar m_erp; - btScalar m_cfm; - -public: - - ///for backwards compatibility during the transition to 'getInfo/getInfo2' - bool m_useSolveConstraintObsolete; - - btConstraintSetting m_setting; - - btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB); - - btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA); - - - virtual void buildJacobian(); - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans); - - void updateRHS(btScalar timeStep); - - void setPivotA(const btVector3& pivotA) - { - m_pivotInA = pivotA; - } - - void setPivotB(const btVector3& pivotB) - { - m_pivotInB = pivotB; - } - - const btVector3& getPivotInA() const - { - return m_pivotInA; - } - - const btVector3& getPivotInB() const - { - 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; - - -}; - -///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 //POINT2POINTCONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp deleted file mode 100644 index 4e1048823c0..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ /dev/null @@ -1,1174 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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. -*/ - -//#define COMPUTE_IMPULSE_DENOM 1 -//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. - -#include "btSequentialImpulseConstraintSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "btContactConstraint.h" -#include "btSolve2LinearConstraint.h" -#include "btContactSolverInfo.h" -#include "LinearMath/btIDebugDraw.h" -#include "btJacobianEntry.h" -#include "LinearMath/btMinMax.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include <new> -#include "LinearMath/btStackAlloc.h" -#include "LinearMath/btQuickprof.h" -#include "btSolverBody.h" -#include "btSolverConstraint.h" -#include "LinearMath/btAlignedObjectArray.h" -#include <string.h> //for memset - -int gNumSplitImpulseRecoveries = 0; - -btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() -:m_btSeed2(0) -{ - -} - -btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() -{ -} - -#ifdef USE_SIMD -#include <emmintrin.h> -#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) -static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 ) -{ - __m128 result = _mm_mul_ps( vec0, vec1); - return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) ); -} -#endif//USE_SIMD - -// Project Gauss Seidel or the equivalent Sequential Impulse -void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) -{ -#ifdef USE_SIMD - __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); - __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); - __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); - __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - btSimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); - __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); - __m128 impulseMagnitude = deltaImpulse; - body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); -#else - resolveSingleConstraintRowGeneric(body1,body2,c); -#endif -} - -// Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) -{ - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); - -// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else if (sum > c.m_upperLimit) - { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_upperLimit; - } - else - { - c.m_appliedImpulse = sum; - } - body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); -} - - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) -{ -#ifdef USE_SIMD - __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); - __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); - __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); - __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - btSimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); - __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); - __m128 impulseMagnitude = deltaImpulse; - body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); -#else - resolveSingleConstraintRowLowerLimit(body1,body2,c); -#endif -} - -// Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) -{ - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); - - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else - { - c.m_appliedImpulse = sum; - } - body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); -} - - -void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( - btRigidBody& body1, - btRigidBody& body2, - const btSolverConstraint& c) -{ - if (c.m_rhsPenetration) - { - gNumSplitImpulseRecoveries++; - btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); - - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse; - c.m_appliedPushImpulse = c.m_lowerLimit; - } - else - { - c.m_appliedPushImpulse = sum; - } - body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); - } -} - - void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) -{ -#ifdef USE_SIMD - if (!c.m_rhsPenetration) - return; - - gNumSplitImpulseRecoveries++; - - __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse); - __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); - __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); - __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - btSimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); - __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); - __m128 impulseMagnitude = deltaImpulse; - body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); -#else - resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); -#endif -} - - - -unsigned long btSequentialImpulseConstraintSolver::btRand2() -{ - m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; - return m_btSeed2; -} - - - -//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) -int btSequentialImpulseConstraintSolver::btRandInt2 (int n) -{ - // seems good; xor-fold and modulus - const unsigned long un = static_cast<unsigned long>(n); - unsigned long r = btRand2(); - - // note: probably more aggressive than it needs to be -- might be - // able to get away without one or two of the innermost branches. - if (un <= 0x00010000UL) { - r ^= (r >> 16); - if (un <= 0x00000100UL) { - r ^= (r >> 8); - if (un <= 0x00000010UL) { - r ^= (r >> 4); - if (un <= 0x00000004UL) { - r ^= (r >> 2); - if (un <= 0x00000002UL) { - r ^= (r >> 1); - } - } - } - } - } - - return (int) (r % un); -} - - -#if 0 -void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) -{ - btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; - - solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f); - solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - - if (rb) - { - solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor(); - solverBody->m_originalBody = rb; - solverBody->m_angularFactor = rb->getAngularFactor(); - } else - { - solverBody->internalGetInvMass().setValue(0,0,0); - solverBody->m_originalBody = 0; - solverBody->m_angularFactor.setValue(1,1,1); - } -} -#endif - - - - - -btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution) -{ - btScalar rest = restitution * -rel_vel; - return rest; -} - - - -void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection); -void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection) -{ - if (colObj && colObj->hasAnisotropicFriction()) - { - // transform to local coordinates - btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis(); - const btVector3& friction_scaling = colObj->getAnisotropicFriction(); - //apply anisotropic friction - loc_lateral *= friction_scaling; - // ... and transform it back to global coordinates - frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; - } -} - - -void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) -{ - - - btRigidBody* body0=btRigidBody::upcast(colObj0); - btRigidBody* body1=btRigidBody::upcast(colObj1); - - solverConstraint.m_contactNormal = normalAxis; - - solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody(); - solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody(); - - solverConstraint.m_friction = cp.m_combinedFriction; - solverConstraint.m_originalContactPoint = 0; - - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - - { - btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); - solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0); - } - { - btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal); - solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0); - } - -#ifdef COMPUTE_IMPULSE_DENOM - btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); -#else - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - if (body0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = body0->getInvMass() + normalAxis.dot(vec); - } - if (body1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = body1->getInvMass() + normalAxis.dot(vec); - } - - -#endif //COMPUTE_IMPULSE_DENOM - btScalar denom = relaxation/(denom0+denom1); - solverConstraint.m_jacDiagABInv = denom; - -#ifdef _USE_JACOBIAN - solverConstraint.m_jac = btJacobianEntry ( - rel_pos1,rel_pos2,solverConstraint.m_contactNormal, - body0->getInvInertiaDiagLocal(), - body0->getInvMass(), - body1->getInvInertiaDiagLocal(), - body1->getInvMass()); -#endif //_USE_JACOBIAN - - - { - btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0)); - btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0)); - - rel_vel = vel1Dotn+vel2Dotn; - -// btScalar positionalError = 0.f; - - btSimdScalar velocityError = desiredVelocity - rel_vel; - btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_cfm = cfmSlip; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; - } -} - - - -btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) -{ - btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); - solverConstraint.m_frictionIndex = frictionIndex; - setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2, - colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); - return solverConstraint; -} - -int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body) -{ -#if 0 - int solverBodyIdA = -1; - - if (body.getCompanionId() >= 0) - { - //body has already been converted - solverBodyIdA = body.getCompanionId(); - } else - { - btRigidBody* rb = btRigidBody::upcast(&body); - if (rb && rb->getInvMass()) - { - solverBodyIdA = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,&body); - body.setCompanionId(solverBodyIdA); - } else - { - return 0;//assume first one is a fixed solver body - } - } - return solverBodyIdA; -#endif - return 0; -} -#include <stdio.h> - - -void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, - btCollisionObject* colObj0, btCollisionObject* colObj1, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btVector3& vel, btScalar& rel_vel, btScalar& relaxation, - btVector3& rel_pos1, btVector3& rel_pos2) -{ - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - -// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); -// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); - rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - - relaxation = 1.f; - - btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - - { -#ifdef COMPUTE_IMPULSE_DENOM - btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); -#else - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } - if (rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } -#endif //COMPUTE_IMPULSE_DENOM - - btScalar denom = relaxation/(denom0+denom1); - solverConstraint.m_jacDiagABInv = denom; - } - - solverConstraint.m_contactNormal = cp.m_normalWorldOnB; - solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB); - - - - - btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); - btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - vel = vel1 - vel2; - rel_vel = cp.m_normalWorldOnB.dot(vel); - - btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; - - - solverConstraint.m_friction = cp.m_combinedFriction; - - btScalar restitution = 0.f; - - if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold) - { - restitution = 0.f; - } else - { - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - }; - } - - - ///warm starting (or zero if disabled) - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - if (rb0) - rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - if (rb1) - rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); - } else - { - solverConstraint.m_appliedImpulse = 0.f; - } - - solverConstraint.m_appliedPushImpulse = 0.f; - - { - btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0)); - btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0)); - - rel_vel = vel1Dotn+vel2Dotn; - - btScalar positionalError = 0.f; - positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; - btScalar velocityError = restitution - rel_vel;// * damping; - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; - } - - - - -} - - - -void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, - btRigidBody* rb0, btRigidBody* rb1, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) -{ - if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) - { - { - btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; - if (rb0) - rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); - if (rb1) - rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); - } else - { - frictionConstraint1.m_appliedImpulse = 0.f; - } - } - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; - if (rb0) - rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); - if (rb1) - rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); - } else - { - frictionConstraint2.m_appliedImpulse = 0.f; - } - } - } else - { - btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; - frictionConstraint1.m_appliedImpulse = 0.f; - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; - frictionConstraint2.m_appliedImpulse = 0.f; - } - } -} - - - - -void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) -{ - btCollisionObject* colObj0=0,*colObj1=0; - - colObj0 = (btCollisionObject*)manifold->getBody0(); - colObj1 = (btCollisionObject*)manifold->getBody1(); - - - btRigidBody* solverBodyA = btRigidBody::upcast(colObj0); - btRigidBody* solverBodyB = btRigidBody::upcast(colObj1); - - ///avoid collision response between two static objects - if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass())) - return; - - for (int j=0;j<manifold->getNumContacts();j++) - { - - btManifoldPoint& cp = manifold->getContactPoint(j); - - if (cp.getDistance() <= manifold->getContactProcessingThreshold()) - { - btVector3 rel_pos1; - btVector3 rel_pos2; - btScalar relaxation; - btScalar rel_vel; - btVector3 vel; - - int frictionIndex = m_tmpSolverContactConstraintPool.size(); - btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); - solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody(); - solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody(); - solverConstraint.m_originalContactPoint = &cp; - - setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); - -// const btVector3& pos1 = cp.getPositionWorldOnA(); -// const btVector3& pos2 = cp.getPositionWorldOnB(); - - /////setup the friction constraints - - solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); - - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) - { - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) - { - cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); - if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); - cp.m_lateralFrictionDir2.normalize();//?? - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - cp.m_lateralFrictionInitialized = true; - } else - { - //re-calculate friction direction every frame, todo: check if this is really needed - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - cp.m_lateralFrictionInitialized = true; - } - - } else - { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); - } - - setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal); - - } - } -} - - -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) -{ - BT_PROFILE("solveGroupCacheFriendlySetup"); - (void)stackAlloc; - (void)debugDrawer; - - - if (!(numConstraints + numManifolds)) - { - // printf("empty\n"); - return 0.f; - } - - if (1) - { - int j; - for (j=0;j<numConstraints;j++) - { - btTypedConstraint* constraint = constraints[j]; - constraint->buildJacobian(); - } - } - //btRigidBody* rb0=0,*rb1=0; - - //if (1) - { - { - - int totalNumRows = 0; - int i; - - m_tmpConstraintSizesPool.resize(numConstraints); - //calculate the total number of contraint rows - for (i=0;i<numConstraints;i++) - { - btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; - constraints[i]->getInfo1(&info1); - totalNumRows += info1.m_numConstraintRows; - } - m_tmpSolverNonContactConstraintPool.resize(totalNumRows); - - - ///setup the btSolverConstraints - int currentRow = 0; - - for (i=0;i<numConstraints;i++) - { - const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; - - if (info1.m_numConstraintRows) - { - btAssert(currentRow<totalNumRows); - - btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; - btTypedConstraint* constraint = constraints[i]; - - - - btRigidBody& rbA = constraint->getRigidBodyA(); - btRigidBody& rbB = constraint->getRigidBodyB(); - - - int j; - for ( j=0;j<info1.m_numConstraintRows;j++) - { - memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint)); - currentConstraintRow[j].m_lowerLimit = -FLT_MAX; - currentConstraintRow[j].m_upperLimit = FLT_MAX; - currentConstraintRow[j].m_appliedImpulse = 0.f; - currentConstraintRow[j].m_appliedPushImpulse = 0.f; - currentConstraintRow[j].m_solverBodyA = &rbA; - currentConstraintRow[j].m_solverBodyB = &rbB; - } - - rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - - - - btTypedConstraint::btConstraintInfo2 info2; - info2.fps = 1.f/infoGlobal.m_timeStep; - info2.erp = infoGlobal.m_erp; - info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; - info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; - info2.m_J2linearAxis = 0; - info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; - info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this - ///the size of btSolverConstraint needs be a multiple of btScalar - btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); - info2.m_constraintError = ¤tConstraintRow->m_rhs; - currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; - info2.cfm = ¤tConstraintRow->m_cfm; - info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; - info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; - info2.m_numIterations = infoGlobal.m_numIterations; - constraints[i]->getInfo2(&info2); - - ///finalize the constraint setup - for ( j=0;j<info1.m_numConstraintRows;j++) - { - btSolverConstraint& solverConstraint = currentConstraintRow[j]; - solverConstraint.m_originalContactPoint = constraint; - - { - const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; - solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); - } - { - const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; - solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); - } - - { - btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass(); - btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; - btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal? - btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; - - btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal); - sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); - sum += iMJlB.dot(solverConstraint.m_contactNormal); - sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - - solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; - } - - - ///fix rhs - ///todo: add force/torque accelerators - { - btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()); - btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()); - - rel_vel = vel1Dotn+vel2Dotn; - - btScalar restitution = 0.f; - btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 - btScalar velocityError = restitution - rel_vel;// * damping; - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_appliedImpulse = 0.f; - - } - } - } - currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; - } - } - - { - int i; - btPersistentManifold* manifold = 0; -// btCollisionObject* colObj0=0,*colObj1=0; - - - for (i=0;i<numManifolds;i++) - { - manifold = manifoldPtr[i]; - convertContact(manifold,infoGlobal); - } - } - } - - btContactSolverInfo info = infoGlobal; - - - - int numConstraintPool = m_tmpSolverContactConstraintPool.size(); - int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - - ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints - m_orderTmpConstraintPool.resize(numConstraintPool); - m_orderFrictionConstraintPool.resize(numFrictionPool); - { - int i; - for (i=0;i<numConstraintPool;i++) - { - m_orderTmpConstraintPool[i] = i; - } - for (i=0;i<numFrictionPool;i++) - { - m_orderFrictionConstraintPool[i] = i; - } - } - - return 0.f; - -} - -btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) -{ - - int numConstraintPool = m_tmpSolverContactConstraintPool.size(); - int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - - int j; - - if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) - { - if ((iteration & 7) == 0) { - for (j=0; j<numConstraintPool; ++j) { - int tmp = m_orderTmpConstraintPool[j]; - int swapi = btRandInt2(j+1); - m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; - m_orderTmpConstraintPool[swapi] = tmp; - } - - for (j=0; j<numFrictionPool; ++j) { - int tmp = m_orderFrictionConstraintPool[j]; - int swapi = btRandInt2(j+1); - m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; - m_orderFrictionConstraintPool[swapi] = tmp; - } - } - } - - if (infoGlobal.m_solverMode & SOLVER_SIMD) - { - ///solve all joint constraints, using SIMD, if available - for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) - { - btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; - resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); - } - - for (j=0;j<numConstraints;j++) - { - constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - } - - ///solve all contact constraints using SIMD, if available - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - - } - ///solve all friction constraints, using SIMD, if available - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;j<numFrictionPoolConstraints;j++) - { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - - if (totalImpulse>btScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold); - } - } - } else - { - - ///solve all joint constraints - for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) - { - btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; - resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); - } - - for (j=0;j<numConstraints;j++) - { - constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - } - ///solve all contact constraints - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - } - ///solve all friction constraints - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;j<numFrictionPoolConstraints;j++) - { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - - if (totalImpulse>btScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - } - } - } - return 0.f; -} - - -void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) -{ - int iteration; - if (infoGlobal.m_splitImpulse) - { - if (infoGlobal.m_solverMode & SOLVER_SIMD) - { - for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) - { - { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - - resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - } - } - } - } - else - { - for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) - { - { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - - resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - } - } - } - } - } -} - -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) -{ - BT_PROFILE("solveGroupCacheFriendlyIterations"); - - - //should traverse the contacts random order... - int iteration; - { - for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) - { - solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); - } - - solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); - } - return 0.f; -} - -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) -{ - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int i,j; - - for (j=0;j<numPoolConstraints;j++) - { - - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; - btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint; - btAssert(pt); - pt->m_appliedImpulse = solveManifold.m_appliedImpulse; - if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) - { - pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; - } - - //do a callback here? - } - - numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; - btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; - btScalar sum = constr->internalGetAppliedImpulse(); - sum += solverConstr.m_appliedImpulse; - constr->internalSetAppliedImpulse(sum); - } - - - if (infoGlobal.m_splitImpulse) - { - for ( i=0;i<numBodies;i++) - { - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body) - body->internalWritebackVelocity(infoGlobal.m_timeStep); - } - } else - { - for ( i=0;i<numBodies;i++) - { - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body) - body->internalWritebackVelocity(); - } - } - - - m_tmpSolverContactConstraintPool.resize(0); - m_tmpSolverNonContactConstraintPool.resize(0); - m_tmpSolverContactFrictionConstraintPool.resize(0); - - return 0.f; -} - - - -/// btSequentialImpulseConstraintSolver Sequentially applies impulses -btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) -{ - - BT_PROFILE("solveGroup"); - //you need to provide at least some bodies - btAssert(bodies); - btAssert(numBodies); - - solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - - solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - - solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - - return 0.f; -} - -void btSequentialImpulseConstraintSolver::reset() -{ - m_btSeed2 = 0; -} - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h deleted file mode 100644 index a26fbad787b..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ /dev/null @@ -1,132 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H -#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H - -#include "btConstraintSolver.h" -class btIDebugDraw; -#include "btContactConstraint.h" -#include "btSolverBody.h" -#include "btSolverConstraint.h" -#include "btTypedConstraint.h" -#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" - -///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. -class btSequentialImpulseConstraintSolver : public btConstraintSolver -{ -protected: - - btConstraintArray m_tmpSolverContactConstraintPool; - btConstraintArray m_tmpSolverNonContactConstraintPool; - btConstraintArray m_tmpSolverContactFrictionConstraintPool; - btAlignedObjectArray<int> m_orderTmpConstraintPool; - btAlignedObjectArray<int> m_orderFrictionConstraintPool; - btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool; - - void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* 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,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - - void setupContactConstraint(btSolverConstraint& solverConstraint, btCollisionObject* colObj0, btCollisionObject* colObj1, btManifoldPoint& cp, - const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation, - btVector3& rel_pos1, btVector3& rel_pos2); - - void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, btRigidBody* rb0, btRigidBody* rb1, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); - - ///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( - btRigidBody& body1, - btRigidBody& body2, - const btSolverConstraint& contactConstraint); - - void resolveSplitPenetrationImpulseCacheFriendly( - btRigidBody& body1, - btRigidBody& body2, - const btSolverConstraint& contactConstraint); - - //internal method - int getOrInitSolverBody(btCollisionObject& body); - - void resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - - void resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - - void resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - - void resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - -protected: - static btRigidBody& getFixedBody() - { - static btRigidBody s_fixed(0, 0,0); - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); - return s_fixed; - } - 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,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - 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: - - - 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); - - - - ///clear internal cached data and reset random seed - virtual void reset(); - - unsigned long btRand2(); - - int btRandInt2 (int n); - - void setRandSeed(unsigned long seed) - { - m_btSeed2 = seed; - } - unsigned long getRandSeed() const - { - return m_btSeed2; - } - -}; - -#ifndef BT_PREFER_SIMD -typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered; -#endif - - -#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp deleted file mode 100644 index b69f46da1b4..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ /dev/null @@ -1,857 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 -*/ - - - -#include "btSliderConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include <new> - -#define USE_OFFSET_FOR_CONSTANT_FRAME true - -void btSliderConstraint::initParams() -{ - m_lowerLinLimit = btScalar(1.0); - m_upperLinLimit = btScalar(-1.0); - m_lowerAngLimit = btScalar(0.); - m_upperAngLimit = btScalar(0.); - m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirLin = btScalar(0.); - m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirAng = btScalar(0.); - m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM; - - m_poweredLinMotor = false; - m_targetLinMotorVelocity = btScalar(0.); - m_maxLinMotorForce = btScalar(0.); - m_accumulatedLinMotorImpulse = btScalar(0.0); - - m_poweredAngMotor = false; - m_targetAngMotorVelocity = btScalar(0.); - m_maxAngMotorForce = btScalar(0.); - m_accumulatedAngMotorImpulse = btScalar(0.0); - - m_flags = 0; - m_flags = 0; - - m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME; - - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); -} - - - - - -btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB), - m_useSolveConstraintObsolete(false), - m_frameInA(frameInA), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) -{ - initParams(); -} - - - -btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB), - m_useSolveConstraintObsolete(false), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) -{ - ///not providing rigidbody A means implicitly using worldspace for body A - m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; -// m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin()); - - initParams(); -} - - - - - - -void btSliderConstraint::getInfo1(btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } - else - { - info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular - info->nub = 2; - //prepare constraint - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - testAngLimits(); - testLinLimits(); - if(getSolveLinLimit() || getPoweredLinMotor()) - { - info->m_numConstraintRows++; // limit 3rd linear as well - info->nub--; - } - if(getSolveAngLimit() || getPoweredAngMotor()) - { - info->m_numConstraintRows++; // limit 3rd angular as well - info->nub--; - } - } -} - -void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info) -{ - - info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used) - info->nub = 0; -} - -void btSliderConstraint::getInfo2(btConstraintInfo2* info) -{ - getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass()); -} - - - - - - - -void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) -{ - if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) - { - m_calculatedTransformA = transA * m_frameInA; - m_calculatedTransformB = transB * m_frameInB; - } - else - { - m_calculatedTransformA = transB * m_frameInB; - m_calculatedTransformB = transA * m_frameInA; - } - m_realPivotAInW = m_calculatedTransformA.getOrigin(); - m_realPivotBInW = m_calculatedTransformB.getOrigin(); - m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X - if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete) - { - m_delta = m_realPivotBInW - m_realPivotAInW; - } - else - { - m_delta = m_realPivotAInW - m_realPivotBInW; - } - m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - btVector3 normalWorld; - int i; - //linear part - for(i = 0; i < 3; i++) - { - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - m_depth[i] = m_delta.dot(normalWorld); - } -} - - - -void btSliderConstraint::testLinLimits(void) -{ - m_solveLinLim = false; - m_linPos = m_depth[0]; - if(m_lowerLinLimit <= m_upperLinLimit) - { - if(m_depth[0] > m_upperLinLimit) - { - m_depth[0] -= m_upperLinLimit; - m_solveLinLim = true; - } - else if(m_depth[0] < m_lowerLinLimit) - { - m_depth[0] -= m_lowerLinLimit; - m_solveLinLim = true; - } - else - { - m_depth[0] = btScalar(0.); - } - } - else - { - m_depth[0] = btScalar(0.); - } -} - - - -void btSliderConstraint::testAngLimits(void) -{ - m_angDepth = btScalar(0.); - m_solveAngLim = false; - if(m_lowerAngLimit <= m_upperAngLimit) - { - const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1); - const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); - const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); -// btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); - btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0)); - rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit); - m_angPos = rot; - if(rot < m_lowerAngLimit) - { - m_angDepth = rot - m_lowerAngLimit; - m_solveAngLim = true; - } - else if(rot > m_upperAngLimit) - { - m_angDepth = rot - m_upperAngLimit; - m_solveAngLim = true; - } - } -} - -btVector3 btSliderConstraint::getAncorInA(void) -{ - btVector3 ancorInA; - ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis; - ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA; - return ancorInA; -} - - - -btVector3 btSliderConstraint::getAncorInB(void) -{ - btVector3 ancorInB; - ancorInB = m_frameInB.getOrigin(); - return ancorInB; -} - - -void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass ) -{ - const btTransform& trA = getCalculatedTransformA(); - const btTransform& trB = getCalculatedTransformB(); - - btAssert(!m_useSolveConstraintObsolete); - int i, s = info->rowskip; - - btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f); - - // difference between frames in WCS - btVector3 ofs = trB.getOrigin() - trA.getOrigin(); - // now get weight factors depending on masses - btScalar miA = rbAinvMass; - btScalar miB = rbBinvMass; - 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; - btVector3 ax1, p, q; - btVector3 ax1A = trA.getBasis().getColumn(0); - btVector3 ax1B = trB.getBasis().getColumn(0); - if(m_useOffsetForConstraintFrame) - { - // get the desired direction of slider axis - // as weighted sum of X-orthos of frameA and frameB in WCS - ax1 = ax1A * factA + ax1B * factB; - ax1.normalize(); - // construct two orthos to slider axis - btPlaneSpace1 (ax1, p, q); - } - else - { // old way - use frameA - ax1 = trA.getBasis().getColumn(0); - // get 2 orthos to slider axis (Y, Z) - p = trA.getBasis().getColumn(1); - q = trA.getBasis().getColumn(2); - } - // make rotations around these orthos equal - // the slider axis should be the only unconstrained - // rotational axis, the angular velocity of the two bodies perpendicular to - // the slider 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 slider axis, and w1 and w2 - // are the angular velocity vectors of the two bodies. - info->m_J1angularAxis[0] = p[0]; - info->m_J1angularAxis[1] = p[1]; - info->m_J1angularAxis[2] = p[2]; - info->m_J1angularAxis[s+0] = q[0]; - info->m_J1angularAxis[s+1] = q[1]; - info->m_J1angularAxis[s+2] = q[2]; - - info->m_J2angularAxis[0] = -p[0]; - info->m_J2angularAxis[1] = -p[1]; - info->m_J2angularAxis[2] = -p[2]; - info->m_J2angularAxis[s+0] = -q[0]; - info->m_J2angularAxis[s+1] = -q[1]; - info->m_J2angularAxis[s+2] = -q[2]; - // compute the right hand side of the constraint equation. set relative - // body velocities along p and q to bring the slider back into alignment. - // if ax1A,ax1B are the unit length slider 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. -// btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); - btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp; - btScalar k = info->fps * currERP; - - btVector3 u = ax1A.cross(ax1B); - info->m_constraintError[0] = k * u.dot(p); - info->m_constraintError[s] = k * u.dot(q); - if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG) - { - info->cfm[0] = m_cfmOrthoAng; - info->cfm[s] = m_cfmOrthoAng; - } - - int nrow = 1; // last filled row - int srow; - btScalar limit_err; - int limit; - int powered; - - // next two rows. - // we want: velA + wA x relA == velB + wB x relB ... but this would - // result in three equations, so we project along two orthos to the slider axis - - btTransform bodyA_trans = transA; - btTransform bodyB_trans = transB; - nrow++; - int s2 = nrow * s; - nrow++; - int s3 = nrow * s; - btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0); - if(m_useOffsetForConstraintFrame) - { - // get vector from bodyB to frameB in WCS - relB = trB.getOrigin() - bodyB_trans.getOrigin(); - // get its projection to slider axis - btVector3 projB = ax1 * relB.dot(ax1); - // get vector directed from bodyB to slider 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; - // get desired offset between frames A and B along slider axis - btScalar sliderOffs = m_linPos - m_depth[0]; - // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis - btVector3 totalDist = projA + ax1 * sliderOffs - projB; - // get offset vectors relA and relB - relA = orthoA + totalDist * factA; - relB = orthoB - totalDist * factB; - // now choose average ortho to slider 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 two rows - tmpA = relA.cross(p); - tmpB = relB.cross(p); - 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]; - tmpA = relA.cross(q); - tmpB = relB.cross(q); - if(hasStaticBody && getSolveAngLimit()) - { // 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[s3+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; - } - else - { // old way - maybe incorrect if bodies are not on the slider axis - // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0 - c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); - btVector3 tmp = c.cross(p); - for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i]; - tmp = c.cross(q); - for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i]; - - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; - } - // compute two elements of right hand side - - // k = info->fps * info->erp * getSoftnessOrthoLin(); - currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp; - k = info->fps * currERP; - - btScalar rhs = k * p.dot(ofs); - info->m_constraintError[s2] = rhs; - rhs = k * q.dot(ofs); - info->m_constraintError[s3] = rhs; - if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN) - { - info->cfm[s2] = m_cfmOrthoLin; - info->cfm[s3] = m_cfmOrthoLin; - } - - - // check linear limits - limit_err = btScalar(0.0); - limit = 0; - if(getSolveLinLimit()) - { - limit_err = getLinDepth() * signFact; - limit = (limit_err > btScalar(0.0)) ? 2 : 1; - } - powered = 0; - if(getPoweredLinMotor()) - { - powered = 1; - } - // if the slider has joint limits or motor, add in the extra row - if (limit || powered) - { - nrow++; - srow = nrow * info->rowskip; - info->m_J1linearAxis[srow+0] = ax1[0]; - info->m_J1linearAxis[srow+1] = ax1[1]; - info->m_J1linearAxis[srow+2] = ax1[2]; - // linear torque decoupling step: - // - // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies - // do not create a torque couple. in other words, the points that the - // constraint force is applied at must lie along the same ax1 axis. - // a torque couple will result in limited slider-jointed free - // bodies from gaining angular momentum. - if(m_useOffsetForConstraintFrame) - { - // this is needed only when bodyA and bodyB are both dynamic. - if(!hasStaticBody) - { - tmpA = relA.cross(ax1); - tmpB = relB.cross(ax1); - info->m_J1angularAxis[srow+0] = tmpA[0]; - info->m_J1angularAxis[srow+1] = tmpA[1]; - info->m_J1angularAxis[srow+2] = tmpA[2]; - info->m_J2angularAxis[srow+0] = -tmpB[0]; - info->m_J2angularAxis[srow+1] = -tmpB[1]; - info->m_J2angularAxis[srow+2] = -tmpB[2]; - } - } - else - { // The old way. May be incorrect if bodies are not on the slider axis - btVector3 ltd; // Linear Torque Decoupling vector (a torque) - ltd = c.cross(ax1); - info->m_J1angularAxis[srow+0] = factA*ltd[0]; - info->m_J1angularAxis[srow+1] = factA*ltd[1]; - info->m_J1angularAxis[srow+2] = factA*ltd[2]; - info->m_J2angularAxis[srow+0] = factB*ltd[0]; - info->m_J2angularAxis[srow+1] = factB*ltd[1]; - info->m_J2angularAxis[srow+2] = factB*ltd[2]; - } - // right-hand part - btScalar lostop = getLowerLinLimit(); - btScalar histop = getUpperLinLimit(); - if(limit && (lostop == histop)) - { // the joint motor is ineffective - powered = 0; - } - info->m_constraintError[srow] = 0.; - info->m_lowerLimit[srow] = 0.; - info->m_upperLimit[srow] = 0.; - currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp; - if(powered) - { - if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN) - { - info->cfm[srow] = m_cfmDirLin; - } - btScalar tag_vel = getTargetLinMotorVelocity(); - btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP); - info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity(); - info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps; - info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps; - } - if(limit) - { - k = info->fps * currERP; - info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN) - { - info->cfm[srow] = m_cfmLimLin; - } - 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] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - else - { // high limit - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) - btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin()); - if(bounce > btScalar(0.0)) - { - btScalar vel = linVelA.dot(ax1); - vel -= linVelB.dot(ax1); - vel *= signFact; - // 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; - } - } - } - } - info->m_constraintError[srow] *= getSoftnessLimLin(); - } // if(limit) - } // if linear limit - // check angular limits - limit_err = btScalar(0.0); - limit = 0; - if(getSolveAngLimit()) - { - limit_err = getAngDepth(); - limit = (limit_err > btScalar(0.0)) ? 1 : 2; - } - // if the slider has joint limits, add in the extra row - powered = 0; - if(getPoweredAngMotor()) - { - 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 = getLowerAngLimit(); - btScalar histop = getUpperAngLimit(); - if(limit && (lostop == histop)) - { // the joint motor is ineffective - powered = 0; - } - currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp; - if(powered) - { - if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG) - { - info->cfm[srow] = m_cfmDirAng; - } - btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP); - info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity(); - info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps; - info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps; - } - if(limit) - { - k = info->fps * currERP; - info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG) - { - info->cfm[srow] = m_cfmLimAng; - } - 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) - btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng()); - if(bounce > btScalar(0.0)) - { - btScalar vel = m_rbA.getAngularVelocity().dot(ax1); - vel -= m_rbB.getAngularVelocity().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; - } - } - } - } - info->m_constraintError[srow] *= getSoftnessLimAng(); - } // 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 btSliderConstraint::setParam(int num, btScalar value, int axis) -{ - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - if(axis < 1) - { - m_softnessLimLin = value; - m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN; - } - else if(axis < 3) - { - m_softnessOrthoLin = value; - m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN; - } - else if(axis == 3) - { - m_softnessLimAng = value; - m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG; - } - else if(axis < 6) - { - m_softnessOrthoAng = value; - m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_CFM : - if(axis < 1) - { - m_cfmDirLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN; - } - else if(axis == 3) - { - m_cfmDirAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_STOP_CFM : - if(axis < 1) - { - m_cfmLimLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN; - } - else if(axis < 3) - { - m_cfmOrthoLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN; - } - else if(axis == 3) - { - m_cfmLimAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG; - } - else if(axis < 6) - { - m_cfmOrthoAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG; - } - else - { - btAssertConstrParams(0); - } - break; - } -} - -///return the local value of parameter -btScalar btSliderConstraint::getParam(int num, int axis) const -{ - btScalar retVal(SIMD_INFINITY); - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN); - retVal = m_softnessLimLin; - } - else if(axis < 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN); - retVal = m_softnessOrthoLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG); - retVal = m_softnessLimAng; - } - else if(axis < 6) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG); - retVal = m_softnessOrthoAng; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_CFM : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN); - retVal = m_cfmDirLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG); - retVal = m_cfmDirAng; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_STOP_CFM : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN); - retVal = m_cfmLimLin; - } - else if(axis < 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN); - retVal = m_cfmOrthoLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG); - retVal = m_cfmLimAng; - } - else if(axis < 6) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG); - retVal = m_cfmOrthoAng; - } - else - { - btAssertConstrParams(0); - } - break; - } - return retVal; -} - - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.h deleted file mode 100644 index 7d2a5022753..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ /dev/null @@ -1,321 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 - -TODO: - - add clamping od accumulated impulse to improve stability - - add conversion for ODE constraint solver -*/ - -#ifndef SLIDER_CONSTRAINT_H -#define SLIDER_CONSTRAINT_H - - - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - - - -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 -{ -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 - bool m_useLinearReferenceFrameA; - // linear limits - btScalar m_lowerLinLimit; - btScalar m_upperLinLimit; - // angular limits - btScalar m_lowerAngLimit; - btScalar m_upperAngLimit; - // softness, restitution and damping for different cases - // DirLin - moving inside linear limits - // LimLin - hitting linear limit - // DirAng - moving inside angular limits - // LimAng - hitting angular limit - // OrthoLin, OrthoAng - against constraint axis - 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]; - - btJacobianEntry m_jacAng[3]; - - btScalar m_timeStep; - btTransform m_calculatedTransformA; - btTransform m_calculatedTransformB; - - btVector3 m_sliderAxis; - btVector3 m_realPivotAInW; - btVector3 m_realPivotBInW; - btVector3 m_projPivotInW; - btVector3 m_delta; - btVector3 m_depth; - btVector3 m_relPosA; - btVector3 m_relPosB; - - btScalar m_linPos; - btScalar m_angPos; - - btScalar m_angDepth; - btScalar m_kAngle; - - bool m_poweredLinMotor; - btScalar m_targetLinMotorVelocity; - btScalar m_maxLinMotorForce; - btScalar m_accumulatedLinMotorImpulse; - - bool m_poweredAngMotor; - btScalar m_targetAngMotorVelocity; - btScalar m_maxAngMotorForce; - btScalar m_accumulatedAngMotorImpulse; - - //------------------------ - void initParams(); -public: - // constructors - btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA); - - // overrides - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual(btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass); - - - // access - const btRigidBody& getRigidBodyA() const { return m_rbA; } - const btRigidBody& getRigidBodyB() const { return m_rbB; } - const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } - const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; } - const btTransform & getFrameOffsetA() const { return m_frameInA; } - const btTransform & getFrameOffsetB() const { return m_frameInB; } - btTransform & getFrameOffsetA() { return m_frameInA; } - btTransform & getFrameOffsetB() { return m_frameInB; } - btScalar getLowerLinLimit() { return m_lowerLinLimit; } - void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; } - btScalar getUpperLinLimit() { return m_upperLinLimit; } - void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; } - btScalar getLowerAngLimit() { return m_lowerAngLimit; } - void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); } - btScalar getUpperAngLimit() { return m_upperAngLimit; } - void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); } - bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; } - btScalar getSoftnessDirLin() { return m_softnessDirLin; } - btScalar getRestitutionDirLin() { return m_restitutionDirLin; } - btScalar getDampingDirLin() { return m_dampingDirLin ; } - btScalar getSoftnessDirAng() { return m_softnessDirAng; } - btScalar getRestitutionDirAng() { return m_restitutionDirAng; } - btScalar getDampingDirAng() { return m_dampingDirAng; } - btScalar getSoftnessLimLin() { return m_softnessLimLin; } - btScalar getRestitutionLimLin() { return m_restitutionLimLin; } - btScalar getDampingLimLin() { return m_dampingLimLin; } - btScalar getSoftnessLimAng() { return m_softnessLimAng; } - btScalar getRestitutionLimAng() { return m_restitutionLimAng; } - btScalar getDampingLimAng() { return m_dampingLimAng; } - btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; } - btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; } - btScalar getDampingOrthoLin() { return m_dampingOrthoLin; } - btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; } - btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; } - btScalar getDampingOrthoAng() { return m_dampingOrthoAng; } - void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; } - void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } - void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; } - void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; } - void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } - void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; } - void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; } - void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } - void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; } - void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; } - void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } - void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; } - void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } - void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } - void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } - void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } - void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } - void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } - void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } - bool getPoweredLinMotor() { return m_poweredLinMotor; } - void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } - btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } - void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } - btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; } - void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } - bool getPoweredAngMotor() { return m_poweredAngMotor; } - void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } - btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } - void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } - btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } - btScalar getLinearPos() { return m_linPos; } - - - // access for ODE solver - bool getSolveLinLimit() { return m_solveLinLim; } - btScalar getLinDepth() { return m_depth[0]; } - bool getSolveAngLimit() { return m_solveAngLim; } - btScalar getAngDepth() { return m_angDepth; } - // shared code used by ODE solver - void calculateTransforms(const btTransform& transA,const btTransform& transB); - void testLinLimits(); - 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; } - - ///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); -} - - ///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 //SLIDER_CONSTRAINT_H - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp deleted file mode 100644 index 0c7dbd668bb..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp +++ /dev/null @@ -1,255 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 "btSolve2LinearConstraint.h" - -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" - - -void btSolve2LinearConstraint::resolveUnilateralPairConstraint( - btRigidBody* body1, - btRigidBody* body2, - - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1) -{ - (void)linvelA; - (void)linvelB; - (void)angvelB; - (void)angvelA; - - - - imp0 = btScalar(0.); - imp1 = btScalar(0.); - - btScalar len = btFabs(normalA.length()) - btScalar(1.); - if (btFabs(len) >= SIMD_EPSILON) - return; - - btAssert(len < SIMD_EPSILON); - - - //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - - //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - - const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); - const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); - -// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv - btScalar massTerm = btScalar(1.) / (invMassA + invMassB); - - - // calculate rhs (or error) terms - const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping; - const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping; - - - // dC/dv * dv = -C - - // jacobian * impulse = -error - // - - //impulse = jacobianInverse * -error - - // inverting 2x2 symmetric system (offdiagonal are equal!) - // - - - btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); - - //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; - - imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; - - //[a b] [d -c] - //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) - - //[jA nD] * [imp0] = [dv0] - //[nD jB] [imp1] [dv1] - -} - - - -void btSolve2LinearConstraint::resolveBilateralPairConstraint( - btRigidBody* body1, - btRigidBody* body2, - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1) -{ - - (void)linvelA; - (void)linvelB; - (void)angvelA; - (void)angvelB; - - - - imp0 = btScalar(0.); - imp1 = btScalar(0.); - - btScalar len = btFabs(normalA.length()) - btScalar(1.); - if (btFabs(len) >= SIMD_EPSILON) - return; - - btAssert(len < SIMD_EPSILON); - - - //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - - //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - - const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); - const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); - - // calculate rhs (or error) terms - const btScalar dv0 = depthA * m_tau - vel0 * m_damping; - const btScalar dv1 = depthB * m_tau - vel1 * m_damping; - - // dC/dv * dv = -C - - // jacobian * impulse = -error - // - - //impulse = jacobianInverse * -error - - // inverting 2x2 symmetric system (offdiagonal are equal!) - // - - - btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); - - //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; - - imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; - - //[a b] [d -c] - //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) - - //[jA nD] * [imp0] = [dv0] - //[nD jB] [imp1] [dv1] - - if ( imp0 > btScalar(0.0)) - { - if ( imp1 > btScalar(0.0) ) - { - //both positive - } - else - { - imp1 = btScalar(0.); - - // now imp0>0 imp1<0 - imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > btScalar(0.0) ) - { - } else - { - imp0 = btScalar(0.); - } - } - } - else - { - imp0 = btScalar(0.); - - imp1 = dv1 / jacB.getDiagonal(); - if ( imp1 <= btScalar(0.0) ) - { - imp1 = btScalar(0.); - // now imp0>0 imp1<0 - imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > btScalar(0.0) ) - { - } else - { - imp0 = btScalar(0.); - } - } else - { - } - } -} - - -/* -void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btMatrix3x3& invInertiaBWS, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1) -{ - -} -*/ - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h deleted file mode 100644 index 057d3fac827..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h +++ /dev/null @@ -1,107 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 SOLVE_2LINEAR_CONSTRAINT_H -#define SOLVE_2LINEAR_CONSTRAINT_H - -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btVector3.h" - - -class btRigidBody; - - - -/// constraint class used for lateral tyre friction. -class btSolve2LinearConstraint -{ - btScalar m_tau; - btScalar m_damping; - -public: - - btSolve2LinearConstraint(btScalar tau,btScalar damping) - { - m_tau = tau; - m_damping = damping; - } - // - // solve unilateral constraint (equality, direct method) - // - void resolveUnilateralPairConstraint( - btRigidBody* body0, - btRigidBody* body1, - - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1); - - - // - // solving 2x2 lcp problem (inequality, direct solution ) - // - void resolveBilateralPairConstraint( - btRigidBody* body0, - btRigidBody* body1, - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1); - -/* - void resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btMatrix3x3& invInertiaBWS, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1); - -*/ - -}; - -#endif //SOLVE_2LINEAR_CONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btSolverBody.h deleted file mode 100644 index 8de515812ee..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolverBody.h +++ /dev/null @@ -1,191 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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_SOLVER_BODY_H -#define BT_SOLVER_BODY_H - -class btRigidBody; -#include "LinearMath/btVector3.h" -#include "LinearMath/btMatrix3x3.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btAlignedAllocator.h" -#include "LinearMath/btTransformUtil.h" - -///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision -#ifdef BT_USE_SSE -#define USE_SIMD 1 -#endif // - - -#ifdef USE_SIMD - -struct btSimdScalar -{ - SIMD_FORCE_INLINE btSimdScalar() - { - - } - - SIMD_FORCE_INLINE btSimdScalar(float fl) - :m_vec128 (_mm_set1_ps(fl)) - { - } - - SIMD_FORCE_INLINE btSimdScalar(__m128 v128) - :m_vec128(v128) - { - } - union - { - __m128 m_vec128; - float m_floats[4]; - int m_ints[4]; - btScalar m_unusedPadding; - }; - SIMD_FORCE_INLINE __m128 get128() - { - return m_vec128; - } - - SIMD_FORCE_INLINE const __m128 get128() const - { - return m_vec128; - } - - SIMD_FORCE_INLINE void set128(__m128 v128) - { - m_vec128 = v128; - } - - SIMD_FORCE_INLINE operator __m128() - { - return m_vec128; - } - SIMD_FORCE_INLINE operator const __m128() const - { - return m_vec128; - } - - SIMD_FORCE_INLINE operator float() const - { - return m_floats[0]; - } - -}; - -///@brief Return the elementwise product of two btSimdScalar -SIMD_FORCE_INLINE btSimdScalar -operator*(const btSimdScalar& v1, const btSimdScalar& v2) -{ - return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128())); -} - -///@brief Return the elementwise product of two btSimdScalar -SIMD_FORCE_INLINE btSimdScalar -operator+(const btSimdScalar& v1, const btSimdScalar& v2) -{ - return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128())); -} - - -#else -#define btSimdScalar btScalar -#endif - -///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. -ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - btVector3 m_deltaLinearVelocity; - btVector3 m_deltaAngularVelocity; - btVector3 m_angularFactor; - btVector3 m_invMass; - btRigidBody* m_originalBody; - btVector3 m_pushVelocity; - btVector3 m_turnVelocity; - - - 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); - else - velocity.setValue(0,0,0); - } - - SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const - { - if (m_originalBody) - angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity; - else - angVel.setValue(0,0,0); - } - - - //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) - { - m_deltaLinearVelocity += linearComponent*impulseMagnitude; - m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } - } - - SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) - { - if (m_originalBody) - { - m_pushVelocity += linearComponent*impulseMagnitude; - m_turnVelocity += 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_originalBody->setCompanionId(-1); - } - } - - - void writebackVelocity(btScalar timeStep) - { - (void) timeStep; - if (m_originalBody) - { - m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); - m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+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); - - //m_originalBody->setCompanionId(-1); - } - } - - - -}; - -#endif //BT_SOLVER_BODY_H - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btSolverConstraint.h deleted file mode 100644 index 929cf6d3ee3..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ /dev/null @@ -1,96 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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_SOLVER_CONSTRAINT_H -#define BT_SOLVER_CONSTRAINT_H - -class btRigidBody; -#include "LinearMath/btVector3.h" -#include "LinearMath/btMatrix3x3.h" -#include "btJacobianEntry.h" - -//#define NO_FRICTION_TANGENTIALS 1 -#include "btSolverBody.h" - - -///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. -ATTRIBUTE_ALIGNED64 (struct) btSolverConstraint -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btVector3 m_relpos1CrossNormal; - btVector3 m_contactNormal; - - btVector3 m_relpos2CrossNormal; - //btVector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal - - btVector3 m_angularComponentA; - btVector3 m_angularComponentB; - - 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 - { - btRigidBody* m_solverBodyA; - btScalar m_unusedPadding2; - }; - union - { - btRigidBody* m_solverBodyB; - btScalar m_unusedPadding3; - }; - - union - { - void* m_originalContactPoint; - btScalar m_unusedPadding4; - }; - - btScalar m_rhs; - btScalar m_cfm; - btScalar m_lowerLimit; - btScalar m_upperLimit; - - btScalar m_rhsPenetration; - - enum btSolverConstraintType - { - BT_SOLVER_CONTACT_1D = 0, - BT_SOLVER_FRICTION_1D - }; -}; - -typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray; - - -#endif //BT_SOLVER_CONSTRAINT_H - - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp deleted file mode 100644 index e7c94b19308..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ /dev/null @@ -1,136 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 "btTypedConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btSerializer.h" - - -#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f) - -btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) -:btTypedObject(type), -m_userConstraintType(-1), -m_userConstraintId(-1), -m_needsFeedback(false), -m_rbA(rbA), -m_rbB(getFixedBody()), -m_appliedImpulse(btScalar(0.)), -m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) -{ -} - - -btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB) -:btTypedObject(type), -m_userConstraintType(-1), -m_userConstraintId(-1), -m_needsFeedback(false), -m_rbA(rbA), -m_rbB(rbB), -m_appliedImpulse(btScalar(0.)), -m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) -{ -} - - - - -btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact) -{ - if(lowLim > uppLim) - { - return btScalar(1.0f); - } - else if(lowLim == uppLim) - { - return btScalar(0.0f); - } - btScalar lim_fact = btScalar(1.0f); - btScalar delta_max = vel / timeFact; - if(delta_max < btScalar(0.0f)) - { - if((pos >= lowLim) && (pos < (lowLim - delta_max))) - { - lim_fact = (lowLim - pos) / delta_max; - } - else if(pos < lowLim) - { - lim_fact = btScalar(0.0f); - } - else - { - lim_fact = btScalar(1.0f); - } - } - else if(delta_max > btScalar(0.0f)) - { - if((pos <= uppLim) && (pos > (uppLim - delta_max))) - { - lim_fact = (uppLim - pos) / delta_max; - } - else if(pos > uppLim) - { - lim_fact = btScalar(0.0f); - } - else - { - lim_fact = btScalar(1.0f); - } - } - else - { - lim_fact = btScalar(0.0f); - } - 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_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"; -} - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.h deleted file mode 100644 index b24dc4a40ed..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ /dev/null @@ -1,302 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -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. -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 TYPED_CONSTRAINT_H -#define TYPED_CONSTRAINT_H - -class btRigidBody; -#include "LinearMath/btScalar.h" -#include "btSolverConstraint.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" - -class btSerializer; - -enum btTypedConstraintType -{ - POINT2POINT_CONSTRAINT_TYPE=MAX_CONTACT_MANIFOLD_TYPE+1, - HINGE_CONSTRAINT_TYPE, - CONETWIST_CONSTRAINT_TYPE, - D6_CONSTRAINT_TYPE, - SLIDER_CONSTRAINT_TYPE, - CONTACT_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 - - -///TypedConstraint is the baseclass for Bullet constraints and vehicles -class btTypedConstraint : public btTypedObject -{ - int m_userConstraintType; - int m_userConstraintId; - bool m_needsFeedback; - - btTypedConstraint& operator=(btTypedConstraint& other) - { - btAssert(0); - (void) other; - return *this; - } - -protected: - btRigidBody& m_rbA; - btRigidBody& m_rbB; - btScalar m_appliedImpulse; - btScalar m_dbgDrawSize; - - ///internal method used by the constraint solver, don't use them directly - btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact); - - static btRigidBody& getFixedBody() - { - static btRigidBody s_fixed(0, 0,0); - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); - return s_fixed; - } - - -public: - - virtual ~btTypedConstraint() {}; - btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA); - btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB); - - struct btConstraintInfo1 { - int m_numConstraintRows,nub; - }; - - struct btConstraintInfo2 { - // integrator parameters: frames per second (1/stepsize), default error - // reduction parameter (0..1). - btScalar fps,erp; - - // for the first and second body, pointers to two (linear and angular) - // n*3 jacobian sub matrices, stored by rows. these matrices will have - // been initialized to 0 on entry. if the second body is zero then the - // J2xx pointers may be 0. - btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis; - - // elements to jump from one row to the next in J's - int rowskip; - - // right hand sides of the equation J*v = c + cfm * lambda. cfm is the - // "constraint force mixing" vector. c is set to zero on entry, cfm is - // set to a constant value (typically very small or zero) value on entry. - btScalar *m_constraintError,*cfm; - - // lo and hi limits for variables (set to -/+ infinity on entry). - btScalar *m_lowerLimit,*m_upperLimit; - - // findex vector for variables. see the LCP solver interface for a - // description of what this does. this is set to -1 on entry. - // note that the returned indexes are relative to the first index of - // the constraint. - int *findex; - // number of solver iterations - int m_numIterations; - }; - - ///internal method used by the constraint solver, don't use them directly - 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 - virtual void getInfo1 (btConstraintInfo1* info)=0; - - ///internal method used by the constraint solver, don't use them directly - virtual void getInfo2 (btConstraintInfo2* info)=0; - - ///internal method used by the constraint solver, don't use them directly - void internalSetAppliedImpulse(btScalar appliedImpulse) - { - m_appliedImpulse = appliedImpulse; - } - ///internal method used by the constraint solver, don't use them directly - btScalar internalGetAppliedImpulse() - { - return m_appliedImpulse; - } - - ///internal method used by the constraint solver, don't use them directly - virtual void solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep) {}; - - - const btRigidBody& getRigidBodyA() const - { - return m_rbA; - } - const btRigidBody& getRigidBodyB() const - { - return m_rbB; - } - - btRigidBody& getRigidBodyA() - { - return m_rbA; - } - btRigidBody& getRigidBodyB() - { - return m_rbB; - } - - 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; - } - - int getUid() const - { - return m_userConstraintId; - } - - bool needsFeedback() const - { - return m_needsFeedback; - } - - ///enableFeedback will allow to read the applied linear and angular impulse - ///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information - void enableFeedback(bool needsFeedback) - { - m_needsFeedback = needsFeedback; - } - - ///getAppliedImpulse is an estimated total applied impulse. - ///This feedback could be used to determine breaking constraints or playing sounds. - btScalar getAppliedImpulse() const - { - btAssert(m_needsFeedback); - return m_appliedImpulse; - } - - btTypedConstraintType getConstraintType () const - { - return btTypedConstraintType(m_objectType); - } - - void setDbgDrawSize(btScalar dbgDrawSize) - { - m_dbgDrawSize = dbgDrawSize; - } - btScalar getDbgDrawSize() - { - 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 -// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI]) -SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians) -{ - if(angleLowerLimitInRadians >= angleUpperLimitInRadians) - { - return angleInRadians; - } - else if(angleInRadians < angleLowerLimitInRadians) - { - btScalar diffLo = btNormalizeAngle(angleLowerLimitInRadians - angleInRadians); // this is positive - 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 diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians)); - return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians; - } - else - { - return angleInRadians; - } -} - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btTypedConstraintData -{ - btRigidBodyData *m_rbA; - btRigidBodyData *m_rbB; - char *m_name; - - int m_objectType; - int m_userConstraintType; - int m_userConstraintId; - int m_needsFeedback; - - float m_appliedImpulse; - float m_dbgDrawSize; - - int m_disableCollisionsBetweenLinkedBodies; - char m_pad4[4]; - -}; - -SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const -{ - return sizeof(btTypedConstraintData); -} - - - - -#endif //TYPED_CONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp deleted file mode 100644 index 3a4c2afa642..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -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 "btUniversalConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" - - - -#define UNIV_EPS btScalar(0.01f) - - -// 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) -: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), - m_anchor(anchor), - m_axis1(axis1), - m_axis2(axis2) -{ - // build frame basis - // 6DOF constraint uses Euler angles and to define limits - // it is assumed that rotational order is : - // Z - first, allowed limits are (-PI,PI); - // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number - // used to prevent constraint from instability on poles; - // 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 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(anchor); - // now get constraint frame in local coordinate systems - m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; - m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; - // sei limits - setLinearLowerLimit(btVector3(0., 0., 0.)); - setLinearUpperLimit(btVector3(0., 0., 0.)); - setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS)); - setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS)); -} - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.h deleted file mode 100644 index 4e64a7d7e0e..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.h +++ /dev/null @@ -1,60 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -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 UNIVERSAL_CONSTRAINT_H -#define UNIVERSAL_CONSTRAINT_H - - - -#include "LinearMath/btVector3.h" -#include "btTypedConstraint.h" -#include "btGeneric6DofConstraint.h" - - - -/// Constraint similar to ODE Universal Joint -/// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1) -/// and Y (axis 2) -/// Description from ODE manual : -/// "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 -{ -protected: - btVector3 m_anchor; - btVector3 m_axis1; - btVector3 m_axis2; -public: - // 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); - // access - const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); } - const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); } - const btVector3& getAxis1() { return m_axis1; } - const btVector3& getAxis2() { return m_axis2; } - btScalar getAngle1() { return getAngle(2); } - btScalar getAngle2() { return getAngle(1); } - // 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)); } -}; - - - -#endif // UNIVERSAL_CONSTRAINT_H - diff --git a/extern/bullet2/BulletDynamics/Dynamics/Bullet-C-API.cpp b/extern/bullet2/BulletDynamics/Dynamics/Bullet-C-API.cpp deleted file mode 100644 index bd8e2748383..00000000000 --- a/extern/bullet2/BulletDynamics/Dynamics/Bullet-C-API.cpp +++ /dev/null @@ -1,405 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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. -*/ - -/* - Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's. - Work in progress, functionality will be added on demand. - - If possible, use the richer Bullet C++ API, by including <src/btBulletDynamicsCommon.h> -*/ - -#include "Bullet-C-Api.h" -#include "btBulletDynamicsCommon.h" -#include "LinearMath/btAlignedAllocator.h" - - - -#include "LinearMath/btVector3.h" -#include "LinearMath/btScalar.h" -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btTransform.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" - -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" -#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" -#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" - - -/* - Create and Delete a Physics SDK -*/ - -struct btPhysicsSdk -{ - -// btDispatcher* m_dispatcher; -// btOverlappingPairCache* m_pairCache; -// btConstraintSolver* m_constraintSolver - - btVector3 m_worldAabbMin; - btVector3 m_worldAabbMax; - - - //todo: version, hardware/optimization settings etc? - btPhysicsSdk() - :m_worldAabbMin(-1000,-1000,-1000), - m_worldAabbMax(1000,1000,1000) - { - - } - - -}; - -plPhysicsSdkHandle plNewBulletSdk() -{ - void* mem = btAlignedAlloc(sizeof(btPhysicsSdk),16); - return (plPhysicsSdkHandle)new (mem)btPhysicsSdk; -} - -void plDeletePhysicsSdk(plPhysicsSdkHandle physicsSdk) -{ - btPhysicsSdk* phys = reinterpret_cast<btPhysicsSdk*>(physicsSdk); - btAlignedFree(phys); -} - - -/* Dynamics World */ -plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle) -{ - btPhysicsSdk* physicsSdk = reinterpret_cast<btPhysicsSdk*>(physicsSdkHandle); - void* mem = btAlignedAlloc(sizeof(btDefaultCollisionConfiguration),16); - btDefaultCollisionConfiguration* collisionConfiguration = new (mem)btDefaultCollisionConfiguration(); - mem = btAlignedAlloc(sizeof(btCollisionDispatcher),16); - btDispatcher* dispatcher = new (mem)btCollisionDispatcher(collisionConfiguration); - mem = btAlignedAlloc(sizeof(btAxisSweep3),16); - btBroadphaseInterface* pairCache = new (mem)btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax); - mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16); - btConstraintSolver* constraintSolver = new(mem) btSequentialImpulseConstraintSolver(); - - mem = btAlignedAlloc(sizeof(btDiscreteDynamicsWorld),16); - return (plDynamicsWorldHandle) new (mem)btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration); -} -void plDeleteDynamicsWorld(plDynamicsWorldHandle world) -{ - //todo: also clean up the other allocations, axisSweep, pairCache,dispatcher,constraintSolver,collisionConfiguration - btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - btAlignedFree(dynamicsWorld); -} - -void plStepSimulation(plDynamicsWorldHandle world, plReal timeStep) -{ - btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - btAssert(dynamicsWorld); - dynamicsWorld->stepSimulation(timeStep); -} - -void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) -{ - btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - btAssert(dynamicsWorld); - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - - dynamicsWorld->addRigidBody(body); -} - -void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) -{ - btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - btAssert(dynamicsWorld); - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - - dynamicsWorld->removeRigidBody(body); -} - -/* Rigid Body */ - -plRigidBodyHandle plCreateRigidBody( void* user_data, float mass, plCollisionShapeHandle cshape ) -{ - btTransform trans; - trans.setIdentity(); - btVector3 localInertia(0,0,0); - btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); - btAssert(shape); - if (mass) - { - shape->calculateLocalInertia(mass,localInertia); - } - void* mem = btAlignedAlloc(sizeof(btRigidBody),16); - btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia); - btRigidBody* body = new (mem)btRigidBody(rbci); - body->setWorldTransform(trans); - body->setUserPointer(user_data); - return (plRigidBodyHandle) body; -} - -void plDeleteRigidBody(plRigidBodyHandle cbody) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody); - btAssert(body); - btAlignedFree( body); -} - - -/* Collision Shape definition */ - -plCollisionShapeHandle plNewSphereShape(plReal radius) -{ - void* mem = btAlignedAlloc(sizeof(btSphereShape),16); - return (plCollisionShapeHandle) new (mem)btSphereShape(radius); - -} - -plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z) -{ - void* mem = btAlignedAlloc(sizeof(btBoxShape),16); - return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z)); -} - -plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height) -{ - //capsule is convex hull of 2 spheres, so use btMultiSphereShape - - const int numSpheres = 2; - btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)}; - btScalar radi[numSpheres] = {radius,radius}; - void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16); - return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres); -} -plCollisionShapeHandle plNewConeShape(plReal radius, plReal height) -{ - void* mem = btAlignedAlloc(sizeof(btConeShape),16); - return (plCollisionShapeHandle) new (mem)btConeShape(radius,height); -} - -plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height) -{ - void* mem = btAlignedAlloc(sizeof(btCylinderShape),16); - return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius)); -} - -/* Convex Meshes */ -plCollisionShapeHandle plNewConvexHullShape() -{ - void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16); - return (plCollisionShapeHandle) new (mem)btConvexHullShape(); -} - - -/* Concave static triangle meshes */ -plMeshInterfaceHandle plNewMeshInterface() -{ - return 0; -} - -plCollisionShapeHandle plNewCompoundShape() -{ - void* mem = btAlignedAlloc(sizeof(btCompoundShape),16); - return (plCollisionShapeHandle) new (mem)btCompoundShape(); -} - -void plAddChildShape(plCollisionShapeHandle compoundShapeHandle,plCollisionShapeHandle childShapeHandle, plVector3 childPos,plQuaternion childOrn) -{ - btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>(compoundShapeHandle); - btAssert(colShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE); - btCompoundShape* compoundShape = reinterpret_cast<btCompoundShape*>(colShape); - btCollisionShape* childShape = reinterpret_cast<btCollisionShape*>(childShapeHandle); - btTransform localTrans; - localTrans.setIdentity(); - localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2])); - localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3])); - compoundShape->addChildShape(localTrans,childShape); -} - -void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient) -{ - btQuaternion orn; - orn.setEuler(yaw,pitch,roll); - orient[0] = orn.getX(); - orient[1] = orn.getY(); - orient[2] = orn.getZ(); - orient[3] = orn.getW(); - -} - - -// extern void plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2); -// extern plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle); - - -void plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z) -{ - btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>( cshape); - (void)colShape; - btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE); - btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape); - convexHullShape->addPoint(btVector3(x,y,z)); - -} - -void plDeleteShape(plCollisionShapeHandle cshape) -{ - btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); - btAssert(shape); - btAlignedFree(shape); -} -void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling) -{ - btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); - btAssert(shape); - btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]); - shape->setLocalScaling(scaling); -} - - - -void plSetPosition(plRigidBodyHandle object, const plVector3 position) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - btVector3 pos(position[0],position[1],position[2]); - btTransform worldTrans = body->getWorldTransform(); - worldTrans.setOrigin(pos); - body->setWorldTransform(worldTrans); -} - -void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]); - btTransform worldTrans = body->getWorldTransform(); - worldTrans.setRotation(orn); - body->setWorldTransform(worldTrans); -} - -void plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - btTransform& worldTrans = body->getWorldTransform(); - worldTrans.setFromOpenGLMatrix(matrix); -} - -void plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - body->getWorldTransform().getOpenGLMatrix(matrix); - -} - -void plGetPosition(plRigidBodyHandle object,plVector3 position) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - const btVector3& pos = body->getWorldTransform().getOrigin(); - position[0] = pos.getX(); - position[1] = pos.getY(); - position[2] = pos.getZ(); -} - -void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - const btQuaternion& orn = body->getWorldTransform().getRotation(); - orientation[0] = orn.getX(); - orientation[1] = orn.getY(); - orientation[2] = orn.getZ(); - orientation[3] = orn.getW(); -} - - - -//plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); - -// extern plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); - -double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3]) -{ - btVector3 vp(p1[0], p1[1], p1[2]); - btTriangleShape trishapeA(vp, - btVector3(p2[0], p2[1], p2[2]), - btVector3(p3[0], p3[1], p3[2])); - trishapeA.setMargin(0.000001f); - btVector3 vq(q1[0], q1[1], q1[2]); - btTriangleShape trishapeB(vq, - btVector3(q2[0], q2[1], q2[2]), - btVector3(q3[0], q3[1], q3[2])); - trishapeB.setMargin(0.000001f); - - // btVoronoiSimplexSolver sGjkSimplexSolver; - // btGjkEpaPenetrationDepthSolver penSolverPtr; - - static btSimplexSolverInterface sGjkSimplexSolver; - sGjkSimplexSolver.reset(); - - static btGjkEpaPenetrationDepthSolver Solver0; - static btMinkowskiPenetrationDepthSolver Solver1; - - btConvexPenetrationDepthSolver* Solver = NULL; - - Solver = &Solver1; - - btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver); - - convexConvex.m_catchDegeneracies = 1; - - // btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0); - - btPointCollector gjkOutput; - btGjkPairDetector::ClosestPointInput input; - - - btTransform tr; - tr.setIdentity(); - - input.m_transformA = tr; - input.m_transformB = tr; - - convexConvex.getClosestPoints(input, gjkOutput, 0); - - - if (gjkOutput.m_hasResult) - { - - pb[0] = pa[0] = gjkOutput.m_pointInWorld[0]; - pb[1] = pa[1] = gjkOutput.m_pointInWorld[1]; - pb[2] = pa[2] = gjkOutput.m_pointInWorld[2]; - - pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance; - pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance; - pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance; - - normal[0] = gjkOutput.m_normalOnBInWorld[0]; - normal[1] = gjkOutput.m_normalOnBInWorld[1]; - normal[2] = gjkOutput.m_normalOnBInWorld[2]; - - return gjkOutput.m_distance; - } - return -1.0f; -} - diff --git a/extern/bullet2/BulletDynamics/Dynamics/btActionInterface.h b/extern/bullet2/BulletDynamics/Dynamics/btActionInterface.h deleted file mode 100644 index 40a07c6eae3..00000000000 --- a/extern/bullet2/BulletDynamics/Dynamics/btActionInterface.h +++ /dev/null @@ -1,50 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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_ACTION_INTERFACE_H -#define _BT_ACTION_INTERFACE_H - -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 -{ -protected: - - static btRigidBody& getFixedBody() - { - static btRigidBody s_fixed(0, 0,0); - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); - return s_fixed; - } - -public: - - virtual ~btActionInterface() - { - } - - virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0; - - virtual void debugDraw(btIDebugDraw* debugDrawer) = 0; - -}; - -#endif //_BT_ACTION_INTERFACE_H - diff --git a/extern/bullet2/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp b/extern/bullet2/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp deleted file mode 100644 index 23501c4435e..00000000000 --- a/extern/bullet2/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp +++ /dev/null @@ -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); - - - -} - - - diff --git a/extern/bullet2/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h b/extern/bullet2/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h deleted file mode 100644 index 61c8dea03eb..00000000000 --- a/extern/bullet2/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h +++ /dev/null @@ -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 diff --git a/extern/bullet2/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp deleted file mode 100644 index a88ed1cafa8..00000000000 --- a/extern/bullet2/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ /dev/null @@ -1,1161 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 "btDiscreteDynamicsWorld.h" - -//collision detection -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btQuickprof.h" - -//rigidbody & constraints -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" -#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" -#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" -#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" - -#include "LinearMath/btIDebugDraw.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" - - -#include "BulletDynamics/Dynamics/btActionInterface.h" -#include "LinearMath/btQuickprof.h" -#include "LinearMath/btMotionState.h" - -#include "LinearMath/btSerializer.h" - - - -btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) -:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), -m_constraintSolver(constraintSolver), -m_gravity(0,-10,0), -m_localTime(btScalar(1.)/btScalar(60.)), -m_synchronizeAllMotionStates(false), -m_profileTimings(0) -{ - if (!m_constraintSolver) - { - void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16); - m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver; - m_ownsConstraintSolver = true; - } else - { - m_ownsConstraintSolver = false; - } - - { - void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16); - m_islandManager = new (mem) btSimulationIslandManager(); - } - - m_ownsIslandManager = true; -} - - -btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() -{ - //only delete it when we created it - if (m_ownsIslandManager) - { - m_islandManager->~btSimulationIslandManager(); - btAlignedFree( m_islandManager); - } - if (m_ownsConstraintSolver) - { - - m_constraintSolver->~btConstraintSolver(); - btAlignedFree(m_constraintSolver); - } -} - -void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) -{ -///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows -///to switch status _after_ adding kinematic objects to the world -///fix it for Bullet 3.x release - for (int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body && body->getActivationState() != ISLAND_SLEEPING) - { - if (body->isKinematicObject()) - { - //to calculate velocities next frame - body->saveKinematicState(timeStep); - } - } - } - -} - -void btDiscreteDynamicsWorld::debugDrawWorld() -{ - BT_PROFILE("debugDrawWorld"); - - btCollisionWorld::debugDrawWorld(); - - bool drawConstraints = false; - if (getDebugDrawer()) - { - int mode = getDebugDrawer()->getDebugMode(); - if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) - { - drawConstraints = true; - } - } - if(drawConstraints) - { - for(int i = getNumConstraints()-1; i>=0 ;i--) - { - btTypedConstraint* constraint = getConstraint(i); - debugDrawConstraint(constraint); - } - } - - - - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) - { - int i; - - if (getDebugDrawer() && getDebugDrawer()->getDebugMode()) - { - for (i=0;i<m_actions.size();i++) - { - m_actions[i]->debugDraw(m_debugDrawer); - } - } - } -} - -void btDiscreteDynamicsWorld::clearForces() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - //need to check if next line is ok - //it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up - body->clearForces(); - } -} - -///apply gravity, call this once per timestep -void btDiscreteDynamicsWorld::applyGravity() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - if (body->isActive()) - { - body->applyGravity(); - } - } -} - - -void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) -{ - btAssert(body); - - if (body->getMotionState() && !body->isStaticOrKinematicObject()) - { - //we need to call the update at least once, even for sleeping objects - //otherwise the 'graphics' transform never updates properly - ///@todo: add 'dirty' flag - //if (body->getActivationState() != ISLAND_SLEEPING) - { - btTransform interpolatedTransform; - btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform); - body->getMotionState()->setWorldTransform(interpolatedTransform); - } - } -} - - -void btDiscreteDynamicsWorld::synchronizeMotionStates() -{ - BT_PROFILE("synchronizeMotionStates"); - if (m_synchronizeAllMotionStates) - { - //iterate over all collision objects - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - synchronizeSingleMotionState(body); - } - } else - { - //iterate over all active rigid bodies - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - if (body->isActive()) - synchronizeSingleMotionState(body); - } - } -} - - -int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) -{ - startProfiling(timeStep); - - BT_PROFILE("stepSimulation"); - - int numSimulationSubSteps = 0; - - if (maxSubSteps) - { - //fixed timestep with interpolation - m_localTime += timeStep; - if (m_localTime >= fixedTimeStep) - { - numSimulationSubSteps = int( m_localTime / fixedTimeStep); - m_localTime -= numSimulationSubSteps * fixedTimeStep; - } - } else - { - //variable timestep - fixedTimeStep = timeStep; - m_localTime = timeStep; - if (btFuzzyZero(timeStep)) - { - numSimulationSubSteps = 0; - maxSubSteps = 0; - } else - { - numSimulationSubSteps = 1; - maxSubSteps = 1; - } - } - - //process some debugging flags - if (getDebugDrawer()) - { - btIDebugDraw* debugDrawer = getDebugDrawer (); - gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; - } - if (numSimulationSubSteps) - { - - //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt - int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; - - saveKinematicState(fixedTimeStep*clampedSimulationSteps); - - applyGravity(); - - - - for (int i=0;i<clampedSimulationSteps;i++) - { - internalSingleStepSimulation(fixedTimeStep); - synchronizeMotionStates(); - } - - } else - { - synchronizeMotionStates(); - } - - clearForces(); - -#ifndef BT_NO_PROFILE - CProfileManager::Increment_Frame_Counter(); -#endif //BT_NO_PROFILE - - return numSimulationSubSteps; -} - -void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) -{ - - BT_PROFILE("internalSingleStepSimulation"); - - if(0 != m_internalPreTickCallback) { - (*m_internalPreTickCallback)(this, timeStep); - } - - ///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(); - - ///integrate transforms - integrateTransforms(timeStep); - - ///update vehicle simulation - updateActions(timeStep); - - updateActivationState( timeStep ); - - if(0 != m_internalTickCallback) { - (*m_internalTickCallback)(this, timeStep); - } -} - -void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) -{ - m_gravity = gravity; - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) - { - body->setGravity(gravity); - } - } -} - -btVector3 btDiscreteDynamicsWorld::getGravity () const -{ - return m_gravity; -} - -void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) -{ - btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask); -} - -void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) -{ - btRigidBody* body = btRigidBody::upcast(collisionObject); - if (body) - removeRigidBody(body); - else - btCollisionWorld::removeCollisionObject(collisionObject); -} - -void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) -{ - m_nonStaticRigidBodies.remove(body); - btCollisionWorld::removeCollisionObject(body); -} - - -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) -{ - if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) - { - body->setGravity(m_gravity); - } - - if (body->getCollisionShape()) - { - if (!body->isStaticObject()) - { - m_nonStaticRigidBodies.push_back(body); - } else - { - body->setActivationState(ISLAND_SLEEPING); - } - - bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); - short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); - short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - addCollisionObject(body,collisionFilterGroup,collisionFilterMask); - } -} - -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask) -{ - if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) - { - body->setGravity(m_gravity); - } - - if (body->getCollisionShape()) - { - if (!body->isStaticObject()) - { - m_nonStaticRigidBodies.push_back(body); - } - else - { - body->setActivationState(ISLAND_SLEEPING); - } - addCollisionObject(body,group,mask); - } -} - - -void btDiscreteDynamicsWorld::updateActions(btScalar timeStep) -{ - BT_PROFILE("updateActions"); - - for ( int i=0;i<m_actions.size();i++) - { - m_actions[i]->updateAction( this, timeStep); - } -} - - -void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) -{ - BT_PROFILE("updateActivationState"); - - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - if (body) - { - body->updateDeactivation(timeStep); - - if (body->wantsSleeping()) - { - if (body->isStaticOrKinematicObject()) - { - body->setActivationState(ISLAND_SLEEPING); - } else - { - if (body->getActivationState() == ACTIVE_TAG) - body->setActivationState( WANTS_DEACTIVATION ); - if (body->getActivationState() == ISLAND_SLEEPING) - { - body->setAngularVelocity(btVector3(0,0,0)); - body->setLinearVelocity(btVector3(0,0,0)); - } - - } - } else - { - if (body->getActivationState() != DISABLE_DEACTIVATION) - body->setActivationState( ACTIVE_TAG ); - } - } - } -} - -void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies) -{ - m_constraints.push_back(constraint); - if (disableCollisionsBetweenLinkedBodies) - { - constraint->getRigidBodyA().addConstraintRef(constraint); - constraint->getRigidBodyB().addConstraintRef(constraint); - } -} - -void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) -{ - m_constraints.remove(constraint); - constraint->getRigidBodyA().removeConstraintRef(constraint); - constraint->getRigidBodyB().removeConstraintRef(constraint); -} - -void btDiscreteDynamicsWorld::addAction(btActionInterface* action) -{ - m_actions.push_back(action); -} - -void btDiscreteDynamicsWorld::removeAction(btActionInterface* action) -{ - m_actions.remove(action); -} - - -void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle) -{ - addAction(vehicle); -} - -void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle) -{ - removeAction(vehicle); -} - -void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character) -{ - addAction(character); -} - -void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character) -{ - removeAction(character); -} - - -SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) -{ - int islandId; - - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); - return islandId; - -} - - -class btSortConstraintOnIslandPredicate -{ - public: - - bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetConstraintIslandId(rhs); - lIslandId0 = btGetConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } -}; - - - -void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) -{ - BT_PROFILE("solveConstraints"); - - struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback - { - - btContactSolverInfo& m_solverInfo; - btConstraintSolver* m_solver; - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btStackAlloc* m_stackAlloc; - btDispatcher* m_dispatcher; - - btAlignedObjectArray<btCollisionObject*> m_bodies; - btAlignedObjectArray<btPersistentManifold*> m_manifolds; - btAlignedObjectArray<btTypedConstraint*> m_constraints; - - - InplaceSolverIslandCallback( - btContactSolverInfo& solverInfo, - btConstraintSolver* solver, - btTypedConstraint** sortedConstraints, - int numConstraints, - btIDebugDraw* debugDrawer, - btStackAlloc* stackAlloc, - btDispatcher* dispatcher) - :m_solverInfo(solverInfo), - m_solver(solver), - m_sortedConstraints(sortedConstraints), - m_numConstraints(numConstraints), - m_debugDrawer(debugDrawer), - m_stackAlloc(stackAlloc), - m_dispatcher(dispatcher) - { - - } - - - InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) - { - btAssert(0); - (void)other; - return *this; - } - virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) - { - if (islandId<0) - { - if (numManifolds + m_numConstraints) - { - ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); - } - } else - { - //also add all non-contact constraints/joints for this island - btTypedConstraint** startConstraint = 0; - int numCurConstraints = 0; - int i; - - //find the first constraint for this island - for (i=0;i<m_numConstraints;i++) - { - if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) - { - startConstraint = &m_sortedConstraints[i]; - break; - } - } - //count the number of constraints in this island - for (;i<m_numConstraints;i++) - { - if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) - { - numCurConstraints++; - } - } - - if (m_solverInfo.m_minimumSolverBatchSize<=1) - { - ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive - if (numManifolds + numCurConstraints) - { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); - } - } else - { - - for (i=0;i<numBodies;i++) - m_bodies.push_back(bodies[i]); - for (i=0;i<numManifolds;i++) - m_manifolds.push_back(manifolds[i]); - for (i=0;i<numCurConstraints;i++) - m_constraints.push_back(startConstraint[i]); - if ((m_constraints.size()+m_manifolds.size())>m_solverInfo.m_minimumSolverBatchSize) - { - processConstraints(); - } else - { - //printf("deferred\n"); - } - } - } - } - void processConstraints() - { - if (m_manifolds.size() + m_constraints.size()>0) - { - m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); - } - m_bodies.resize(0); - m_manifolds.resize(0); - m_constraints.resize(0); - - } - - }; - - - - //sorted version of all btTypedConstraint, based on islandId - btAlignedObjectArray<btTypedConstraint*> sortedConstraints; - sortedConstraints.resize( m_constraints.size()); - int i; - for (i=0;i<getNumConstraints();i++) - { - sortedConstraints[i] = m_constraints[i]; - } - -// btAssert(0); - - - - sortedConstraints.quickSort(btSortConstraintOnIslandPredicate()); - - btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0; - - InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc,m_dispatcher1); - - m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); - - /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback); - - solverCallback.processConstraints(); - - m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc); -} - - - - -void btDiscreteDynamicsWorld::calculateSimulationIslands() -{ - BT_PROFILE("calculateSimulationIslands"); - - getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); - - { - int i; - int numConstraints = int(m_constraints.size()); - for (i=0;i< numConstraints ; i++ ) - { - btTypedConstraint* constraint = m_constraints[i]; - - const btRigidBody* colObj0 = &constraint->getRigidBodyA(); - const btRigidBody* colObj1 = &constraint->getRigidBodyB(); - - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - if (colObj0->isActive() || colObj1->isActive()) - { - - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), - (colObj1)->getIslandTag()); - } - } - } - } - - //Store the island id in each body - getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - - -} - - - - -class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback -{ - btCollisionObject* m_me; - btScalar m_allowedPenetration; - btOverlappingPairCache* m_pairCache; - btDispatcher* m_dispatcher; - - -public: - btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : - btCollisionWorld::ClosestConvexResultCallback(fromA,toA), - m_me(me), - m_allowedPenetration(0.0f), - m_pairCache(pairCache), - m_dispatcher(dispatcher) - { - } - - virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) - { - if (convexResult.m_hitCollisionObject == m_me) - return 1.0f; - - //ignore result if there is no contact response - if(!convexResult.m_hitCollisionObject->hasContactResponse()) - return 1.0f; - - btVector3 linVelA,linVelB; - linVelA = m_convexToWorld-m_convexFromWorld; - linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin(); - - btVector3 relativeVelocity = (linVelA-linVelB); - //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration) - return 1.f; - - return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); - } - - virtual bool needsCollision(btBroadphaseProxy* proxy0) const - { - //don't collide with itself - if (proxy0->m_clientObject == m_me) - return false; - - ///don't do CCD when the collision filters are not matching - if (!ClosestConvexResultCallback::needsCollision(proxy0)) - return false; - - btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; - - //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179 - if (m_dispatcher->needsResponse(m_me,otherObj)) - { - ///don't do CCD when there are already contact points (touching contact/penetration) - btAlignedObjectArray<btPersistentManifold*> manifoldArray; - btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); - if (collisionPair) - { - if (collisionPair->m_algorithm) - { - manifoldArray.resize(0); - collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); - for (int j=0;j<manifoldArray.size();j++) - { - btPersistentManifold* manifold = manifoldArray[j]; - if (manifold->getNumContacts()>0) - return false; - } - } - } - } - return true; - } - - -}; - -///internal debugging variable. this value shouldn't be too high -int gNumClampedCcdMotions=0; - -//#include "stdio.h" -void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) -{ - BT_PROFILE("integrateTransforms"); - btTransform predictedTrans; - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - body->setHitFraction(1.f); - - if (body->isActive() && (!body->isStaticOrKinematicObject())) - { - body->predictIntegratedTransform(timeStep, predictedTrans); - btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - - if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) - { - BT_PROFILE("CCD motion clamping"); - if (body->getCollisionShape()->isConvex()) - { - gNumClampedCcdMotions++; - - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); - //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); - btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); - - sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; - - convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults); - if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) - { - body->setHitFraction(sweepResults.m_closestHitFraction); - body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); - body->setHitFraction(0.f); -// printf("clamped integration to hit fraction = %f\n",fraction); - } - } - } - - body->proceedToTransform( predictedTrans); - } - } -} - - - - - -void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - BT_PROFILE("predictUnconstraintMotion"); - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - if (!body->isStaticOrKinematicObject()) - { - body->integrateVelocities( timeStep); - //damping - body->applyDamping(timeStep); - - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); - } - } -} - - -void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) -{ - (void)timeStep; - -#ifndef BT_NO_PROFILE - CProfileManager::Reset(); -#endif //BT_NO_PROFILE - -} - - - - - - -void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) -{ - bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0; - bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0; - btScalar dbgDrawSize = constraint->getDbgDrawSize(); - if(dbgDrawSize <= btScalar(0.f)) - { - return; - } - - switch(constraint->getConstraintType()) - { - case POINT2POINT_CONSTRAINT_TYPE: - { - btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; - btTransform tr; - tr.setIdentity(); - btVector3 pivot = p2pC->getPivotInA(); - pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; - tr.setOrigin(pivot); - getDebugDrawer()->drawTransform(tr, dbgDrawSize); - // that ideally should draw the same frame - pivot = p2pC->getPivotInB(); - pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; - tr.setOrigin(pivot); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - } - break; - case HINGE_CONSTRAINT_TYPE: - { - btHingeConstraint* pHinge = (btHingeConstraint*)constraint; - btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - btScalar minAng = pHinge->getLowerLimit(); - btScalar maxAng = pHinge->getUpperLimit(); - if(minAng == maxAng) - { - break; - } - bool drawSect = true; - if(minAng > maxAng) - { - minAng = btScalar(0.f); - maxAng = SIMD_2_PI; - drawSect = false; - } - if(drawLimits) - { - btVector3& center = tr.getOrigin(); - btVector3 normal = tr.getBasis().getColumn(2); - btVector3 axis = tr.getBasis().getColumn(0); - getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect); - } - } - break; - case CONETWIST_CONSTRAINT_TYPE: - { - btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; - btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) - { - //const btScalar length = btScalar(5); - const btScalar length = dbgDrawSize; - static int nSegments = 8*4; - btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments); - btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); - pPrev = tr * pPrev; - for (int i=0; i<nSegments; i++) - { - fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments); - btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length); - pCur = tr * pCur; - getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0)); - - if (i%(nSegments/8) == 0) - getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0)); - - pPrev = pCur; - } - btScalar tws = pCT->getTwistSpan(); - btScalar twa = pCT->getTwistAngle(); - bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); - if(useFrameB) - { - tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); - } - else - { - tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); - } - btVector3 pivot = tr.getOrigin(); - btVector3 normal = tr.getBasis().getColumn(0); - btVector3 axis1 = tr.getBasis().getColumn(1); - getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true); - - } - } - break; - case D6_CONSTRAINT_TYPE: - { - btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; - btTransform tr = p6DOF->getCalculatedTransformA(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = p6DOF->getCalculatedTransformB(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) - { - tr = p6DOF->getCalculatedTransformA(); - const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); - btVector3 up = tr.getBasis().getColumn(2); - btVector3 axis = tr.getBasis().getColumn(0); - btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; - btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; - btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; - btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; - getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0)); - axis = tr.getBasis().getColumn(1); - btScalar ay = p6DOF->getAngle(1); - btScalar az = p6DOF->getAngle(2); - btScalar cy = btCos(ay); - btScalar sy = btSin(ay); - btScalar cz = btCos(az); - btScalar sz = btSin(az); - btVector3 ref; - ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; - ref[1] = -sz*axis[0] + cz*axis[1]; - ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; - tr = p6DOF->getCalculatedTransformB(); - btVector3 normal = -tr.getBasis().getColumn(0); - btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; - btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; - if(minFi > maxFi) - { - getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false); - } - else if(minFi < maxFi) - { - getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true); - } - tr = p6DOF->getCalculatedTransformA(); - btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; - btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; - getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0)); - } - } - break; - case SLIDER_CONSTRAINT_TYPE: - { - btSliderConstraint* pSlider = (btSliderConstraint*)constraint; - btTransform tr = pSlider->getCalculatedTransformA(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pSlider->getCalculatedTransformB(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) - { - btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB(); - btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); - btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); - getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); - btVector3 normal = tr.getBasis().getColumn(0); - btVector3 axis = tr.getBasis().getColumn(1); - btScalar a_min = pSlider->getLowerAngLimit(); - btScalar a_max = pSlider->getUpperAngLimit(); - const btVector3& center = pSlider->getCalculatedTransformB().getOrigin(); - getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true); - } - } - break; - default : - break; - } - return; -} - - - - - -void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) -{ - if (m_ownsConstraintSolver) - { - btAlignedFree( m_constraintSolver); - } - m_ownsConstraintSolver = false; - m_constraintSolver = solver; -} - -btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver() -{ - return m_constraintSolver; -} - - -int btDiscreteDynamicsWorld::getNumConstraints() const -{ - return int(m_constraints.size()); -} -btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) -{ - return m_constraints[index]; -} -const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const -{ - return m_constraints[index]; -} - - - -void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer) -{ - int i; - //serialize all collision objects - for (i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - if (colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - int len = colObj->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); - const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj); - } - } - - for (i=0;i<m_constraints.size();i++) - { - btTypedConstraint* constraint = m_constraints[i]; - int size = constraint->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(size,1); - const char* structType = constraint->serialize(chunk->m_oldPtr,serializer); - serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint); - } -} - - -void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) -{ - - serializer->startSerialization(); - - serializeRigidBodies(serializer); - - serializeCollisionObjects(serializer); - - serializer->finishSerialization(); -} - diff --git a/extern/bullet2/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h deleted file mode 100644 index df47c29044f..00000000000 --- a/extern/bullet2/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ /dev/null @@ -1,198 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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_DISCRETE_DYNAMICS_WORLD_H -#define BT_DISCRETE_DYNAMICS_WORLD_H - -#include "btDynamicsWorld.h" - -class btDispatcher; -class btOverlappingPairCache; -class btConstraintSolver; -class btSimulationIslandManager; -class btTypedConstraint; -class btActionInterface; - -class btIDebugDraw; -#include "LinearMath/btAlignedObjectArray.h" - - -///btDiscreteDynamicsWorld provides discrete rigid body simulation -///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController -class btDiscreteDynamicsWorld : public btDynamicsWorld -{ -protected: - - btConstraintSolver* m_constraintSolver; - - btSimulationIslandManager* m_islandManager; - - btAlignedObjectArray<btTypedConstraint*> m_constraints; - - btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies; - - btVector3 m_gravity; - - //for variable timesteps - btScalar m_localTime; - //for variable timesteps - - bool m_ownsIslandManager; - bool m_ownsConstraintSolver; - bool m_synchronizeAllMotionStates; - - btAlignedObjectArray<btActionInterface*> m_actions; - - int m_profileTimings; - - virtual void predictUnconstraintMotion(btScalar timeStep); - - virtual void integrateTransforms(btScalar timeStep); - - virtual void calculateSimulationIslands(); - - virtual void solveConstraints(btContactSolverInfo& solverInfo); - - void updateActivationState(btScalar timeStep); - - void updateActions(btScalar timeStep); - - void startProfiling(btScalar timeStep); - - virtual void internalSingleStepSimulation( btScalar timeStep); - - - virtual void saveKinematicState(btScalar timeStep); - - void serializeRigidBodies(btSerializer* serializer); - -public: - - - ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those - btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); - - virtual ~btDiscreteDynamicsWorld(); - - ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); - - - virtual void synchronizeMotionStates(); - - ///this can be useful to synchronize a single rigid body -> graphics object - void synchronizeSingleMotionState(btRigidBody* body); - - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); - - virtual void removeConstraint(btTypedConstraint* constraint); - - virtual void addAction(btActionInterface*); - - virtual void removeAction(btActionInterface*); - - btSimulationIslandManager* getSimulationIslandManager() - { - return m_islandManager; - } - - const btSimulationIslandManager* getSimulationIslandManager() const - { - return m_islandManager; - } - - btCollisionWorld* getCollisionWorld() - { - return this; - } - - virtual void setGravity(const btVector3& gravity); - - virtual btVector3 getGravity () const; - - virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - virtual void addRigidBody(btRigidBody* body); - - virtual void addRigidBody(btRigidBody* body, short group, short mask); - - virtual void removeRigidBody(btRigidBody* body); - - ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject - virtual void removeCollisionObject(btCollisionObject* collisionObject); - - - void debugDrawConstraint(btTypedConstraint* constraint); - - virtual void debugDrawWorld(); - - virtual void setConstraintSolver(btConstraintSolver* solver); - - virtual btConstraintSolver* getConstraintSolver(); - - virtual int getNumConstraints() const; - - virtual btTypedConstraint* getConstraint(int index) ; - - virtual const btTypedConstraint* getConstraint(int index) const; - - - virtual btDynamicsWorldType getWorldType() const - { - return BT_DISCRETE_DYNAMICS_WORLD; - } - - ///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep. - virtual void clearForces(); - - ///apply gravity, call this once per timestep - virtual void applyGravity(); - - virtual void setNumTasks(int numTasks) - { - (void) numTasks; - } - - ///obsolete, use updateActions instead - virtual void updateVehicles(btScalar timeStep) - { - updateActions(timeStep); - } - - ///obsolete, use addAction instead - virtual void addVehicle(btActionInterface* vehicle); - ///obsolete, use removeAction instead - virtual void removeVehicle(btActionInterface* vehicle); - ///obsolete, use addAction instead - virtual void addCharacter(btActionInterface* character); - ///obsolete, use removeAction instead - virtual void removeCharacter(btActionInterface* character); - - void setSynchronizeAllMotionStates(bool synchronizeAll) - { - m_synchronizeAllMotionStates = synchronizeAll; - } - bool getSynchronizeAllMotionStates() const - { - return m_synchronizeAllMotionStates; - } - - ///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 diff --git a/extern/bullet2/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/BulletDynamics/Dynamics/btDynamicsWorld.h deleted file mode 100644 index a7b85afbec9..00000000000 --- a/extern/bullet2/BulletDynamics/Dynamics/btDynamicsWorld.h +++ /dev/null @@ -1,148 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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_DYNAMICS_WORLD_H -#define BT_DYNAMICS_WORLD_H - -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - -class btTypedConstraint; -class btActionInterface; -class btConstraintSolver; -class btDynamicsWorld; - - -/// Type for the callback for each tick -typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep); - -enum btDynamicsWorldType -{ - BT_SIMPLE_DYNAMICS_WORLD=1, - BT_DISCRETE_DYNAMICS_WORLD=2, - BT_CONTINUOUS_DYNAMICS_WORLD=3 -}; - -///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. -class btDynamicsWorld : public btCollisionWorld -{ - -protected: - btInternalTickCallback m_internalTickCallback; - btInternalTickCallback m_internalPreTickCallback; - void* m_worldUserInfo; - - btContactSolverInfo m_solverInfo; - -public: - - - btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration) - :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0) - { - } - - virtual ~btDynamicsWorld() - { - } - - ///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds. - ///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'. - ///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'. - ///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant. - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; - - virtual void debugDrawWorld() = 0; - - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) - { - (void)constraint; (void)disableCollisionsBetweenLinkedBodies; - } - - virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;} - - virtual void addAction(btActionInterface* action) = 0; - - virtual void removeAction(btActionInterface* action) = 0; - - //once a rigidbody is added to the dynamics world, it will get this gravity assigned - //existing rigidbodies in the world get gravity assigned too, during this method - virtual void setGravity(const btVector3& gravity) = 0; - virtual btVector3 getGravity () const = 0; - - virtual void synchronizeMotionStates() = 0; - - virtual void addRigidBody(btRigidBody* body) = 0; - - virtual void removeRigidBody(btRigidBody* body) = 0; - - virtual void setConstraintSolver(btConstraintSolver* solver) = 0; - - virtual btConstraintSolver* getConstraintSolver() = 0; - - virtual int getNumConstraints() const { return 0; } - - virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; } - - virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; } - - virtual btDynamicsWorldType getWorldType() const=0; - - virtual void clearForces() = 0; - - /// Set the callback for when an internal tick (simulation substep) happens, optional user info - void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0,bool isPreTick=false) - { - if (isPreTick) - { - m_internalPreTickCallback = cb; - } else - { - m_internalTickCallback = cb; - } - m_worldUserInfo = worldUserInfo; - } - - void setWorldUserInfo(void* worldUserInfo) - { - m_worldUserInfo = worldUserInfo; - } - - void* getWorldUserInfo() const - { - return m_worldUserInfo; - } - - btContactSolverInfo& getSolverInfo() - { - return m_solverInfo; - } - - - ///obsolete, use addAction instead. - virtual void addVehicle(btActionInterface* vehicle) {(void)vehicle;} - ///obsolete, use removeAction instead - virtual void removeVehicle(btActionInterface* vehicle) {(void)vehicle;} - ///obsolete, use addAction instead. - virtual void addCharacter(btActionInterface* character) {(void)character;} - ///obsolete, use removeAction instead - virtual void removeCharacter(btActionInterface* character) {(void)character;} - - -}; - -#endif //BT_DYNAMICS_WORLD_H - - diff --git a/extern/bullet2/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/BulletDynamics/Dynamics/btRigidBody.cpp deleted file mode 100644 index 35de8e47570..00000000000 --- a/extern/bullet2/BulletDynamics/Dynamics/btRigidBody.cpp +++ /dev/null @@ -1,400 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 "btRigidBody.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "LinearMath/btMinMax.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btMotionState.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "LinearMath/btSerializer.h" - -//'temporarily' global variables -btScalar gDeactivationTime = btScalar(2.); -bool gDisableDeactivation = false; -static int uniqueId = 0; - - -btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) -{ - setupRigidBody(constructionInfo); -} - -btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia) -{ - btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia); - setupRigidBody(cinfo); -} - -void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) -{ - - m_internalType=CO_RIGID_BODY; - - m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - m_angularFactor.setValue(1,1,1); - m_linearFactor.setValue(1,1,1); - m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); - 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); - m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold; - m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold; - m_optionalMotionState = constructionInfo.m_motionState; - m_contactSolverType = 0; - m_frictionSolverType = 0; - m_additionalDamping = constructionInfo.m_additionalDamping; - m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor; - m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr; - m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr; - m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor; - - if (m_optionalMotionState) - { - m_optionalMotionState->getWorldTransform(m_worldTransform); - } else - { - m_worldTransform = constructionInfo.m_startWorldTransform; - } - - m_interpolationWorldTransform = m_worldTransform; - m_interpolationLinearVelocity.setValue(0,0,0); - m_interpolationAngularVelocity.setValue(0,0,0); - - //moved to btCollisionObject - m_friction = constructionInfo.m_friction; - 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(); - - - -} - - -void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) -{ - btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); -} - -void btRigidBody::saveKinematicState(btScalar timeStep) -{ - //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities - if (timeStep != btScalar(0.)) - { - //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform - if (getMotionState()) - getMotionState()->getWorldTransform(m_worldTransform); - btVector3 linVel,angVel; - - btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity); - m_interpolationLinearVelocity = m_linearVelocity; - m_interpolationAngularVelocity = m_angularVelocity; - m_interpolationWorldTransform = m_worldTransform; - //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ()); - } -} - -void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const -{ - getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax); -} - - - - -void btRigidBody::setGravity(const btVector3& acceleration) -{ - if (m_inverseMass != btScalar(0.0)) - { - m_gravity = acceleration * (btScalar(1.0) / m_inverseMass); - } - m_gravity_acceleration = 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)); -} - - - - -///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping -void btRigidBody::applyDamping(btScalar timeStep) -{ - //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74 - //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway - -//#define USE_OLD_DAMPING_METHOD 1 -#ifdef USE_OLD_DAMPING_METHOD - m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); - m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); -#else - m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep); - m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep); -#endif - - if (m_additionalDamping) - { - //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc. - //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete - if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) && - (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr)) - { - m_angularVelocity *= m_additionalDampingFactor; - m_linearVelocity *= m_additionalDampingFactor; - } - - - btScalar speed = m_linearVelocity.length(); - if (speed < m_linearDamping) - { - btScalar dampVel = btScalar(0.005); - if (speed > dampVel) - { - btVector3 dir = m_linearVelocity.normalized(); - m_linearVelocity -= dir * dampVel; - } else - { - m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - } - } - - btScalar angSpeed = m_angularVelocity.length(); - if (angSpeed < m_angularDamping) - { - btScalar angDampVel = btScalar(0.005); - if (angSpeed > angDampVel) - { - btVector3 dir = m_angularVelocity.normalized(); - m_angularVelocity -= dir * angDampVel; - } else - { - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - } - } - } -} - - -void btRigidBody::applyGravity() -{ - if (isStaticOrKinematicObject()) - return; - - applyCentralForce(m_gravity); - -} - -void btRigidBody::proceedToTransform(const btTransform& newTrans) -{ - setCenterOfMassTransform( newTrans ); -} - - -void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) -{ - if (mass == btScalar(0.)) - { - m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT; - m_inverseMass = btScalar(0.); - } else - { - m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); - m_inverseMass = btScalar(1.0) / mass; - } - - 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(); -} - - -void btRigidBody::integrateVelocities(btScalar step) -{ - if (isStaticOrKinematicObject()) - return; - - m_linearVelocity += m_totalForce * (m_inverseMass * step); - m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step; - -#define MAX_ANGVEL SIMD_HALF_PI - /// clamp angular velocity. collision calculations will fail on higher angular velocities - btScalar angvel = m_angularVelocity.length(); - if (angvel*step > MAX_ANGVEL) - { - m_angularVelocity *= (MAX_ANGVEL/step) /angvel; - } - -} - -btQuaternion btRigidBody::getOrientation() const -{ - btQuaternion orn; - m_worldTransform.getBasis().getRotation(orn); - return orn; -} - - -void btRigidBody::setCenterOfMassTransform(const btTransform& xform) -{ - - if (isStaticOrKinematicObject()) - { - m_interpolationWorldTransform = m_worldTransform; - } else - { - m_interpolationWorldTransform = xform; - } - m_interpolationLinearVelocity = getLinearVelocity(); - m_interpolationAngularVelocity = getAngularVelocity(); - m_worldTransform = xform; - updateInertiaTensor(); -} - - -bool btRigidBody::checkCollideWithOverride(btCollisionObject* co) -{ - 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; - } - - return true; -} - -void btRigidBody::internalWritebackVelocity(btScalar timeStep) -{ - (void) timeStep; - if (m_inverseMass) - { - setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity); - setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity); - - //correct the position/orientation based on push/turn recovery - btTransform newTransform; - btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform); - setWorldTransform(newTransform); - //m_originalBody->setCompanionId(-1); - } - m_deltaLinearVelocity.setZero(); - m_deltaAngularVelocity .setZero(); - m_pushVelocity.setZero(); - m_turnVelocity.setZero(); -} - - - -void btRigidBody::addConstraintRef(btTypedConstraint* c) -{ - int index = m_constraintRefs.findLinearSearch(c); - if (index == m_constraintRefs.size()) - m_constraintRefs.push_back(c); - - m_checkCollideWith = true; -} - -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); -} - - diff --git a/extern/bullet2/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/BulletDynamics/Dynamics/btRigidBody.h deleted file mode 100644 index 571a30ce6b8..00000000000 --- a/extern/bullet2/BulletDynamics/Dynamics/btRigidBody.h +++ /dev/null @@ -1,670 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 RIGIDBODY_H -#define RIGIDBODY_H - -#include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btTransform.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - -class btCollisionShape; -class btMotionState; -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 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. -///There are 3 types of rigid bodies: -///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics. -///- B) Fixed objects with zero mass. They are not moving (basically collision objects) -///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform. -///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time. -///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects) -class btRigidBody : public btCollisionObject -{ - - btMatrix3x3 m_invInertiaTensorWorld; - btVector3 m_linearVelocity; - btVector3 m_angularVelocity; - btScalar m_inverseMass; - btVector3 m_linearFactor; - - btVector3 m_gravity; - btVector3 m_gravity_acceleration; - btVector3 m_invInertiaLocal; - btVector3 m_totalForce; - btVector3 m_totalTorque; - - btScalar m_linearDamping; - btScalar m_angularDamping; - - bool m_additionalDamping; - btScalar m_additionalDampingFactor; - btScalar m_additionalLinearDampingThresholdSqr; - btScalar m_additionalAngularDampingThresholdSqr; - btScalar m_additionalAngularDampingFactor; - - - btScalar m_linearSleepingThreshold; - btScalar m_angularSleepingThreshold; - - //m_optionalMotionState allows to automatic synchronize the world transform for active objects - btMotionState* m_optionalMotionState; - - //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: - - - ///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body. - ///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument) - ///You can use the motion state to synchronize the world transform between physics and graphics objects. - ///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state, - ///m_startWorldTransform is only used when you don't provide a motion state. - struct btRigidBodyConstructionInfo - { - btScalar m_mass; - - ///When a motionState is provided, the rigid body will initialize its world transform from the motion state - ///In this case, m_startWorldTransform is ignored. - btMotionState* m_motionState; - btTransform m_startWorldTransform; - - btCollisionShape* m_collisionShape; - btVector3 m_localInertia; - btScalar m_linearDamping; - btScalar m_angularDamping; - - ///best simulation results when friction is non-zero - btScalar m_friction; - ///best simulation results using zero restitution. - btScalar m_restitution; - - btScalar m_linearSleepingThreshold; - btScalar m_angularSleepingThreshold; - - //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc. - //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete - bool m_additionalDamping; - btScalar m_additionalDampingFactor; - btScalar m_additionalLinearDampingThresholdSqr; - 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), - m_collisionShape(collisionShape), - m_localInertia(localInertia), - m_linearDamping(btScalar(0.)), - m_angularDamping(btScalar(0.)), - m_friction(btScalar(0.5)), - m_restitution(btScalar(0.)), - m_linearSleepingThreshold(btScalar(0.8)), - m_angularSleepingThreshold(btScalar(1.f)), - m_additionalDamping(false), - m_additionalDampingFactor(btScalar(0.005)), - m_additionalLinearDampingThresholdSqr(btScalar(0.01)), - m_additionalAngularDampingThresholdSqr(btScalar(0.01)), - m_additionalAngularDampingFactor(btScalar(0.01)) - { - m_startWorldTransform.setIdentity(); - } - }; - - ///btRigidBody constructor using construction info - btRigidBody( const btRigidBodyConstructionInfo& constructionInfo); - - ///btRigidBody constructor for backwards compatibility. - ///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo) - btRigidBody( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)); - - - virtual ~btRigidBody() - { - //No constraints should point to this rigidbody - //Remove constraints from the dynamics world before you delete the related rigidbodies. - btAssert(m_constraintRefs.size()==0); - } - -protected: - - ///setupRigidBody is only used internally by the constructor - void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo); - -public: - - void proceedToTransform(const btTransform& newTrans); - - ///to keep collision detection and dynamics separate we don't store a rigidbody pointer - ///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) - return (const btRigidBody*)colObj; - return 0; - } - static btRigidBody* upcast(btCollisionObject* colObj) - { - if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY) - return (btRigidBody*)colObj; - return 0; - } - - /// continuous collision detection needs prediction - void predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ; - - void saveKinematicState(btScalar step); - - void applyGravity(); - - void setGravity(const btVector3& acceleration); - - const btVector3& getGravity() const - { - return m_gravity_acceleration; - } - - void setDamping(btScalar lin_damping, btScalar ang_damping); - - btScalar getLinearDamping() const - { - return m_linearDamping; - } - - btScalar getAngularDamping() const - { - return m_angularDamping; - } - - btScalar getLinearSleepingThreshold() const - { - return m_linearSleepingThreshold; - } - - btScalar getAngularSleepingThreshold() const - { - return m_angularSleepingThreshold; - } - - void applyDamping(btScalar timeStep); - - SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { - return m_collisionShape; - } - - SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() { - return m_collisionShape; - } - - void setMassProps(btScalar mass, const btVector3& inertia); - - const btVector3& getLinearFactor() const - { - return m_linearFactor; - } - void setLinearFactor(const btVector3& linearFactor) - { - m_linearFactor = linearFactor; - m_invMass = m_linearFactor*m_inverseMass; - } - btScalar getInvMass() const { return m_inverseMass; } - const btMatrix3x3& getInvInertiaTensorWorld() const { - return m_invInertiaTensorWorld; - } - - void integrateVelocities(btScalar step); - - void setCenterOfMassTransform(const btTransform& xform); - - void applyCentralForce(const btVector3& force) - { - m_totalForce += force*m_linearFactor; - } - - const btVector3& getTotalForce() - { - return m_totalForce; - }; - - const btVector3& getTotalTorque() - { - return m_totalTorque; - }; - - const btVector3& getInvInertiaDiagLocal() const - { - return m_invInertiaLocal; - }; - - void setInvInertiaDiagLocal(const btVector3& diagInvInertia) - { - m_invInertiaLocal = diagInvInertia; - } - - void setSleepingThresholds(btScalar linear,btScalar angular) - { - m_linearSleepingThreshold = linear; - m_angularSleepingThreshold = angular; - } - - void applyTorque(const btVector3& torque) - { - m_totalTorque += torque*m_angularFactor; - } - - void applyForce(const btVector3& force, const btVector3& rel_pos) - { - applyCentralForce(force); - applyTorque(rel_pos.cross(force*m_linearFactor)); - } - - void applyCentralImpulse(const btVector3& impulse) - { - m_linearVelocity += impulse *m_linearFactor * m_inverseMass; - } - - void applyTorqueImpulse(const btVector3& torque) - { - m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; - } - - void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) - { - if (m_inverseMass != btScalar(0.)) - { - applyCentralImpulse(impulse); - if (m_angularFactor) - { - applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor)); - } - } - } - - void clearForces() - { - m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); - m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); - } - - void updateInertiaTensor(); - - const btVector3& getCenterOfMassPosition() const { - return m_worldTransform.getOrigin(); - } - btQuaternion getOrientation() const; - - const btTransform& getCenterOfMassTransform() const { - return m_worldTransform; - } - const btVector3& getLinearVelocity() const { - return m_linearVelocity; - } - const btVector3& getAngularVelocity() const { - return m_angularVelocity; - } - - - inline void setLinearVelocity(const btVector3& lin_vel) - { - m_linearVelocity = lin_vel; - } - - inline void setAngularVelocity(const btVector3& ang_vel) - { - m_angularVelocity = ang_vel; - } - - btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const - { - //we also calculate lin/ang velocity for kinematic objects - return m_linearVelocity + m_angularVelocity.cross(rel_pos); - - //for kinematic objects, we could also use use: - // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; - } - - void translate(const btVector3& v) - { - m_worldTransform.getOrigin() += v; - } - - - void getAabb(btVector3& aabbMin,btVector3& aabbMax) const; - - - - - - SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const - { - btVector3 r0 = pos - getCenterOfMassPosition(); - - btVector3 c0 = (r0).cross(normal); - - btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0); - - return m_inverseMass + normal.dot(vec); - - } - - SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const - { - btVector3 vec = axis * getInvInertiaTensorWorld(); - return axis.dot(vec); - } - - SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep) - { - if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) - return; - - if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) && - (getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold)) - { - m_deactivationTime += timeStep; - } else - { - m_deactivationTime=btScalar(0.); - setActivationState(0); - } - - } - - SIMD_FORCE_INLINE bool wantsSleeping() - { - - if (getActivationState() == DISABLE_DEACTIVATION) - return false; - - //disable deactivation - if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) - return false; - - if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) - return true; - - if (m_deactivationTime> gDeactivationTime) - { - return true; - } - return false; - } - - - - const btBroadphaseProxy* getBroadphaseProxy() const - { - return m_broadphaseHandle; - } - btBroadphaseProxy* getBroadphaseProxy() - { - return m_broadphaseHandle; - } - void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy) - { - m_broadphaseHandle = broadphaseProxy; - } - - //btMotionState allows to automatic synchronize the world transform for active objects - btMotionState* getMotionState() - { - return m_optionalMotionState; - } - const btMotionState* getMotionState() const - { - return m_optionalMotionState; - } - void setMotionState(btMotionState* motionState) - { - m_optionalMotionState = motionState; - if (m_optionalMotionState) - motionState->getWorldTransform(m_worldTransform); - } - - //for experimental overriding of friction/contact solver func - int m_contactSolverType; - int m_frictionSolverType; - - void setAngularFactor(const btVector3& angFac) - { - m_angularFactor = angFac; - } - - void setAngularFactor(btScalar angFac) - { - m_angularFactor.setValue(angFac,angFac,angFac); - } - const btVector3& getAngularFactor() const - { - return m_angularFactor; - } - - //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase? - bool isInWorld() const - { - return (getBroadphaseProxy() != 0); - } - - virtual bool checkCollideWithOverride(btCollisionObject* co); - - void addConstraintRef(btTypedConstraint* c); - void removeConstraintRef(btTypedConstraint* c); - - btTypedConstraint* getConstraintRef(int index) - { - return m_constraintRefs[index]; - } - - int getNumConstraintRefs() - { - return m_constraintRefs.size(); - } - - void setFlags(int flags) - { - m_rigidbodyFlags = flags; - } - - int getFlags() const - { - return m_rigidbodyFlags; - } - - - //////////////////////////////////////////////// - ///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; - } - - btVector3& internalGetPushVelocity() - { - return m_pushVelocity; - } - - btVector3& internalGetTurnVelocity() - { - return m_turnVelocity; - } - - SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const - { - velocity = getLinearVelocity()+m_deltaLinearVelocity + (getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos); - } - - SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const - { - angVel = getAngularVelocity()+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_inverseMass) - { - m_deltaLinearVelocity += linearComponent*impulseMagnitude; - m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } - } - - SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) - { - if (m_inverseMass) - { - m_pushVelocity += linearComponent*impulseMagnitude; - m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } - } - - void internalWritebackVelocity() - { - if (m_inverseMass) - { - setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity); - setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity); - m_deltaLinearVelocity.setZero(); - m_deltaAngularVelocity .setZero(); - //m_originalBody->setCompanionId(-1); - } - } - - - void internalWritebackVelocity(btScalar timeStep); - - - /////////////////////////////////////////////// - - 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 - diff --git a/extern/bullet2/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp deleted file mode 100644 index ae449f292f6..00000000000 --- a/extern/bullet2/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ /dev/null @@ -1,253 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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 "btSimpleDynamicsWorld.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - - -/* - Make sure this dummy function never changes so that it - can be used by probes that are checking whether the - library is actually installed. -*/ -extern "C" -{ - void btBulletDynamicsProbe (); - void btBulletDynamicsProbe () {} -} - - - - -btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) -:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), -m_constraintSolver(constraintSolver), -m_ownsConstraintSolver(false), -m_gravity(0,0,-10) -{ - -} - - -btSimpleDynamicsWorld::~btSimpleDynamicsWorld() -{ - if (m_ownsConstraintSolver) - btAlignedFree( m_constraintSolver); -} - -int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) -{ - (void)fixedTimeStep; - (void)maxSubSteps; - - - ///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(); - - ///solve contact constraints - int numManifolds = m_dispatcher1->getNumManifolds(); - if (numManifolds) - { - btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer(); - - 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->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc); - } - - ///integrate transforms - integrateTransforms(timeStep); - - updateAabbs(); - - synchronizeMotionStates(); - - clearForces(); - - return 1; - -} - -void btSimpleDynamicsWorld::clearForces() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - { - body->clearForces(); - } - } -} - - -void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) -{ - m_gravity = gravity; - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - { - body->setGravity(gravity); - } - } -} - -btVector3 btSimpleDynamicsWorld::getGravity () const -{ - return m_gravity; -} - -void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) -{ - btCollisionWorld::removeCollisionObject(body); -} - -void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) -{ - btRigidBody* body = btRigidBody::upcast(collisionObject); - if (body) - removeRigidBody(body); - else - btCollisionWorld::removeCollisionObject(collisionObject); -} - - -void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) -{ - body->setGravity(m_gravity); - - if (body->getCollisionShape()) - { - addCollisionObject(body); - } -} - -void btSimpleDynamicsWorld::updateAabbs() -{ - btTransform predictedTrans; - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - { - if (body->isActive() && (!body->isStaticObject())) - { - btVector3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - btBroadphaseInterface* bp = getBroadphase(); - bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); - } - } - } -} - -void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) -{ - btTransform predictedTrans; - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - { - if (body->isActive() && (!body->isStaticObject())) - { - body->predictIntegratedTransform(timeStep, predictedTrans); - body->proceedToTransform( predictedTrans); - } - } - } -} - - - -void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - { - if (!body->isStaticObject()) - { - if (body->isActive()) - { - body->applyGravity(); - body->integrateVelocities( timeStep); - body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); - } - } - } - } -} - - -void btSimpleDynamicsWorld::synchronizeMotionStates() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body && body->getMotionState()) - { - if (body->getActivationState() != ISLAND_SLEEPING) - { - body->getMotionState()->setWorldTransform(body->getWorldTransform()); - } - } - } - -} - - -void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) -{ - if (m_ownsConstraintSolver) - { - btAlignedFree(m_constraintSolver); - } - m_ownsConstraintSolver = false; - m_constraintSolver = solver; -} - -btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver() -{ - return m_constraintSolver; -} diff --git a/extern/bullet2/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h deleted file mode 100644 index ad1f541340f..00000000000 --- a/extern/bullet2/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ /dev/null @@ -1,81 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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_SIMPLE_DYNAMICS_WORLD_H -#define BT_SIMPLE_DYNAMICS_WORLD_H - -#include "btDynamicsWorld.h" - -class btDispatcher; -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). -class btSimpleDynamicsWorld : public btDynamicsWorld -{ -protected: - - btConstraintSolver* m_constraintSolver; - - bool m_ownsConstraintSolver; - - void predictUnconstraintMotion(btScalar timeStep); - - void integrateTransforms(btScalar timeStep); - - btVector3 m_gravity; - -public: - - - - ///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver - btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); - - virtual ~btSimpleDynamicsWorld(); - - ///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); - - virtual void setGravity(const btVector3& gravity); - - virtual btVector3 getGravity () const; - - virtual void addRigidBody(btRigidBody* body); - - virtual void removeRigidBody(btRigidBody* body); - - ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject - virtual void removeCollisionObject(btCollisionObject* collisionObject); - - virtual void updateAabbs(); - - virtual void synchronizeMotionStates(); - - virtual void setConstraintSolver(btConstraintSolver* solver); - - virtual btConstraintSolver* getConstraintSolver(); - - virtual btDynamicsWorldType getWorldType() const - { - return BT_SIMPLE_DYNAMICS_WORLD; - } - - virtual void clearForces(); - -}; - -#endif //BT_SIMPLE_DYNAMICS_WORLD_H diff --git a/extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.cpp deleted file mode 100644 index b42c024f178..00000000000 --- a/extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ /dev/null @@ -1,758 +0,0 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies. - * Erwin Coumans makes no representations about the suitability - * of this software for any purpose. - * It is provided "as is" without express or implied warranty. -*/ - -#include "LinearMath/btVector3.h" -#include "btRaycastVehicle.h" - -#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h" -#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" -#include "LinearMath/btQuaternion.h" -#include "BulletDynamics/Dynamics/btDynamicsWorld.h" -#include "btVehicleRaycaster.h" -#include "btWheelInfo.h" -#include "LinearMath/btMinMax.h" -#include "LinearMath/btIDebugDraw.h" -#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" - - - -btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ) -:m_vehicleRaycaster(raycaster), -m_pitchControl(btScalar(0.)) -{ - m_chassisBody = chassis; - m_indexRightAxis = 0; - m_indexUpAxis = 2; - m_indexForwardAxis = 1; - defaultInit(tuning); -} - - -void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning) -{ - (void)tuning; - m_currentVehicleSpeedKmHour = btScalar(0.); - m_steeringValue = btScalar(0.); - -} - - - -btRaycastVehicle::~btRaycastVehicle() -{ -} - - -// -// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed -// -btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel) -{ - - btWheelInfoConstructionInfo ci; - - ci.m_chassisConnectionCS = connectionPointCS; - ci.m_wheelDirectionCS = wheelDirectionCS0; - ci.m_wheelAxleCS = wheelAxleCS; - ci.m_suspensionRestLength = suspensionRestLength; - ci.m_wheelRadius = wheelRadius; - ci.m_suspensionStiffness = tuning.m_suspensionStiffness; - ci.m_wheelsDampingCompression = tuning.m_suspensionCompression; - ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping; - 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)); - - btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1]; - - updateWheelTransformsWS( wheel , false ); - updateWheelTransform(getNumWheels()-1,false); - return wheel; -} - - - - -const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const -{ - btAssert(wheelIndex < getNumWheels()); - const btWheelInfo& wheel = m_wheelInfo[wheelIndex]; - return wheel.m_worldTransform; - -} - -void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform) -{ - - btWheelInfo& wheel = m_wheelInfo[ wheelIndex ]; - updateWheelTransformsWS(wheel,interpolatedTransform); - btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; - const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS; - btVector3 fwd = up.cross(right); - fwd = fwd.normalize(); -// up = right.cross(fwd); -// up.normalize(); - - //rotate around steering over de wheelAxleWS - btScalar steering = wheel.m_steering; - - btQuaternion steeringOrn(up,steering);//wheel.m_steering); - btMatrix3x3 steeringMat(steeringOrn); - - btQuaternion rotatingOrn(right,-wheel.m_rotation); - btMatrix3x3 rotatingMat(rotatingOrn); - - btMatrix3x3 basis2( - right[0],fwd[0],up[0], - right[1],fwd[1],up[1], - right[2],fwd[2],up[2] - ); - - wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2); - wheel.m_worldTransform.setOrigin( - wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength - ); -} - -void btRaycastVehicle::resetSuspension() -{ - - int i; - for (i=0;i<m_wheelInfo.size(); i++) - { - btWheelInfo& wheel = m_wheelInfo[i]; - wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); - wheel.m_suspensionRelativeVelocity = btScalar(0.0); - - wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; - //wheel_info.setContactFriction(btScalar(0.0)); - wheel.m_clippedInvContactDotSuspension = btScalar(1.0); - } -} - -void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform) -{ - wheel.m_raycastInfo.m_isInContact = false; - - btTransform chassisTrans = getChassisWorldTransform(); - if (interpolatedTransform && (getRigidBody()->getMotionState())) - { - getRigidBody()->getMotionState()->getWorldTransform(chassisTrans); - } - - wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS ); - wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ; - wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS; -} - -btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) -{ - updateWheelTransformsWS( wheel,false); - - - btScalar depth = -1; - - btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius; - - btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); - const btVector3& source = wheel.m_raycastInfo.m_hardPointWS; - wheel.m_raycastInfo.m_contactPointWS = source + rayvector; - const btVector3& target = wheel.m_raycastInfo.m_contactPointWS; - - btScalar param = btScalar(0.); - - btVehicleRaycaster::btVehicleRaycasterResult rayResults; - - btAssert(m_vehicleRaycaster); - - void* object = m_vehicleRaycaster->castRay(source,target,rayResults); - - wheel.m_raycastInfo.m_groundObject = 0; - - if (object) - { - param = rayResults.m_distFraction; - depth = raylen * rayResults.m_distFraction; - wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; - wheel.m_raycastInfo.m_isInContact = true; - - wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!; - //wheel.m_raycastInfo.m_groundObject = object; - - - btScalar hitDistance = param*raylen; - wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius; - //clamp on max suspension travel - - btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01); - btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01); - if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) - { - wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength; - } - if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength) - { - wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength; - } - - wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld; - - btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS ); - - btVector3 chassis_velocity_at_contactPoint; - btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition(); - - chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos); - - btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); - - if ( denominator >= btScalar(-0.1)) - { - wheel.m_suspensionRelativeVelocity = btScalar(0.0); - wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); - } - else - { - btScalar inv = btScalar(-1.) / denominator; - wheel.m_suspensionRelativeVelocity = projVel * inv; - wheel.m_clippedInvContactDotSuspension = inv; - } - - } else - { - //put wheel info as in rest position - wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); - wheel.m_suspensionRelativeVelocity = btScalar(0.0); - wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; - wheel.m_clippedInvContactDotSuspension = btScalar(1.0); - } - - return depth; -} - - -const btTransform& btRaycastVehicle::getChassisWorldTransform() const -{ - /*if (getRigidBody()->getMotionState()) - { - btTransform chassisWorldTrans; - getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans); - return chassisWorldTrans; - } - */ - - - return getRigidBody()->getCenterOfMassTransform(); -} - - -void btRaycastVehicle::updateVehicle( btScalar step ) -{ - { - for (int i=0;i<getNumWheels();i++) - { - updateWheelTransform(i,false); - } - } - - - m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length(); - - const btTransform& chassisTrans = getChassisWorldTransform(); - - btVector3 forwardW ( - chassisTrans.getBasis()[0][m_indexForwardAxis], - chassisTrans.getBasis()[1][m_indexForwardAxis], - chassisTrans.getBasis()[2][m_indexForwardAxis]); - - if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.)) - { - m_currentVehicleSpeedKmHour *= btScalar(-1.); - } - - // - // simulate suspension - // - - int i=0; - for (i=0;i<m_wheelInfo.size();i++) - { - btScalar depth; - depth = rayCast( m_wheelInfo[i]); - } - - updateSuspension(step); - - - for (i=0;i<m_wheelInfo.size();i++) - { - //apply suspension force - btWheelInfo& wheel = m_wheelInfo[i]; - - btScalar suspensionForce = wheel.m_wheelsSuspensionForce; - - if (suspensionForce > wheel.m_maxSuspensionForce) - { - suspensionForce = wheel.m_maxSuspensionForce; - } - btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; - btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition(); - - getRigidBody()->applyImpulse(impulse, relpos); - - } - - - - updateFriction( step); - - - for (i=0;i<m_wheelInfo.size();i++) - { - btWheelInfo& wheel = m_wheelInfo[i]; - btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition(); - btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos ); - - if (wheel.m_raycastInfo.m_isInContact) - { - const btTransform& chassisWorldTransform = getChassisWorldTransform(); - - btVector3 fwd ( - chassisWorldTransform.getBasis()[0][m_indexForwardAxis], - chassisWorldTransform.getBasis()[1][m_indexForwardAxis], - chassisWorldTransform.getBasis()[2][m_indexForwardAxis]); - - btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); - fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; - - btScalar proj2 = fwd.dot(vel); - - wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius); - wheel.m_rotation += wheel.m_deltaRotation; - - } else - { - wheel.m_rotation += wheel.m_deltaRotation; - } - - wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact - - } - - - -} - - -void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel) -{ - btAssert(wheel>=0 && wheel < getNumWheels()); - - btWheelInfo& wheelInfo = getWheelInfo(wheel); - wheelInfo.m_steering = steering; -} - - - -btScalar btRaycastVehicle::getSteeringValue(int wheel) const -{ - return getWheelInfo(wheel).m_steering; -} - - -void btRaycastVehicle::applyEngineForce(btScalar force, int wheel) -{ - btAssert(wheel>=0 && wheel < getNumWheels()); - btWheelInfo& wheelInfo = getWheelInfo(wheel); - wheelInfo.m_engineForce = force; -} - - -const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const -{ - btAssert((index >= 0) && (index < getNumWheels())); - - return m_wheelInfo[index]; -} - -btWheelInfo& btRaycastVehicle::getWheelInfo(int index) -{ - btAssert((index >= 0) && (index < getNumWheels())); - - return m_wheelInfo[index]; -} - -void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex) -{ - btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels())); - getWheelInfo(wheelIndex).m_brake = brake; -} - - -void btRaycastVehicle::updateSuspension(btScalar deltaTime) -{ - (void)deltaTime; - - btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass(); - - for (int w_it=0; w_it<getNumWheels(); w_it++) - { - btWheelInfo &wheel_info = m_wheelInfo[w_it]; - - if ( wheel_info.m_raycastInfo.m_isInContact ) - { - btScalar force; - // Spring - { - btScalar susp_length = wheel_info.getSuspensionRestLength(); - btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength; - - btScalar length_diff = (susp_length - current_length); - - force = wheel_info.m_suspensionStiffness - * length_diff * wheel_info.m_clippedInvContactDotSuspension; - } - - // Damper - { - btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity; - { - btScalar susp_damping; - if ( projected_rel_vel < btScalar(0.0) ) - { - susp_damping = wheel_info.m_wheelsDampingCompression; - } - else - { - susp_damping = wheel_info.m_wheelsDampingRelaxation; - } - force -= susp_damping * projected_rel_vel; - } - } - - // RESULT - wheel_info.m_wheelsSuspensionForce = force * chassisMass; - if (wheel_info.m_wheelsSuspensionForce < btScalar(0.)) - { - wheel_info.m_wheelsSuspensionForce = btScalar(0.); - } - } - else - { - wheel_info.m_wheelsSuspensionForce = btScalar(0.0); - } - } - -} - - -struct btWheelContactPoint -{ - btRigidBody* m_body0; - btRigidBody* m_body1; - btVector3 m_frictionPositionWorld; - btVector3 m_frictionDirectionWorld; - btScalar m_jacDiagABInv; - btScalar m_maxImpulse; - - - btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse) - :m_body0(body0), - m_body1(body1), - m_frictionPositionWorld(frictionPosWorld), - m_frictionDirectionWorld(frictionDirectionWorld), - m_maxImpulse(maxImpulse) - { - btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); - btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); - btScalar relaxation = 1.f; - m_jacDiagABInv = relaxation/(denom0+denom1); - } - - - -}; - -btScalar calcRollingFriction(btWheelContactPoint& contactPoint); -btScalar calcRollingFriction(btWheelContactPoint& contactPoint) -{ - - btScalar j1=0.f; - - const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld; - - btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); - btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition(); - - btScalar maxImpulse = contactPoint.m_maxImpulse; - - btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel); - - // calculate j that moves us to zero relative velocity - j1 = -vrel * contactPoint.m_jacDiagABInv; - btSetMin(j1, maxImpulse); - btSetMax(j1, -maxImpulse); - - return j1; -} - - - - -btScalar sideFrictionStiffness2 = btScalar(1.0); -void btRaycastVehicle::updateFriction(btScalar timeStep) -{ - - //calculate the impulse, so that the wheels don't move sidewards - int numWheel = getNumWheels(); - if (!numWheel) - return; - - m_forwardWS.resize(numWheel); - m_axle.resize(numWheel); - m_forwardImpulse.resize(numWheel); - m_sideImpulse.resize(numWheel); - - int numWheelsOnGround = 0; - - - //collapse all those loops into one! - for (int i=0;i<getNumWheels();i++) - { - btWheelInfo& wheelInfo = m_wheelInfo[i]; - class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; - if (groundObject) - numWheelsOnGround++; - m_sideImpulse[i] = btScalar(0.); - m_forwardImpulse[i] = btScalar(0.); - - } - - { - - for (int i=0;i<getNumWheels();i++) - { - - btWheelInfo& wheelInfo = m_wheelInfo[i]; - - class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; - - if (groundObject) - { - - const btTransform& wheelTrans = getWheelTransformWS( i ); - - btMatrix3x3 wheelBasis0 = wheelTrans.getBasis(); - m_axle[i] = btVector3( - wheelBasis0[0][m_indexRightAxis], - wheelBasis0[1][m_indexRightAxis], - wheelBasis0[2][m_indexRightAxis]); - - const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS; - btScalar proj = m_axle[i].dot(surfNormalWS); - m_axle[i] -= surfNormalWS * proj; - m_axle[i] = m_axle[i].normalize(); - - m_forwardWS[i] = surfNormalWS.cross(m_axle[i]); - m_forwardWS[i].normalize(); - - - resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS, - *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, - btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep); - - m_sideImpulse[i] *= sideFrictionStiffness2; - - } - - - } - } - - btScalar sideFactor = btScalar(1.); - btScalar fwdFactor = 0.5; - - bool sliding = false; - { - for (int wheel =0;wheel <getNumWheels();wheel++) - { - btWheelInfo& wheelInfo = m_wheelInfo[wheel]; - class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; - - btScalar rollingFriction = 0.f; - - if (groundObject) - { - if (wheelInfo.m_engineForce != 0.f) - { - rollingFriction = wheelInfo.m_engineForce* timeStep; - } else - { - btScalar defaultRollingFrictionImpulse = 0.f; - btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse; - btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse); - rollingFriction = calcRollingFriction(contactPt); - } - } - - //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) - - - - - m_forwardImpulse[wheel] = btScalar(0.); - m_wheelInfo[wheel].m_skidInfo= btScalar(1.); - - if (groundObject) - { - m_wheelInfo[wheel].m_skidInfo= btScalar(1.); - - btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip; - btScalar maximpSide = maximp; - - btScalar maximpSquared = maximp * maximpSide; - - - m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep; - - btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor; - btScalar y = (m_sideImpulse[wheel] ) * sideFactor; - - btScalar impulseSquared = (x*x + y*y); - - if (impulseSquared > maximpSquared) - { - sliding = true; - - btScalar factor = maximp / btSqrt(impulseSquared); - - m_wheelInfo[wheel].m_skidInfo *= factor; - } - } - - } - } - - - - - if (sliding) - { - for (int wheel = 0;wheel < getNumWheels(); wheel++) - { - if (m_sideImpulse[wheel] != btScalar(0.)) - { - if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.)) - { - m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; - m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; - } - } - } - } - - // apply the impulses - { - for (int wheel = 0;wheel<getNumWheels() ; wheel++) - { - btWheelInfo& wheelInfo = m_wheelInfo[wheel]; - - btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - - m_chassisBody->getCenterOfMassPosition(); - - if (m_forwardImpulse[wheel] != btScalar(0.)) - { - m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos); - } - if (m_sideImpulse[wheel] != btScalar(0.)) - { - class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject; - - btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - - groundObject->getCenterOfMassPosition(); - - - btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; - - rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence; - m_chassisBody->applyImpulse(sideImp,rel_pos); - - //apply friction impulse on the ground - groundObject->applyImpulse(-sideImp,rel_pos2); - } - } - } - - -} - - - -void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer) -{ - - for (int v=0;v<this->getNumWheels();v++) - { - btVector3 wheelColor(0,1,1); - if (getWheelInfo(v).m_raycastInfo.m_isInContact) - { - wheelColor.setValue(0,0,1); - } else - { - wheelColor.setValue(1,0,1); - } - - btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin(); - - btVector3 axle = btVector3( - getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()], - getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()], - getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]); - - //debug wheels (cylinders) - debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); - debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); - - } -} - - -void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) -{ -// RayResultCallback& resultCallback; - - btCollisionWorld::ClosestRayResultCallback rayCallback(from,to); - - m_dynamicsWorld->rayTest(from, to, rayCallback); - - if (rayCallback.hasHit()) - { - - 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 0; -} - diff --git a/extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.h deleted file mode 100644 index 5ce80f4d275..00000000000 --- a/extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.h +++ /dev/null @@ -1,236 +0,0 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies. - * Erwin Coumans makes no representations about the suitability - * of this software for any purpose. - * It is provided "as is" without express or implied warranty. -*/ -#ifndef RAYCASTVEHICLE_H -#define RAYCASTVEHICLE_H - -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "btVehicleRaycaster.h" -class btDynamicsWorld; -#include "LinearMath/btAlignedObjectArray.h" -#include "btWheelInfo.h" -#include "BulletDynamics/Dynamics/btActionInterface.h" - -class btVehicleTuning; - -///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle. -class btRaycastVehicle : public btActionInterface -{ - - btAlignedObjectArray<btVector3> m_forwardWS; - btAlignedObjectArray<btVector3> m_axle; - btAlignedObjectArray<btScalar> m_forwardImpulse; - btAlignedObjectArray<btScalar> m_sideImpulse; - - ///backwards compatibility - int m_userConstraintType; - int m_userConstraintId; - -public: - class btVehicleTuning - { - public: - - btVehicleTuning() - :m_suspensionStiffness(btScalar(5.88)), - m_suspensionCompression(btScalar(0.83)), - m_suspensionDamping(btScalar(0.88)), - m_maxSuspensionTravelCm(btScalar(500.)), - m_frictionSlip(btScalar(10.5)), - m_maxSuspensionForce(btScalar(6000.)) - { - } - btScalar m_suspensionStiffness; - btScalar m_suspensionCompression; - btScalar m_suspensionDamping; - btScalar m_maxSuspensionTravelCm; - btScalar m_frictionSlip; - btScalar m_maxSuspensionForce; - - }; -private: - - btScalar m_tau; - btScalar m_damping; - btVehicleRaycaster* m_vehicleRaycaster; - btScalar m_pitchControl; - btScalar m_steeringValue; - btScalar m_currentVehicleSpeedKmHour; - - btRigidBody* m_chassisBody; - - int m_indexRightAxis; - int m_indexUpAxis; - int m_indexForwardAxis; - - void defaultInit(const btVehicleTuning& tuning); - -public: - - //constructor to create a car from an existing rigidbody - btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ); - - virtual ~btRaycastVehicle() ; - - - ///btActionInterface interface - virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step) - { - (void) collisionWorld; - updateVehicle(step); - } - - - ///btActionInterface interface - void debugDraw(btIDebugDraw* debugDrawer); - - const btTransform& getChassisWorldTransform() const; - - btScalar rayCast(btWheelInfo& wheel); - - virtual void updateVehicle(btScalar step); - - - void resetSuspension(); - - btScalar getSteeringValue(int wheel) const; - - void setSteeringValue(btScalar steering,int wheel); - - - void applyEngineForce(btScalar force, int wheel); - - const btTransform& getWheelTransformWS( int wheelIndex ) const; - - void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true ); - - 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); - - inline int getNumWheels() const { - return int (m_wheelInfo.size()); - } - - btAlignedObjectArray<btWheelInfo> m_wheelInfo; - - - const btWheelInfo& getWheelInfo(int index) const; - - btWheelInfo& getWheelInfo(int index); - - void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true); - - - void setBrake(btScalar brake,int wheelIndex); - - void setPitchControl(btScalar pitch) - { - m_pitchControl = pitch; - } - - void updateSuspension(btScalar deltaTime); - - virtual void updateFriction(btScalar timeStep); - - - - inline btRigidBody* getRigidBody() - { - return m_chassisBody; - } - - const btRigidBody* getRigidBody() const - { - return m_chassisBody; - } - - inline int getRightAxis() const - { - return m_indexRightAxis; - } - inline int getUpAxis() const - { - return m_indexUpAxis; - } - - inline int getForwardAxis() const - { - return m_indexForwardAxis; - } - - - ///Worldspace forward vector - btVector3 getForwardVector() const - { - const btTransform& chassisTrans = getChassisWorldTransform(); - - btVector3 forwardW ( - chassisTrans.getBasis()[0][m_indexForwardAxis], - chassisTrans.getBasis()[1][m_indexForwardAxis], - chassisTrans.getBasis()[2][m_indexForwardAxis]); - - return forwardW; - } - - ///Velocity of vehicle (positive if velocity vector has same direction as foward vector) - btScalar getCurrentSpeedKmHour() const - { - return m_currentVehicleSpeedKmHour; - } - - virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex) - { - m_indexRightAxis = rightIndex; - m_indexUpAxis = upIndex; - m_indexForwardAxis = forwardIndex; - } - - - ///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; - } - -}; - -class btDefaultVehicleRaycaster : public btVehicleRaycaster -{ - btDynamicsWorld* m_dynamicsWorld; -public: - btDefaultVehicleRaycaster(btDynamicsWorld* world) - :m_dynamicsWorld(world) - { - } - - virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result); - -}; - - -#endif //RAYCASTVEHICLE_H - diff --git a/extern/bullet2/BulletDynamics/Vehicle/btVehicleRaycaster.h b/extern/bullet2/BulletDynamics/Vehicle/btVehicleRaycaster.h deleted file mode 100644 index 5112ce6d420..00000000000 --- a/extern/bullet2/BulletDynamics/Vehicle/btVehicleRaycaster.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies. - * Erwin Coumans makes no representations about the suitability - * of this software for any purpose. - * It is provided "as is" without express or implied warranty. -*/ -#ifndef VEHICLE_RAYCASTER_H -#define VEHICLE_RAYCASTER_H - -#include "LinearMath/btVector3.h" - -/// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting -struct btVehicleRaycaster -{ -virtual ~btVehicleRaycaster() -{ -} - struct btVehicleRaycasterResult - { - btVehicleRaycasterResult() :m_distFraction(btScalar(-1.)){}; - btVector3 m_hitPointInWorld; - btVector3 m_hitNormalInWorld; - btScalar m_distFraction; - }; - - virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) = 0; - -}; - -#endif //VEHICLE_RAYCASTER_H - diff --git a/extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.cpp b/extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.cpp deleted file mode 100644 index ef93c16fffc..00000000000 --- a/extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies. - * Erwin Coumans makes no representations about the suitability - * of this software for any purpose. - * It is provided "as is" without express or implied warranty. -*/ -#include "btWheelInfo.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity - - -btScalar btWheelInfo::getSuspensionRestLength() const -{ - - return m_suspensionRestLength1; - -} - -void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo) -{ - (void)raycastInfo; - - - if (m_raycastInfo.m_isInContact) - - { - btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); - btVector3 chassis_velocity_at_contactPoint; - btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); - chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); - btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); - if ( project >= btScalar(-0.1)) - { - m_suspensionRelativeVelocity = btScalar(0.0); - m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); - } - else - { - btScalar inv = btScalar(-1.) / project; - m_suspensionRelativeVelocity = projVel * inv; - m_clippedInvContactDotSuspension = inv; - } - - } - - else // Not in contact : position wheel in a nice (rest length) position - { - m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); - m_suspensionRelativeVelocity = btScalar(0.0); - m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; - m_clippedInvContactDotSuspension = btScalar(1.0); - } -} diff --git a/extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.h b/extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.h deleted file mode 100644 index b74f8c80acb..00000000000 --- a/extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.h +++ /dev/null @@ -1,119 +0,0 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies. - * Erwin Coumans makes no representations about the suitability - * of this software for any purpose. - * It is provided "as is" without express or implied warranty. -*/ -#ifndef WHEEL_INFO_H -#define WHEEL_INFO_H - -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" - -class btRigidBody; - -struct btWheelInfoConstructionInfo -{ - btVector3 m_chassisConnectionCS; - btVector3 m_wheelDirectionCS; - btVector3 m_wheelAxleCS; - btScalar m_suspensionRestLength; - btScalar m_maxSuspensionTravelCm; - btScalar m_wheelRadius; - - btScalar m_suspensionStiffness; - btScalar m_wheelsDampingCompression; - btScalar m_wheelsDampingRelaxation; - btScalar m_frictionSlip; - btScalar m_maxSuspensionForce; - bool m_bIsFrontWheel; - -}; - -/// btWheelInfo contains information per wheel about friction and suspension. -struct btWheelInfo -{ - struct RaycastInfo - { - //set by raycaster - btVector3 m_contactNormalWS;//contactnormal - btVector3 m_contactPointWS;//raycast hitpoint - btScalar m_suspensionLength; - btVector3 m_hardPointWS;//raycast starting point - btVector3 m_wheelDirectionWS; //direction in worldspace - btVector3 m_wheelAxleWS; // axle in worldspace - bool m_isInContact; - void* m_groundObject; //could be general void* ptr - }; - - RaycastInfo m_raycastInfo; - - btTransform m_worldTransform; - - btVector3 m_chassisConnectionPointCS; //const - btVector3 m_wheelDirectionCS;//const - btVector3 m_wheelAxleCS; // const or modified by steering - btScalar m_suspensionRestLength1;//const - btScalar m_maxSuspensionTravelCm; - btScalar getSuspensionRestLength() const; - btScalar m_wheelsRadius;//const - btScalar m_suspensionStiffness;//const - btScalar m_wheelsDampingCompression;//const - btScalar m_wheelsDampingRelaxation;//const - btScalar m_frictionSlip; - btScalar m_steering; - btScalar m_rotation; - btScalar m_deltaRotation; - btScalar m_rollInfluence; - btScalar m_maxSuspensionForce; - - btScalar m_engineForce; - - btScalar m_brake; - - bool m_bIsFrontWheel; - - void* m_clientInfo;//can be used to store pointer to sync transforms... - - btWheelInfo(btWheelInfoConstructionInfo& ci) - - { - - m_suspensionRestLength1 = ci.m_suspensionRestLength; - m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm; - - m_wheelsRadius = ci.m_wheelRadius; - m_suspensionStiffness = ci.m_suspensionStiffness; - m_wheelsDampingCompression = ci.m_wheelsDampingCompression; - m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation; - m_chassisConnectionPointCS = ci.m_chassisConnectionCS; - m_wheelDirectionCS = ci.m_wheelDirectionCS; - m_wheelAxleCS = ci.m_wheelAxleCS; - m_frictionSlip = ci.m_frictionSlip; - m_steering = btScalar(0.); - m_engineForce = btScalar(0.); - m_rotation = btScalar(0.); - m_deltaRotation = btScalar(0.); - m_brake = btScalar(0.); - m_rollInfluence = btScalar(0.1); - m_bIsFrontWheel = ci.m_bIsFrontWheel; - m_maxSuspensionForce = ci.m_maxSuspensionForce; - - } - - void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo); - - btScalar m_clippedInvContactDotSuspension; - btScalar m_suspensionRelativeVelocity; - //calculated by suspension - btScalar m_wheelsSuspensionForce; - btScalar m_skidInfo; - -}; - -#endif //WHEEL_INFO_H - |