Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGuillermo S. Romero <gsr.b3d@infernal-iceberg.com>2010-06-17 07:29:16 +0400
committerGuillermo S. Romero <gsr.b3d@infernal-iceberg.com>2010-06-17 07:29:16 +0400
commit3398d901a17fbb093f593830b3ee40f32ada993c (patch)
treee5c9946e6c3acaa197a32804049c2ba71d4a5b13 /extern/bullet2/BulletDynamics
parented59822857de7e7b41b33b79c306f5e9b8755c62 (diff)
Revert changes back to original state, before 29506.
Diffstat (limited to 'extern/bullet2/BulletDynamics')
-rw-r--r--extern/bullet2/BulletDynamics/CMakeLists.txt112
-rw-r--r--extern/bullet2/BulletDynamics/Character/btCharacterControllerInterface.h45
-rw-r--r--extern/bullet2/BulletDynamics/Character/btKinematicCharacterController.cpp553
-rw-r--r--extern/bullet2/BulletDynamics/Character/btKinematicCharacterController.h145
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp1117
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h332
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btConstraintSolver.h52
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.cpp134
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.h68
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btContactSolverInfo.h87
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp1012
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h588
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp146
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h54
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp66
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.h58
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp992
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.h332
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btJacobianEntry.h156
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp229
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h161
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp1174
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h132
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp857
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.h321
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp255
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h107
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btSolverBody.h191
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btSolverConstraint.h96
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp136
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.h302
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp63
-rw-r--r--extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.h60
-rw-r--r--extern/bullet2/BulletDynamics/Dynamics/Bullet-C-API.cpp405
-rw-r--r--extern/bullet2/BulletDynamics/Dynamics/btActionInterface.h50
-rw-r--r--extern/bullet2/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp196
-rw-r--r--extern/bullet2/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h46
-rw-r--r--extern/bullet2/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp1161
-rw-r--r--extern/bullet2/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h198
-rw-r--r--extern/bullet2/BulletDynamics/Dynamics/btDynamicsWorld.h148
-rw-r--r--extern/bullet2/BulletDynamics/Dynamics/btRigidBody.cpp400
-rw-r--r--extern/bullet2/BulletDynamics/Dynamics/btRigidBody.h670
-rw-r--r--extern/bullet2/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp253
-rw-r--r--extern/bullet2/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h81
-rw-r--r--extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.cpp758
-rw-r--r--extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.h236
-rw-r--r--extern/bullet2/BulletDynamics/Vehicle/btVehicleRaycaster.h35
-rw-r--r--extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.cpp56
-rw-r--r--extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.h119
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(&currentConstraintRow[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 = &currentConstraintRow->m_rhs;
- currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
- info2.cfm = &currentConstraintRow->m_cfm;
- info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
- info2.m_upperLimit = &currentConstraintRow->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
-