archive/blender-archive
Archived
1
1
Fork 0
Code Pull Requests Packages Activity

Upgrade Bullet to version 2.83.

Browse Source 
I tried to carefully preserve all patches since the last upgrade.

Improves T47195, cloth collision detection bug.

Differential Revision: https://developer.blender.org/D1739
This commit is contained in:
Brecht Van Lommel
2016-01-17 21:35:32 +01:00
parent 3c72e302e1
commit b64d5809e7
133 changed files with 13946 additions and 3829 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
extern/bullet2/CMakeLists.txt vendored
View File

@@ -43,53 +43,53 @@ set(SRC
src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp
src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btGhostObject.cpp
src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp
src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h
src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btUnionFind.cpp
src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp
src/BulletCollision/CollisionShapes/btBoxShape.cpp
src/BulletCollision/CollisionShapes/btBox2dShape.cpp
src/BulletCollision/CollisionShapes/btBoxShape.cpp
src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
src/BulletCollision/CollisionShapes/btCapsuleShape.cpp
src/BulletCollision/CollisionShapes/btCollisionShape.cpp
src/BulletCollision/CollisionShapes/btCompoundShape.cpp
src/BulletCollision/CollisionShapes/btConcaveShape.cpp
src/BulletCollision/CollisionShapes/btConeShape.cpp
src/BulletCollision/CollisionShapes/btConvex2dShape.cpp
src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp
src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp
src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
src/BulletCollision/CollisionShapes/btConvexShape.cpp
src/BulletCollision/CollisionShapes/btConvex2dShape.cpp
src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp
src/BulletCollision/CollisionShapes/btCylinderShape.cpp
src/BulletCollision/CollisionShapes/btEmptyShape.cpp
src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp
src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp
src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp
src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp
@@ -106,11 +106,11 @@ set(SRC
src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp
src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp
src/BulletCollision/Gimpact/btContactProcessing.cpp
src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp
src/BulletCollision/Gimpact/btGImpactBvh.cpp
src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp
src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
src/BulletCollision/Gimpact/btGImpactShape.cpp
src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp
src/BulletCollision/Gimpact/btTriangleShapeEx.cpp
src/BulletCollision/Gimpact/gim_box_set.cpp
src/BulletCollision/Gimpact/gim_contact.cpp
@@ -124,32 +124,32 @@ set(SRC
src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
src/BulletDynamics/Character/btKinematicCharacterController.cpp
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp
src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp
src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
src/BulletDynamics/Dynamics/Bullet-C-API.cpp
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
src/BulletDynamics/Dynamics/btRigidBody.cpp
src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
src/BulletDynamics/Dynamics/Bullet-C-API.cpp
src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
src/BulletDynamics/Vehicle/btWheelInfo.cpp
src/BulletDynamics/Character/btKinematicCharacterController.cpp
src/BulletDynamics/Featherstone/btMultiBody.cpp
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -158,8 +158,12 @@ set(SRC
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp
src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
src/BulletDynamics/Vehicle/btWheelInfo.cpp
src/BulletSoftBody/btDefaultSoftBodySolver.cpp
src/BulletSoftBody/btSoftBody.cpp
src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp
src/BulletSoftBody/btSoftBodyHelpers.cpp
@@ -167,18 +171,16 @@ set(SRC
src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp
src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp
src/BulletSoftBody/btDefaultSoftBodySolver.cpp
src/LinearMath/btAlignedAllocator.cpp
src/LinearMath/btConvexHull.cpp
src/LinearMath/btConvexHullComputer.cpp
src/LinearMath/btGeometryUtil.cpp
src/LinearMath/btPolarDecomposition.cpp
src/LinearMath/btQuickprof.cpp
src/LinearMath/btSerializer.cpp
src/LinearMath/btVector3.cpp
src/LinearMath/btPolarDecomposition.cpp
src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
@@ -191,35 +193,37 @@ set(SRC
src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h
src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h
src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h
src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h
src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h
src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h
src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h
src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h
src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h
src/BulletCollision/CollisionDispatch/btCollisionObject.h
src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h
src/BulletCollision/CollisionDispatch/btCollisionWorld.h
src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h
src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h
src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h
src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h
src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
src/BulletCollision/CollisionDispatch/btGhostObject.h
src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h
src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h
src/BulletCollision/CollisionDispatch/btManifoldResult.h
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
src/BulletCollision/CollisionDispatch/btUnionFind.h
src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h
src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h
src/BulletCollision/CollisionShapes/btBoxShape.h
src/BulletCollision/CollisionShapes/btBox2dShape.h
src/BulletCollision/CollisionShapes/btBoxShape.h
src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h
src/BulletCollision/CollisionShapes/btCapsuleShape.h
src/BulletCollision/CollisionShapes/btCollisionMargin.h
@@ -227,20 +231,20 @@ set(SRC
src/BulletCollision/CollisionShapes/btCompoundShape.h
src/BulletCollision/CollisionShapes/btConcaveShape.h
src/BulletCollision/CollisionShapes/btConeShape.h
src/BulletCollision/CollisionShapes/btConvex2dShape.h
src/BulletCollision/CollisionShapes/btConvexHullShape.h
src/BulletCollision/CollisionShapes/btConvexInternalShape.h
src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h
src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
src/BulletCollision/CollisionShapes/btConvexShape.h
src/BulletCollision/CollisionShapes/btConvex2dShape.h
src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h
src/BulletCollision/CollisionShapes/btCylinderShape.h
src/BulletCollision/CollisionShapes/btEmptyShape.h
src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
src/BulletCollision/CollisionShapes/btMaterial.h
src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h
src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h
src/BulletCollision/CollisionShapes/btMultiSphereShape.h
src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h
src/BulletCollision/CollisionShapes/btOptimizedBvh.h
src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h
@@ -286,33 +290,43 @@ set(SRC
src/BulletCollision/Gimpact/gim_memory.h
src/BulletCollision/Gimpact/gim_radixsort.h
src/BulletCollision/Gimpact/gim_tri_collision.h
src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h
src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h
src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h
src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h
src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
src/BulletCollision/NarrowPhaseCollision/btPointCollector.h
src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
src/BulletDynamics/Character/btCharacterControllerInterface.h
src/BulletDynamics/Character/btKinematicCharacterController.h
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
src/BulletDynamics/ConstraintSolver/btContactConstraint.h
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
src/BulletDynamics/ConstraintSolver/btGearConstraint.h
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
@@ -321,22 +335,16 @@ set(SRC
src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
src/BulletDynamics/ConstraintSolver/btGearConstraint.h
src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
src/BulletDynamics/Dynamics/btActionInterface.h
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
src/BulletDynamics/Dynamics/btDynamicsWorld.h
src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
src/BulletDynamics/Dynamics/btRigidBody.h
src/BulletDynamics/Vehicle/btRaycastVehicle.h
src/BulletDynamics/Vehicle/btVehicleRaycaster.h
src/BulletDynamics/Vehicle/btWheelInfo.h
src/BulletDynamics/Character/btCharacterControllerInterface.h
src/BulletDynamics/Character/btKinematicCharacterController.h
src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
src/BulletDynamics/Featherstone/btMultiBody.h
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
src/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -345,30 +353,36 @@ set(SRC
src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h
src/BulletDynamics/MLCPSolvers/btDantzigLCP.h
src/BulletDynamics/MLCPSolvers/btDantzigSolver.h
src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
src/BulletDynamics/MLCPSolvers/btPATHSolver.h
src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
src/BulletDynamics/Vehicle/btRaycastVehicle.h
src/BulletDynamics/Vehicle/btVehicleRaycaster.h
src/BulletDynamics/Vehicle/btWheelInfo.h
src/BulletSoftBody/btDefaultSoftBodySolver.h
src/BulletSoftBody/btSoftBody.h
src/BulletSoftBody/btSoftBodyInternals.h
src/BulletSoftBody/btSoftBodyData.h
src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h
src/BulletSoftBody/btSoftBodyData.h
src/BulletSoftBody/btSoftBodyHelpers.h
src/BulletSoftBody/btSoftBodyInternals.h
src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h
src/BulletSoftBody/btSoftBodySolverVertexBuffer.h
src/BulletSoftBody/btSoftBodySolvers.h
src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h
src/BulletSoftBody/btSoftRigidDynamicsWorld.h
src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h
src/BulletSoftBody/btSparseSDF.h
src/BulletSoftBody/btSoftBodySolvers.h
src/BulletSoftBody/btDefaultSoftBodySolver.h
src/BulletSoftBody/btSoftBodySolverVertexBuffer.h
src/LinearMath/btAabbUtil2.h
src/LinearMath/btAlignedAllocator.h
src/LinearMath/btAlignedObjectArray.h
src/LinearMath/btConvexHull.h
src/LinearMath/btConvexHullComputer.h
src/LinearMath/btCpuFeatureUtility.h
src/LinearMath/btDefaultMotionState.h
src/LinearMath/btGeometryUtil.h
src/LinearMath/btGrahamScan2dConvexHull.h
@@ -376,8 +390,10 @@ set(SRC
src/LinearMath/btIDebugDraw.h
src/LinearMath/btList.h
src/LinearMath/btMatrix3x3.h
src/LinearMath/btMatrixX.h
src/LinearMath/btMinMax.h
src/LinearMath/btMotionState.h
src/LinearMath/btPolarDecomposition.h
src/LinearMath/btPoolAllocator.h
src/LinearMath/btQuadWord.h
src/LinearMath/btQuaternion.h
@@ -385,14 +401,12 @@ set(SRC
src/LinearMath/btRandom.h
src/LinearMath/btScalar.h
src/LinearMath/btSerializer.h
src/LinearMath/btSpatialAlgebra.h
src/LinearMath/btStackAlloc.h
src/LinearMath/btTransform.h
src/LinearMath/btTransformUtil.h
src/LinearMath/btVector3.h
src/LinearMath/btPolarDecomposition.h
src/LinearMath/btMatrixX.h
src/btBulletCollisionCommon.h
src/btBulletDynamicsCommon.h
src/Bullet-C-Api.h

150
extern/bullet2/patches/blender.patch vendored Normal file
View File

@@ -0,0 +1,150 @@
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
index be9eca6..ec40c96 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
@@ -15,7 +15,7 @@ subject to the following restrictions:
/**
- * @mainpage Bullet Documentation
+ * @page Bullet Documentation
*
* @section intro_sec Introduction
* Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ).
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp
index 36dd043..57eb817 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp
@@ -579,14 +579,10 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS
btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData;
btCompoundShape* compoundShape = createCompoundShape();
- btCompoundShapeChildData* childShapeDataArray = &compoundData->m_childShapePtr[0];
-
btAlignedObjectArray<btCollisionShape*> childShapes;
for (int i=0;i<compoundData->m_numChildShapes;i++)
{
- btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i];
-
btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape;
btCollisionShape* childShape = convertCollisionShape(cd);
diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
index 57fc119..31faf1d 100644
--- a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
+++ b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
@@ -29,14 +29,11 @@ subject to the following restrictions:
static btVector3
getNormalizedVector(const btVector3& v)
{
- btScalar l = v.length();
- btVector3 n = v;
- if (l < SIMD_EPSILON) {
- n.setValue(0,0,0);
- } else {
- n /= l;
- }
+ btVector3 n(0, 0, 0);
+ if (v.length() > SIMD_EPSILON) {
+ n = v.normalized();
+ }
return n;
}
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
index 27ccefe..8e4456e 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
@@ -37,8 +37,13 @@ struct btSimdScalar
{
}
-
+/* workaround for clang 3.4 ( == apple clang 5.1 ) issue, friction would fail with forced inlining */
+#if (defined(__clang__) && defined(__apple_build_version__) && (__clang_major__ == 5) && (__clang_minor__ == 1)) \
+|| (defined(__clang__) && !defined(__apple_build_version__) && (__clang_major__ == 3) && (__clang_minor__ == 4))
+ inline __attribute__ ((noinline)) btSimdScalar(float fl)
+#else
SIMD_FORCE_INLINE btSimdScalar(float fl)
+#endif
:m_vec128 (_mm_set1_ps(fl))
{
}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp
index 5d62da7..fcd312e 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -28,7 +28,6 @@
#include "btMultiBodyJointFeedback.h"
#include "LinearMath/btTransformUtil.h"
#include "LinearMath/btSerializer.h"
-#include "Bullet3Common/b3Logging.h"
// #define INCLUDE_GYRO_TERM
///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
@@ -1732,7 +1731,6 @@ void btMultiBody::goToSleep()
void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
{
- int num_links = getNumLinks();
extern bool gDisableDeactivation;
if (!m_canSleep || gDisableDeactivation)
{
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index 8a034b3..4f66b20 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -809,7 +809,6 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS
}
#endif
-#include "Bullet3Common/b3Logging.h"
void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
{
#if 1
diff --git a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h
index bcf0c79..8992ddb 100644
--- a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h
+++ b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h
@@ -185,7 +185,6 @@ struct btSparseSdf
{
++nprobes;
++ncells;
- int sz = sizeof(Cell);
if (ncells>m_clampCells)
{
static int numResets=0;
diff --git a/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp b/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp
index d58ac95..3fd77df 100644
--- a/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp
+++ b/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp
@@ -2665,6 +2665,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in
}
vertices.resize(0);
+ original_vertex_index.resize(0);
edges.resize(0);
faces.resize(0);
@@ -2675,6 +2676,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in
{
btConvexHullInternal::Vertex* v = oldVertices[copied];
vertices.push_back(hull.getCoordinates(v));
+ original_vertex_index.push_back(v->point.index);
btConvexHullInternal::Edge* firstEdge = v->edges;
if (firstEdge)
{
diff --git a/extern/bullet2/src/LinearMath/btConvexHullComputer.h b/extern/bullet2/src/LinearMath/btConvexHullComputer.h
index 7240ac4..6871ce8 100644
--- a/extern/bullet2/src/LinearMath/btConvexHullComputer.h
+++ b/extern/bullet2/src/LinearMath/btConvexHullComputer.h
@@ -67,6 +67,7 @@ class btConvexHullComputer
// Vertices of the output hull
btAlignedObjectArray<btVector3> vertices;
+ btAlignedObjectArray<int> original_vertex_index;
// Edges of the output hull
btAlignedObjectArray<Edge> edges;

127
extern/bullet2/patches/convex_hull.patch vendored
View File

@@ -1,127 +0,0 @@
Index: extern/bullet2/src/Bullet-C-Api.h
===================================================================
--- extern/bullet2/src/Bullet-C-Api.h (revision 51556)
+++ extern/bullet2/src/Bullet-C-Api.h (working copy)
@@ -167,6 +167,16 @@ extern "C" {
// needed for source/blender/blenkernel/intern/collision.c
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]);
+
+ /* Convex Hull */
+ PL_DECLARE_HANDLE(plConvexHull);
+ plConvexHull plConvexHullCompute(float (*coords)[3], int count);
+ int plConvexHullNumVertices(plConvexHull hull);
+ int plConvexHullNumFaces(plConvexHull hull);
+ void plConvexHullGetVertex(plConvexHull hull, int n, float coords[3], int *original_index);
+ int plConvexHullGetFaceSize(plConvexHull hull, int n);
+ void plConvexHullGetFaceVertices(plConvexHull hull, int n, int *vertices);
+
#ifdef __cplusplus
}
#endif
Index: extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
===================================================================
--- extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp (revision 51556)
+++ extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp (working copy)
@@ -23,7 +23,7 @@ subject to the following restrictions:
#include "Bullet-C-Api.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btAlignedAllocator.h"
-
+#include "LinearMath/btConvexHullComputer.h"
#include "LinearMath/btVector3.h"
@@ -403,3 +403,60 @@ double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float
return -1.0f;
}
+// Convex hull
+plConvexHull plConvexHullCompute(float (*coords)[3], int count)
+{
+ btConvexHullComputer *computer = new btConvexHullComputer;
+ computer->compute(reinterpret_cast< float* >(coords),
+ sizeof(*coords), count, 0, 0);
+ return reinterpret_cast<plConvexHull>(computer);
+}
+
+int plConvexHullNumVertices(plConvexHull hull)
+{
+ btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
+ return computer->vertices.size();
+}
+
+int plConvexHullNumFaces(plConvexHull hull)
+{
+ btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
+ return computer->faces.size();
+}
+
+void plConvexHullGetVertex(plConvexHull hull, int n, float coords[3],
+ int *original_index)
+{
+ btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
+ const btVector3 &v(computer->vertices[n]);
+ coords[0] = v[0];
+ coords[1] = v[1];
+ coords[2] = v[2];
+ (*original_index) = computer->original_vertex_index[n];
+}
+
+int plConvexHullGetFaceSize(plConvexHull hull, int n)
+{
+ btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
+ const btConvexHullComputer::Edge *e_orig, *e;
+ int count;
+
+ for (e_orig = &computer->edges[computer->faces[n]], e = e_orig, count = 0;
+ count == 0 || e != e_orig;
+ e = e->getNextEdgeOfFace(), count++);
+ return count;
+}
+
+void plConvexHullGetFaceVertices(plConvexHull hull, int n, int *vertices)
+{
+ btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
+ const btConvexHullComputer::Edge *e_orig, *e;
+ int count;
+
+ for (e_orig = &computer->edges[computer->faces[n]], e = e_orig, count = 0;
+ count == 0 || e != e_orig;
+ e = e->getNextEdgeOfFace(), count++)
+ {
+ vertices[count] = e->getTargetVertex();
+ }
+}
Index: extern/bullet2/src/LinearMath/btConvexHullComputer.cpp
===================================================================
--- extern/bullet2/src/LinearMath/btConvexHullComputer.cpp (revision 51556)
+++ extern/bullet2/src/LinearMath/btConvexHullComputer.cpp (working copy)
@@ -2661,6 +2661,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in
}
vertices.resize(0);
+ original_vertex_index.resize(0);
edges.resize(0);
faces.resize(0);
@@ -2671,6 +2672,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in
{
btConvexHullInternal::Vertex* v = oldVertices[copied];
vertices.push_back(hull.getCoordinates(v));
+ original_vertex_index.push_back(v->point.index);
btConvexHullInternal::Edge* firstEdge = v->edges;
if (firstEdge)
{
Index: extern/bullet2/src/LinearMath/btConvexHullComputer.h
===================================================================
--- extern/bullet2/src/LinearMath/btConvexHullComputer.h (revision 51556)
+++ extern/bullet2/src/LinearMath/btConvexHullComputer.h (working copy)
@@ -67,6 +67,7 @@ class btConvexHullComputer
// Vertices of the output hull
btAlignedObjectArray<btVector3> vertices;
+ btAlignedObjectArray<int> original_vertex_index;
// Edges of the output hull
btAlignedObjectArray<Edge> edges;

4
extern/bullet2/readme.txt vendored
View File

@@ -4,8 +4,8 @@ Questions? mail blender at erwincoumans.com, or check the bf-blender mailing lis
Thanks,
Erwin
Apply patches/convex_hull.patch to add access to the convex hull
operation, used in the BMesh convex hull operator.
Apply patches/blender.patch to fix a few build errors and warnings and dd original
vertex access for BMesh convex hull operator.
Documentation is available at:
http://code.google.com/p/bullet/source/browse/trunk/Bullet_User_Manual.pdf

8
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp vendored
View File

@@ -38,8 +38,9 @@ static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a,
const btDbvtVolume& b)
{
#if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)
ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]);
btDbvtVolume& res=*(btDbvtVolume*)locals;
ATTRIBUTE_ALIGNED16( char locals[sizeof(btDbvtAabbMm)]);
btDbvtVolume* ptr = (btDbvtVolume*) locals;
btDbvtVolume& res=*ptr;
#else
btDbvtVolume res;
#endif
@@ -250,7 +251,8 @@ static btDbvtVolume bounds( const tNodeArray& leaves)
{
#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]);
btDbvtVolume& volume=*(btDbvtVolume*)locals;
btDbvtVolume* ptr = (btDbvtVolume*) locals;
btDbvtVolume& volume=*ptr;
volume=leaves[0]->volume;
#else
btDbvtVolume volume=leaves[0]->volume;

23
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h vendored
View File

@@ -1193,19 +1193,34 @@ inline void btDbvt::collideOCL( const btDbvtNode* root,
/* Insert 0 */
j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
stack.push_back(0);
//void * memmove ( void * destination, const void * source, size_t num );
#if DBVT_USE_MEMMOVE
memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
{
int num_items_to_move = stack.size()-1-j;
if(num_items_to_move > 0)
memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move);
}
#else
for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
for(int k=stack.size()-1;k>j;--k) {
stack[k]=stack[k-1];
}
#endif
stack[j]=allocate(ifree,stock,nes[q]);
/* Insert 1 */
j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
stack.push_back(0);
#if DBVT_USE_MEMMOVE
memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
{
int num_items_to_move = stack.size()-1-j;
if(num_items_to_move > 0)
memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move);
}
#else
for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
for(int k=stack.size()-1;k>j;--k) {
stack[k]=stack[k-1];
}
#endif
stack[j]=allocate(ifree,stock,nes[1-q]);
}

1
extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp vendored
View File

@@ -34,7 +34,6 @@ int gFindPairs =0;
btHashedOverlappingPairCache::btHashedOverlappingPairCache():
m_overlapFilterCallback(0),
m_blockedForChanges(false),
m_ghostPairCallback(0)
{
int initialAllocatedSize= 2;

1
extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h vendored
View File

@@ -94,7 +94,6 @@ class btHashedOverlappingPairCache : public btOverlappingPairCache
{
btBroadphasePairArray m_overlappingPairArray;
btOverlapFilterCallback* m_overlapFilterCallback;
bool m_blockedForChanges;
protected:

2
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp vendored
View File

@@ -189,7 +189,7 @@ bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const
if ((!body0->isActive()) && (!body1->isActive()))
needsCollision = false;
else if (!body0->checkCollideWith(body1))
else if ((!body0->checkCollideWith(body1)) || (!body1->checkCollideWith(body0)))
needsCollision = false;
return needsCollision ;

7
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp vendored
View File

@@ -4,8 +4,8 @@ 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,
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.
@@ -31,10 +31,11 @@ btCollisionObject::btCollisionObject()
m_activationState1(1),
m_deactivationTime(btScalar(0.)),
m_friction(btScalar(0.5)),
m_rollingFriction(0.0f),
m_restitution(btScalar(0.)),
m_rollingFriction(0.0f),
m_internalType(CO_COLLISION_OBJECT),
m_userObjectPointer(0),
m_userIndex(-1),
m_hitFraction(btScalar(1.)),
m_ccdSweptSphereRadius(btScalar(0.)),
m_ccdMotionThreshold(btScalar(0.)),

44
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h vendored
View File

@@ -92,11 +92,10 @@ protected:
int m_internalType;
///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
union
{
void* m_userObjectPointer;
int m_userIndex;
};
void* m_userObjectPointer;
int m_userIndex;
///time of impact calculation
btScalar m_hitFraction;
@@ -110,13 +109,11 @@ protected:
/// If some object should have elaborate collision filtering by sub-classes
int m_checkCollideWith;
btAlignedObjectArray<const btCollisionObject*> m_objectsWithoutCollisionCheck;
///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation.
int m_updateRevision;
virtual bool checkCollideWithOverride(const btCollisionObject* /* co */) const
{
return true;
}
public:
@@ -225,7 +222,34 @@ public:
return m_collisionShape;
}
void setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck)
{
if (ignoreCollisionCheck)
{
//We don't check for duplicates. Is it ok to leave that up to the user of this API?
//int index = m_objectsWithoutCollisionCheck.findLinearSearch(co);
//if (index == m_objectsWithoutCollisionCheck.size())
//{
m_objectsWithoutCollisionCheck.push_back(co);
//}
}
else
{
m_objectsWithoutCollisionCheck.remove(co);
}
m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0;
}
virtual bool checkCollideWithOverride(const btCollisionObject* co) const
{
int index = m_objectsWithoutCollisionCheck.findLinearSearch(co);
if (index < m_objectsWithoutCollisionCheck.size())
{
return false;
}
return true;
}

172
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp vendored
View File

@@ -34,7 +34,7 @@ subject to the following restrictions:
#include "LinearMath/btSerializer.h"
#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
#include "BulletCollision/Gimpact/btGImpactShape.h"
//#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
@@ -292,12 +292,13 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
btGjkConvexCast gjkConvexCaster(castShape,convexShape,&simplexSolver);
//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
bool condition = true;
btConvexCast* convexCasterPtr = 0;
if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest)
convexCasterPtr = &subSimplexConvexCaster;
else
//use kF_UseSubSimplexConvexCastRaytest by default
if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest)
convexCasterPtr = &gjkConvexCaster;
else
convexCasterPtr = &subSimplexConvexCaster;
btConvexCast& convexCaster = *convexCasterPtr;
@@ -308,6 +309,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
//todo: figure out what this is about. When is rayFromTest.getBasis() not identity?
#ifdef USE_SUBSIMPLEX_CONVEX_CAST
//rotate normal into worldspace
castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
@@ -387,14 +389,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
}
else if(collisionShape->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
{
btGImpactMeshShape* concaveShape = (btGImpactMeshShape*)collisionShape;
BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
concaveShape->processAllTrianglesRay(&rcb,rayFromLocal,rayToLocal);
}else
else
{
//generic (slower) case
btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
@@ -1251,7 +1246,10 @@ public:
void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
{
// Draw a small simplex at the center of the object
getDebugDrawer()->drawTransform(worldTransform,1);
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames)
{
getDebugDrawer()->drawTransform(worldTransform,1);
}
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
@@ -1429,81 +1427,91 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
void btCollisionWorld::debugDrawWorld()
{
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
if (getDebugDrawer())
{
int numManifolds = getDispatcher()->getNumManifolds();
btVector3 color(1,1,0);
for (int i=0;i<numManifolds;i++)
{
btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
btIDebugDraw::DefaultColors defaultColors = getDebugDrawer()->getDefaultColors();
int numContacts = contactManifold->getNumContacts();
for (int j=0;j<numContacts;j++)
if ( getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
{
if (getDispatcher())
{
btManifoldPoint& cp = contactManifold->getContactPoint(j);
getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
int numManifolds = getDispatcher()->getNumManifolds();
for (int i=0;i<numManifolds;i++)
{
btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
int numContacts = contactManifold->getNumContacts();
for (int j=0;j<numContacts;j++)
{
btManifoldPoint& cp = contactManifold->getContactPoint(j);
getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),defaultColors.m_contactPoint);
}
}
}
}
}
if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)))
{
int i;
for ( i=0;i<m_collisionObjects.size();i++)
if ((getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)))
{
btCollisionObject* colObj = m_collisionObjects[i];
if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
int i;
for ( i=0;i<m_collisionObjects.size();i++)
{
if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe))
btCollisionObject* colObj = m_collisionObjects[i];
if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
{
btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.));
switch(colObj->getActivationState())
if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe))
{
case ACTIVE_TAG:
color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break;
case ISLAND_SLEEPING:
color = btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break;
case WANTS_DEACTIVATION:
color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break;
case DISABLE_DEACTIVATION:
color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break;
case DISABLE_SIMULATION:
color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break;
default:
btVector3 color(btScalar(0.4),btScalar(0.4),btScalar(0.4));
switch(colObj->getActivationState())
{
color = btVector3(btScalar(1),btScalar(0.),btScalar(0.));
}
};
case ACTIVE_TAG:
color = defaultColors.m_activeObject; break;
case ISLAND_SLEEPING:
color = defaultColors.m_deactivatedObject;break;
case WANTS_DEACTIVATION:
color = defaultColors.m_wantsDeactivationObject;break;
case DISABLE_DEACTIVATION:
color = defaultColors.m_disabledDeactivationObject;break;
case DISABLE_SIMULATION:
color = defaultColors.m_disabledSimulationObject;break;
default:
{
color = btVector3(btScalar(.3),btScalar(0.3),btScalar(0.3));
}
};
debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
}
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
{
btVector3 minAabb,maxAabb;
btVector3 colorvec(1,0,0);
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
minAabb -= contactThreshold;
maxAabb += contactThreshold;
btVector3 minAabb2,maxAabb2;
if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
{
colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
minAabb2 -= contactThreshold;
maxAabb2 += contactThreshold;
minAabb.setMin(minAabb2);
maxAabb.setMax(maxAabb2);
debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
}
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
{
btVector3 minAabb,maxAabb;
btVector3 colorvec = defaultColors.m_aabb;
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
minAabb -= contactThreshold;
maxAabb += contactThreshold;
m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
btVector3 minAabb2,maxAabb2;
if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
{
colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
minAabb2 -= contactThreshold;
maxAabb2 += contactThreshold;
minAabb.setMin(minAabb2);
maxAabb.setMax(maxAabb2);
}
m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
}
}
}
}
}
}
@@ -1512,15 +1520,6 @@ void btCollisionWorld::debugDrawWorld()
void btCollisionWorld::serializeCollisionObjects(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_COLLISION_OBJECT)
{
colObj->serializeSingleObject(serializer);
}
}
///keep track of shapes already serialized
btHashMap<btHashPtr,btCollisionShape*> serializedShapes;
@@ -1537,6 +1536,15 @@ void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
}
}
//serialize all collision objects
for (i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if ((colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK))
{
colObj->serializeSingleObject(serializer);
}
}
}

2
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h vendored
View File

@@ -27,7 +27,7 @@ subject to the following restrictions:
* @section install_sec Installation
*
* @subsection step1 Step 1: Download
* You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list
* You can download the Bullet Physics Library from the github repository: https://github.com/bulletphysics/bullet3/releases
*
* @subsection step2 Step 2: Building
* Bullet has multiple build systems, including premake, cmake and autotools. Premake and cmake support all platforms.

1143
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

190
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h vendored Normal file
View File

@@ -0,0 +1,190 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2014 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_COLLISION_WORLD_IMPORTER_H
#define BT_COLLISION_WORLD_IMPORTER_H
#include "LinearMath/btTransform.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btHashMap.h"
class btCollisionShape;
class btCollisionObject;
struct btBulletSerializedArrays;
struct ConstraintInput;
class btCollisionWorld;
struct btCollisionShapeData;
class btTriangleIndexVertexArray;
class btStridingMeshInterface;
struct btStridingMeshInterfaceData;
class btGImpactMeshShape;
class btOptimizedBvh;
struct btTriangleInfoMap;
class btBvhTriangleMeshShape;
class btPoint2PointConstraint;
class btHingeConstraint;
class btConeTwistConstraint;
class btGeneric6DofConstraint;
class btGeneric6DofSpringConstraint;
class btSliderConstraint;
class btGearConstraint;
struct btContactSolverInfo;
class btCollisionWorldImporter
{
protected:
btCollisionWorld* m_collisionWorld;
int m_verboseMode;
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
btAlignedObjectArray<btCollisionObject*> m_allocatedRigidBodies;
btAlignedObjectArray<btOptimizedBvh*> m_allocatedBvhs;
btAlignedObjectArray<btTriangleInfoMap*> m_allocatedTriangleInfoMaps;
btAlignedObjectArray<btTriangleIndexVertexArray*> m_allocatedTriangleIndexArrays;
btAlignedObjectArray<btStridingMeshInterfaceData*> m_allocatedbtStridingMeshInterfaceDatas;
btAlignedObjectArray<btCollisionObject*> m_allocatedCollisionObjects;
btAlignedObjectArray<char*> m_allocatedNames;
btAlignedObjectArray<int*> m_indexArrays;
btAlignedObjectArray<short int*> m_shortIndexArrays;
btAlignedObjectArray<unsigned char*> m_charIndexArrays;
btAlignedObjectArray<btVector3FloatData*> m_floatVertexArrays;
btAlignedObjectArray<btVector3DoubleData*> m_doubleVertexArrays;
btHashMap<btHashPtr,btOptimizedBvh*> m_bvhMap;
btHashMap<btHashPtr,btTriangleInfoMap*> m_timMap;
btHashMap<btHashString,btCollisionShape*> m_nameShapeMap;
btHashMap<btHashString,btCollisionObject*> m_nameColObjMap;
btHashMap<btHashPtr,const char*> m_objectNameMap;
btHashMap<btHashPtr,btCollisionShape*> m_shapeMap;
btHashMap<btHashPtr,btCollisionObject*> m_bodyMap;
//methods
char* duplicateName(const char* name);
btCollisionShape* convertCollisionShape( btCollisionShapeData* shapeData );
public:
btCollisionWorldImporter(btCollisionWorld* world);
virtual ~btCollisionWorldImporter();
bool convertAllObjects( btBulletSerializedArrays* arrays);
///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load.
///make sure you don't use the dynamics world containing objects after you call this method
virtual void deleteAllData();
void setVerboseMode(int verboseMode)
{
m_verboseMode = verboseMode;
}
int getVerboseMode() const
{
return m_verboseMode;
}
// query for data
int getNumCollisionShapes() const;
btCollisionShape* getCollisionShapeByIndex(int index);
int getNumRigidBodies() const;
btCollisionObject* getRigidBodyByIndex(int index) const;
int getNumConstraints() const;
int getNumBvhs() const;
btOptimizedBvh* getBvhByIndex(int index) const;
int getNumTriangleInfoMaps() const;
btTriangleInfoMap* getTriangleInfoMapByIndex(int index) const;
// queris involving named objects
btCollisionShape* getCollisionShapeByName(const char* name);
btCollisionObject* getCollisionObjectByName(const char* name);
const char* getNameForPointer(const void* ptr) const;
///those virtuals are called by load and can be overridden by the user
//bodies
virtual btCollisionObject* createCollisionObject( const btTransform& startTransform, btCollisionShape* shape,const char* bodyName);
///shapes
virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal,btScalar planeConstant);
virtual btCollisionShape* createBoxShape(const btVector3& halfExtents);
virtual btCollisionShape* createSphereShape(btScalar radius);
virtual btCollisionShape* createCapsuleShapeX(btScalar radius, btScalar height);
virtual btCollisionShape* createCapsuleShapeY(btScalar radius, btScalar height);
virtual btCollisionShape* createCapsuleShapeZ(btScalar radius, btScalar height);
virtual btCollisionShape* createCylinderShapeX(btScalar radius,btScalar height);
virtual btCollisionShape* createCylinderShapeY(btScalar radius,btScalar height);
virtual btCollisionShape* createCylinderShapeZ(btScalar radius,btScalar height);
virtual btCollisionShape* createConeShapeX(btScalar radius,btScalar height);
virtual btCollisionShape* createConeShapeY(btScalar radius,btScalar height);
virtual btCollisionShape* createConeShapeZ(btScalar radius,btScalar height);
virtual class btTriangleIndexVertexArray* createTriangleMeshContainer();
virtual btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh);
virtual btCollisionShape* createConvexTriangleMeshShape(btStridingMeshInterface* trimesh);
#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT
virtual btGImpactMeshShape* createGimpactShape(btStridingMeshInterface* trimesh);
#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
virtual btStridingMeshInterfaceData* createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData);
virtual class btConvexHullShape* createConvexHullShape();
virtual class btCompoundShape* createCompoundShape();
virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScalingbtBvhTriangleMeshShape);
virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres);
virtual btTriangleIndexVertexArray* createMeshInterface(btStridingMeshInterfaceData& meshData);
///acceleration and connectivity structures
virtual btOptimizedBvh* createOptimizedBvh();
virtual btTriangleInfoMap* createTriangleInfoMap();
};
#endif //BT_WORLD_IMPORTER_H

12
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp vendored
View File

@@ -123,7 +123,7 @@ public:
//backup
btTransform orgTrans = m_compoundColObjWrap->getWorldTransform();
btTransform orgInterpolationTrans = m_compoundColObjWrap->getWorldTransform();
const btTransform& childTrans = compoundShape->getChildTransform(index);
btTransform newChildWorldTrans = orgTrans*childTrans ;
@@ -232,7 +232,9 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
m_compoundShapeRevision = compoundShape->getUpdateRevision();
}
if (m_childCollisionAlgorithms.size()==0)
return;
const btDbvt* tree = compoundShape->getDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
btCompoundLeafCallback callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
@@ -292,7 +294,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
btManifoldArray manifoldArray;
const btCollisionShape* childShape = 0;
btTransform orgTrans;
btTransform orgInterpolationTrans;
btTransform newChildWorldTrans;
btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
@@ -302,8 +304,8 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
{
childShape = compoundShape->getChildShape(i);
//if not longer overlapping, remove the algorithm
orgTrans = colObjWrap->getWorldTransform();
orgInterpolationTrans = colObjWrap->getWorldTransform();
orgTrans = colObjWrap->getWorldTransform();
const btTransform& childTrans = compoundShape->getChildTransform(i);
newChildWorldTrans = orgTrans*childTrans ;

5
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp vendored
View File

@@ -112,10 +112,9 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
btManifoldResult* resultOut,
btHashedSimplePairCache* childAlgorithmsCache,
btPersistentManifold* sharedManifold)
:m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
:m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
m_childCollisionAlgorithmCache(childAlgorithmsCache),
m_sharedManifold(sharedManifold),
m_numOverlapPairs(0)
m_sharedManifold(sharedManifold)
{
}

8
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp vendored
View File

@@ -47,8 +47,6 @@ subject to the following restrictions:
btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
{
m_numPerturbationIterations = 0;
m_minimumPointsPerturbationThreshold = 3;
m_simplexSolver = simplexSolver;
m_pdSolver = pdSolver;
}
@@ -57,15 +55,13 @@ btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc()
{
}
btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */)
: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
m_simplexSolver(simplexSolver),
m_pdSolver(pdSolver),
m_ownManifold (false),
m_manifoldPtr(mf),
m_lowLevelOfDetail(false),
m_numPerturbationIterations(numPerturbationIterations),
m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
m_lowLevelOfDetail(false)
{
(void)body0Wrap;
(void)body1Wrap;

3
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h vendored
View File

@@ -40,9 +40,6 @@ class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm
btPersistentManifold* m_manifoldPtr;
bool m_lowLevelOfDetail;
int m_numPerturbationIterations;
int m_minimumPointsPerturbationThreshold;
public:
btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);

7
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp vendored
View File

@@ -88,20 +88,19 @@ partId, int triangleIndex)
//just for debugging purposes
//printf("triangle %d",m_triangleCount++);
const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
btCollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher1 = m_dispatcher;
//const btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
#if 0
///debug drawing of the overlapping triangles
if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
{
const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
btVector3 color(1,1,0);
btTransform& tr = ob->getWorldTransform();
m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);

6
extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp vendored
View File

@@ -105,12 +105,12 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
int maxSize = sizeof(btConvexConvexAlgorithm);
int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm);
int maxSize3 = sizeof(btCompoundCollisionAlgorithm);
int sl = sizeof(btConvexSeparatingDistanceUtil);
sl = sizeof(btGjkPairDetector);
int maxSize4 = sizeof(btCompoundCompoundCollisionAlgorithm);
int collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize);
collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4);
if (constructionInfo.m_persistentManifoldPool)
{

4
extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp vendored
View File

@@ -28,9 +28,7 @@ int gFindSimplePairs =0;
btHashedSimplePairCache::btHashedSimplePairCache():
m_blockedForChanges(false)
{
btHashedSimplePairCache::btHashedSimplePairCache() {
int initialAllocatedSize= 2;
m_overlappingPairArray.reserve(initialAllocatedSize);
growTables();

4
extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h vendored
View File

@@ -55,9 +55,7 @@ extern int gFindSimplePairs;
class btHashedSimplePairCache
{
btSimplePairArray m_overlappingPairArray;
bool m_blockedForChanges;
protected:

6
extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp vendored
View File

@@ -193,7 +193,7 @@ struct btConnectivityProcessor : public btTriangleCallback
btScalar len2 = calculatedEdge.length2();
btScalar correctedAngle(0);
btVector3 calculatedNormalB = normalA;
//btVector3 calculatedNormalB = normalA;
bool isConvex = false;
if (len2<m_triangleInfoMap->m_planarEpsilon)
@@ -213,10 +213,6 @@ struct btConnectivityProcessor : public btTriangleCallback
isConvex = (dotA<0.);
correctedAngle = isConvex ? ang4 : -ang4;
btQuaternion orn2(calculatedEdge,-correctedAngle);
calculatedNormalB = btMatrix3x3(orn2)*normalA;
}

4
extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h vendored
View File

@@ -39,7 +39,11 @@ ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape
bool m_useQuantizedAabbCompression;
bool m_ownsBvh;
#ifdef __clang__
bool m_pad[11] __attribute__((unused));////need padding due to alignment
#else
bool m_pad[11];////need padding due to alignment
#endif
public:

10
extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h vendored
View File

@@ -117,6 +117,7 @@ public:
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
SIMD_FORCE_INLINE void deSerializeFloat(struct btCapsuleShapeData* dataBuffer);
};
@@ -181,4 +182,13 @@ SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSeri
return "btCapsuleShapeData";
}
SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer)
{
m_implicitShapeDimensions.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_implicitShapeDimensions);
m_collisionMargin = dataBuffer->m_convexInternalShapeData.m_collisionMargin;
m_localScaling.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_localScaling);
//it is best to already pre-allocate the matching btCapsuleShape*(X/Z) version to match m_upAxis
m_upAxis = dataBuffer->m_upAxis;
}
#endif //BT_CAPSULE_SHAPE_H

13
extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h vendored
View File

@@ -29,12 +29,13 @@ ATTRIBUTE_ALIGNED16(class) btCollisionShape
protected:
int m_shapeType;
void* m_userPointer;
int m_userIndex;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0)
btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1)
{
}
@@ -130,6 +131,16 @@ public:
{
return m_userPointer;
}
void setUserIndex(int index)
{
m_userIndex = index;
}
int getUserIndex() const
{
return m_userIndex;
}
virtual int calculateSerializeBufferSize() const;

11
extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp vendored
View File

@@ -18,7 +18,7 @@ subject to the following restrictions:
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
#include "LinearMath/btSerializer.h"
btCompoundShape::btCompoundShape(bool enableDynamicAabbTree)
btCompoundShape::btCompoundShape(bool enableDynamicAabbTree, const int initialChildCapacity)
: m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)),
m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)),
m_dynamicAabbTree(0),
@@ -34,6 +34,8 @@ m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
m_dynamicAabbTree = new(mem) btDbvt();
btAssert(mem==m_dynamicAabbTree);
}
m_children.reserve(initialChildCapacity);
}
@@ -77,8 +79,8 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio
if (m_dynamicAabbTree)
{
const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
int index = m_children.size();
child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
size_t index = m_children.size();
child.m_node = m_dynamicAabbTree->insert(bounds,reinterpret_cast<void*>(index) );
}
m_children.push_back(child);
@@ -312,7 +314,8 @@ void btCompoundShape::createAabbTreeFromChildren()
child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax);
const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
size_t index2 = index;
child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2) );
}
}
}

4
extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h vendored
View File

@@ -53,6 +53,7 @@ SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompou
/// Currently, removal of child shapes is only supported when disabling the aabb tree (pass 'false' in the constructor of btCompoundShape)
ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape
{
protected:
btAlignedObjectArray<btCompoundShapeChild> m_children;
btVector3 m_localAabbMin;
btVector3 m_localAabbMax;
@@ -64,13 +65,12 @@ ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape
btScalar m_collisionMargin;
protected:
btVector3 m_localScaling;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btCompoundShape(bool enableDynamicAabbTree = true);
explicit btCompoundShape(bool enableDynamicAabbTree = true, const int initialChildCapacity = 0);
virtual ~btCompoundShape();

6
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp vendored
View File

@@ -13,9 +13,9 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
//#if defined (_WIN32) || defined (__i386__)
//#define BT_USE_SSE_IN_API
//#endif
#if defined (_WIN32) || defined (__i386__)
#define BT_USE_SSE_IN_API
#endif
#include "btConvexHullShape.h"
#include "BulletCollision/CollisionShapes/btCollisionMargin.h"

3
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp vendored
View File

@@ -21,6 +21,7 @@ subject to the following restrictions:
#include "btConvexPolyhedron.h"
#include "LinearMath/btHashMap.h"
btConvexPolyhedron::btConvexPolyhedron()
{
@@ -33,7 +34,7 @@ btConvexPolyhedron::~btConvexPolyhedron()
inline bool IsAlmostZero(const btVector3& v)
{
if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6) return false;
return true;
}

14
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp vendored
View File

@@ -13,9 +13,9 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
//#if defined (_WIN32) || defined (__i386__)
//#define BT_USE_SSE_IN_API
//#endif
#if defined (_WIN32) || defined (__i386__)
#define BT_USE_SSE_IN_API
#endif
#include "btConvexShape.h"
#include "btTriangleShape.h"
@@ -48,7 +48,7 @@ btConvexShape::~btConvexShape()
}
void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin,btVector3& witnesPtMax) const
{
btVector3 localAxis = dir*trans.getBasis();
btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
@@ -56,12 +56,16 @@ void btConvexShape::project(const btTransform& trans, const btVector3& dir, btSc
min = vtx1.dot(dir);
max = vtx2.dot(dir);
witnesPtMax = vtx2;
witnesPtMin = vtx1;
if(min>max)
{
btScalar tmp = min;
min = max;
max = tmp;
witnesPtMax = vtx1;
witnesPtMin = vtx2;
}
}

3
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h vendored
View File

@@ -52,7 +52,8 @@ public:
btScalar getMarginNonVirtual () const;
void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
virtual void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const;
virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
//notice that the vectors should be unit length

25
extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp vendored
View File

@@ -59,15 +59,13 @@ PHY_ScalarType hdt, bool flipQuadEdges
)
{
// validation
btAssert(heightStickWidth > 1 && "bad width");
btAssert(heightStickLength > 1 && "bad length");
btAssert(heightfieldData && "null heightfield data");
btAssert(heightStickWidth > 1);// && "bad width");
btAssert(heightStickLength > 1);// && "bad length");
btAssert(heightfieldData);// && "null heightfield data");
// btAssert(heightScale) -- do we care? Trust caller here
btAssert(minHeight <= maxHeight && "bad min/max height");
btAssert(upAxis >= 0 && upAxis < 3 &&
"bad upAxis--should be in range [0,2]");
btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT &&
"Bad height data type enum");
btAssert(minHeight <= maxHeight);// && "bad min/max height");
btAssert(upAxis >= 0 && upAxis < 3);// && "bad upAxis--should be in range [0,2]");
btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);// && "Bad height data type enum");
// initialize member variables
m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
@@ -110,7 +108,7 @@ PHY_ScalarType hdt, bool flipQuadEdges
default:
{
//need to get valid m_upAxis
btAssert(0 && "Bad m_upAxis");
btAssert(0);// && "Bad m_upAxis");
}
}
@@ -365,14 +363,15 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
{
//first triangle
getVertex(x,j,vertices[0]);
getVertex(x+1,j,vertices[1]);
getVertex(x+1,j+1,vertices[2]);
getVertex(x, j + 1, vertices[1]);
getVertex(x + 1, j + 1, vertices[2]);
callback->processTriangle(vertices,x,j);
//second triangle
// getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman
getVertex(x+1,j+1,vertices[1]);
getVertex(x,j+1,vertices[2]);
callback->processTriangle(vertices,x,j);
getVertex(x + 1, j, vertices[2]);
callback->processTriangle(vertices, x, j);
} else
{
//first triangle

6
extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp vendored
View File

@@ -13,9 +13,9 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
//#if defined (_WIN32) || defined (__i386__)
//#define BT_USE_SSE_IN_API
//#endif
#if defined (_WIN32) || defined (__i386__)
#define BT_USE_SSE_IN_API
#endif
#include "btMultiSphereShape.h"
#include "BulletCollision/CollisionShapes/btCollisionMargin.h"

1
extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h vendored
View File

@@ -25,7 +25,6 @@ subject to the following restrictions:
ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape
{
btAlignedObjectArray <btMaterial*> m_materialList;
int ** m_triangleMaterials;
public:

6
extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp vendored
View File

@@ -12,9 +12,9 @@ subject to the following restrictions:
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.
*/
//#if defined (_WIN32) || defined (__i386__)
//#define BT_USE_SSE_IN_API
//#endif
#if defined (_WIN32) || defined (__i386__)
#define BT_USE_SSE_IN_API
#endif
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
#include "btConvexPolyhedron.h"

2
extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp vendored
View File

@@ -21,7 +21,7 @@ subject to the following restrictions:
btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant)
: btConcaveShape (), m_planeNormal(planeNormal.normalized()),
m_planeConstant(planeConstant),
m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.))
m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
{
m_shapeType = STATIC_PLANE_PROXYTYPE;
// btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) );

7
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp vendored
View File

@@ -75,6 +75,13 @@ void btTriangleMesh::addIndex(int index)
}
}
void btTriangleMesh::addTriangleIndices(int index1, int index2, int index3 )
{
m_indexedMeshes[0].m_numTriangles++;
addIndex( index1 );
addIndex( index2 );
addIndex( index3 );
}
int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices)
{

5
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h vendored
View File

@@ -52,7 +52,10 @@ class btTriangleMesh : public btTriangleIndexVertexArray
///By default addTriangle won't search for duplicate vertices, because the search is very slow for large triangle meshes.
///In general it is better to directly use btTriangleIndexVertexArray instead.
void addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2, bool removeDuplicateVertices=false);
///Add a triangle using its indices. Make sure the indices are pointing within the vertices array, so add the vertices first (and to be sure, avoid removal of duplicate vertices)
void addTriangleIndices(int index1, int index2, int index3 );
int getNumTriangles() const;
virtual void preallocateVertices(int numverts);

746
extern/bullet2/src/BulletCollision/Doxyfile vendored
View File

@@ -1,746 +0,0 @@
# Doxyfile 1.2.4
# This file describes the settings to be used by doxygen for a project
#
# All text after a hash (#) is considered a comment and will be ignored
# The format is:
# TAG = value [value, ...]
# For lists items can also be appended using:
# TAG += value [value, ...]
# Values that contain spaces should be placed between quotes (" ")
#---------------------------------------------------------------------------
# General configuration options
#---------------------------------------------------------------------------
# The PROJECT_NAME tag is a single word (or a sequence of words surrounded
# by quotes) that should identify the project.
PROJECT_NAME = "Bullet Continuous Collision Detection Library"
# The PROJECT_NUMBER tag can be used to enter a project or revision number.
# This could be handy for archiving the generated documentation or
# if some version control system is used.
PROJECT_NUMBER =
# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute)
# base path where the generated documentation will be put.
# If a relative path is entered, it will be relative to the location
# where doxygen was started. If left blank the current directory will be used.
OUTPUT_DIRECTORY =
# The OUTPUT_LANGUAGE tag is used to specify the language in which all
# documentation generated by doxygen is written. Doxygen will use this
# information to generate all constant output in the proper language.
# The default language is English, other supported languages are:
# Dutch, French, Italian, Czech, Swedish, German, Finnish, Japanese,
# Korean, Hungarian, Norwegian, Spanish, Romanian, Russian, Croatian,
# Polish, Portuguese and Slovene.
OUTPUT_LANGUAGE = English
# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in
# documentation are documented, even if no documentation was available.
# Private class members and static file members will be hidden unless
# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES
EXTRACT_ALL = YES
# If the EXTRACT_PRIVATE tag is set to YES all private members of a class
# will be included in the documentation.
EXTRACT_PRIVATE = YES
# If the EXTRACT_STATIC tag is set to YES all static members of a file
# will be included in the documentation.
EXTRACT_STATIC = YES
# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all
# undocumented members of documented classes, files or namespaces.
# If set to NO (the default) these members will be included in the
# various overviews, but no documentation section is generated.
# This option has no effect if EXTRACT_ALL is enabled.
HIDE_UNDOC_MEMBERS = NO
# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all
# undocumented classes that are normally visible in the class hierarchy.
# If set to NO (the default) these class will be included in the various
# overviews. This option has no effect if EXTRACT_ALL is enabled.
HIDE_UNDOC_CLASSES = NO
# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will
# include brief member descriptions after the members that are listed in
# the file and class documentation (similar to JavaDoc).
# Set to NO to disable this.
BRIEF_MEMBER_DESC = YES
# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend
# the brief description of a member or function before the detailed description.
# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
# brief descriptions will be completely suppressed.
REPEAT_BRIEF = YES
# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
# Doxygen will generate a detailed section even if there is only a brief
# description.
ALWAYS_DETAILED_SEC = NO
# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full
# path before files name in the file list and in the header files. If set
# to NO the shortest path that makes the file name unique will be used.
FULL_PATH_NAMES = NO
# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag
# can be used to strip a user defined part of the path. Stripping is
# only done if one of the specified strings matches the left-hand part of
# the path. It is allowed to use relative paths in the argument list.
STRIP_FROM_PATH =
# The INTERNAL_DOCS tag determines if documentation
# that is typed after a \internal command is included. If the tag is set
# to NO (the default) then the documentation will be excluded.
# Set it to YES to include the internal documentation.
INTERNAL_DOCS = NO
# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will
# generate a class diagram (in Html and LaTeX) for classes with base or
# super classes. Setting the tag to NO turns the diagrams off.
CLASS_DIAGRAMS = YES
# If the SOURCE_BROWSER tag is set to YES then a list of source files will
# be generated. Documented entities will be cross-referenced with these sources.
SOURCE_BROWSER = YES
# Setting the INLINE_SOURCES tag to YES will include the body
# of functions and classes directly in the documentation.
INLINE_SOURCES = NO
# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct
# doxygen to hide any special comment blocks from generated source code
# fragments. Normal C and C++ comments will always remain visible.
STRIP_CODE_COMMENTS = YES
# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate
# file names in lower case letters. If set to YES upper case letters are also
# allowed. This is useful if you have classes or files whose names only differ
# in case and if your file system supports case sensitive file names. Windows
# users are adviced to set this option to NO.
CASE_SENSE_NAMES = YES
# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen
# will show members with their full class and namespace scopes in the
# documentation. If set to YES the scope will be hidden.
HIDE_SCOPE_NAMES = NO
# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen
# will generate a verbatim copy of the header file for each class for
# which an include is specified. Set to NO to disable this.
VERBATIM_HEADERS = YES
# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen
# will put list of the files that are included by a file in the documentation
# of that file.
SHOW_INCLUDE_FILES = YES
# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen
# will interpret the first line (until the first dot) of a JavaDoc-style
# comment as the brief description. If set to NO, the JavaDoc
# comments will behave just like the Qt-style comments (thus requiring an
# explict @brief command for a brief description.
JAVADOC_AUTOBRIEF = YES
# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented
# member inherits the documentation from any documented member that it
# reimplements.
INHERIT_DOCS = YES
# If the INLINE_INFO tag is set to YES (the default) then a tag [inline]
# is inserted in the documentation for inline members.
INLINE_INFO = YES
# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen
# will sort the (detailed) documentation of file and class members
# alphabetically by member name. If set to NO the members will appear in
# declaration order.
SORT_MEMBER_DOCS = YES
# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
# tag is set to YES, then doxygen will reuse the documentation of the first
# member in the group (if any) for the other members of the group. By default
# all members of a group must be documented explicitly.
DISTRIBUTE_GROUP_DOC = NO
# The TAB_SIZE tag can be used to set the number of spaces in a tab.
# Doxygen uses this value to replace tabs by spaces in code fragments.
TAB_SIZE = 8
# The ENABLE_SECTIONS tag can be used to enable conditional
# documentation sections, marked by \if sectionname ... \endif.
ENABLED_SECTIONS =
# The GENERATE_TODOLIST tag can be used to enable (YES) or
# disable (NO) the todo list. This list is created by putting \todo
# commands in the documentation.
GENERATE_TODOLIST = YES
# The GENERATE_TESTLIST tag can be used to enable (YES) or
# disable (NO) the test list. This list is created by putting \test
# commands in the documentation.
GENERATE_TESTLIST = YES
# This tag can be used to specify a number of aliases that acts
# as commands in the documentation. An alias has the form "name=value".
# For example adding "sideeffect=\par Side Effects:\n" will allow you to
# put the command \sideeffect (or @sideeffect) in the documentation, which
# will result in a user defined paragraph with heading "Side Effects:".
# You can put \n's in the value part of an alias to insert newlines.
ALIASES =
#---------------------------------------------------------------------------
# configuration options related to warning and progress messages
#---------------------------------------------------------------------------
# The QUIET tag can be used to turn on/off the messages that are generated
# by doxygen. Possible values are YES and NO. If left blank NO is used.
QUIET = NO
# The WARNINGS tag can be used to turn on/off the warning messages that are
# generated by doxygen. Possible values are YES and NO. If left blank
# NO is used.
WARNINGS = YES
# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings
# for undocumented members. If EXTRACT_ALL is set to YES then this flag will
# automatically be disabled.
WARN_IF_UNDOCUMENTED = YES
# The WARN_FORMAT tag determines the format of the warning messages that
# doxygen can produce. The string should contain the $file, $line, and $text
# tags, which will be replaced by the file and line number from which the
# warning originated and the warning text.
WARN_FORMAT = "$file:$line: $text"
# The WARN_LOGFILE tag can be used to specify a file to which warning
# and error messages should be written. If left blank the output is written
# to stderr.
WARN_LOGFILE =
#---------------------------------------------------------------------------
# configuration options related to the input files
#---------------------------------------------------------------------------
# The INPUT tag can be used to specify the files and/or directories that contain
# documented source files. You may enter file names like "myfile.cpp" or
# directories like "/usr/src/myproject". Separate the files or directories
# with spaces.
INPUT = .
# If the value of the INPUT tag contains directories, you can use the
# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp
# and *.h) to filter out the source-files in the directories. If left
# blank all files are included.
FILE_PATTERNS = *.h *.cpp *.c
# The RECURSIVE tag can be used to turn specify whether or not subdirectories
# should be searched for input files as well. Possible values are YES and NO.
# If left blank NO is used.
RECURSIVE = YES
# The EXCLUDE tag can be used to specify files and/or directories that should
# excluded from the INPUT source files. This way you can easily exclude a
# subdirectory from a directory tree whose root is specified with the INPUT tag.
EXCLUDE =
# If the value of the INPUT tag contains directories, you can use the
# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
# certain files from those directories.
EXCLUDE_PATTERNS =
# The EXAMPLE_PATH tag can be used to specify one or more files or
# directories that contain example code fragments that are included (see
# the \include command).
EXAMPLE_PATH =
# If the value of the EXAMPLE_PATH tag contains directories, you can use the
# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp
# and *.h) to filter out the source-files in the directories. If left
# blank all files are included.
EXAMPLE_PATTERNS =
# The IMAGE_PATH tag can be used to specify one or more files or
# directories that contain image that are included in the documentation (see
# the \image command).
IMAGE_PATH =
# The INPUT_FILTER tag can be used to specify a program that doxygen should
# invoke to filter for each input file. Doxygen will invoke the filter program
# by executing (via popen()) the command <filter> <input-file>, where <filter>
# is the value of the INPUT_FILTER tag, and <input-file> is the name of an
# input file. Doxygen will then use the output that the filter program writes
# to standard output.
INPUT_FILTER =
# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
# INPUT_FILTER) will be used to filter the input files when producing source
# files to browse.
FILTER_SOURCE_FILES = NO
#---------------------------------------------------------------------------
# configuration options related to the alphabetical class index
#---------------------------------------------------------------------------
# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index
# of all compounds will be generated. Enable this if the project
# contains a lot of classes, structs, unions or interfaces.
ALPHABETICAL_INDEX = NO
# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then
# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns
# in which this list will be split (can be a number in the range [1..20])
COLS_IN_ALPHA_INDEX = 5
# In case all classes in a project start with a common prefix, all
# classes will be put under the same header in the alphabetical index.
# The IGNORE_PREFIX tag can be used to specify one or more prefixes that
# should be ignored while generating the index headers.
IGNORE_PREFIX =
#---------------------------------------------------------------------------
# configuration options related to the HTML output
#---------------------------------------------------------------------------
# If the GENERATE_HTML tag is set to YES (the default) Doxygen will
# generate HTML output.
GENERATE_HTML = YES
# The HTML_OUTPUT tag is used to specify where the HTML docs will be put.
# If a relative path is entered the value of OUTPUT_DIRECTORY will be
# put in front of it. If left blank `html' will be used as the default path.
HTML_OUTPUT = html
# The HTML_HEADER tag can be used to specify a personal HTML header for
# each generated HTML page. If it is left blank doxygen will generate a
# standard header.
HTML_HEADER =
# The HTML_FOOTER tag can be used to specify a personal HTML footer for
# each generated HTML page. If it is left blank doxygen will generate a
# standard footer.
HTML_FOOTER =
# The HTML_STYLESHEET tag can be used to specify a user defined cascading
# style sheet that is used by each HTML page. It can be used to
# fine-tune the look of the HTML output. If the tag is left blank doxygen
# will generate a default style sheet
HTML_STYLESHEET =
# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes,
# files or namespaces will be aligned in HTML using tables. If set to
# NO a bullet list will be used.
HTML_ALIGN_MEMBERS = YES
# If the GENERATE_HTMLHELP tag is set to YES, additional index files
# will be generated that can be used as input for tools like the
# Microsoft HTML help workshop to generate a compressed HTML help file (.chm)
# of the generated HTML documentation.
GENERATE_HTMLHELP = NO
# The DISABLE_INDEX tag can be used to turn on/off the condensed index at
# top of each HTML page. The value NO (the default) enables the index and
# the value YES disables it.
DISABLE_INDEX = NO
# This tag can be used to set the number of enum values (range [1..20])
# that doxygen will group on one line in the generated HTML documentation.
ENUM_VALUES_PER_LINE = 4
# If the GENERATE_TREEVIEW tag is set to YES, a side pannel will be
# generated containing a tree-like index structure (just like the one that
# is generated for HTML Help). For this to work a browser that supports
# JavaScript and frames is required (for instance Netscape 4.0+
# or Internet explorer 4.0+).
GENERATE_TREEVIEW = NO
# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be
# used to set the initial width (in pixels) of the frame in which the tree
# is shown.
TREEVIEW_WIDTH = 250
#---------------------------------------------------------------------------
# configuration options related to the LaTeX output
#---------------------------------------------------------------------------
# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will
# generate Latex output.
GENERATE_LATEX = NO
# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put.
# If a relative path is entered the value of OUTPUT_DIRECTORY will be
# put in front of it. If left blank `latex' will be used as the default path.
LATEX_OUTPUT = latex
# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact
# LaTeX documents. This may be useful for small projects and may help to
# save some trees in general.
COMPACT_LATEX = NO
# The PAPER_TYPE tag can be used to set the paper type that is used
# by the printer. Possible values are: a4, a4wide, letter, legal and
# executive. If left blank a4wide will be used.
PAPER_TYPE = a4wide
# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX
# packages that should be included in the LaTeX output.
EXTRA_PACKAGES =
# The LATEX_HEADER tag can be used to specify a personal LaTeX header for
# the generated latex document. The header should contain everything until
# the first chapter. If it is left blank doxygen will generate a
# standard header. Notice: only use this tag if you know what you are doing!
LATEX_HEADER =
# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated
# is prepared for conversion to pdf (using ps2pdf). The pdf file will
# contain links (just like the HTML output) instead of page references
# This makes the output suitable for online browsing using a pdf viewer.
PDF_HYPERLINKS = NO
# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of
# plain latex in the generated Makefile. Set this option to YES to get a
# higher quality PDF documentation.
USE_PDFLATEX = NO
# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode.
# command to the generated LaTeX files. This will instruct LaTeX to keep
# running if errors occur, instead of asking the user for help.
# This option is also used when generating formulas in HTML.
LATEX_BATCHMODE = NO
#---------------------------------------------------------------------------
# configuration options related to the RTF output
#---------------------------------------------------------------------------
# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output
# The RTF output is optimised for Word 97 and may not look very pretty with
# other RTF readers or editors.
GENERATE_RTF = NO
# The RTF_OUTPUT tag is used to specify where the RTF docs will be put.
# If a relative path is entered the value of OUTPUT_DIRECTORY will be
# put in front of it. If left blank `rtf' will be used as the default path.
RTF_OUTPUT = rtf
# If the COMPACT_RTF tag is set to YES Doxygen generates more compact
# RTF documents. This may be useful for small projects and may help to
# save some trees in general.
COMPACT_RTF = NO
# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated
# will contain hyperlink fields. The RTF file will
# contain links (just like the HTML output) instead of page references.
# This makes the output suitable for online browsing using a WORD or other.
# programs which support those fields.
# Note: wordpad (write) and others do not support links.
RTF_HYPERLINKS = NO
# Load stylesheet definitions from file. Syntax is similar to doxygen's
# config file, i.e. a series of assigments. You only have to provide
# replacements, missing definitions are set to their default value.
RTF_STYLESHEET_FILE =
#---------------------------------------------------------------------------
# configuration options related to the man page output
#---------------------------------------------------------------------------
# If the GENERATE_MAN tag is set to YES (the default) Doxygen will
# generate man pages
GENERATE_MAN = NO
# The MAN_OUTPUT tag is used to specify where the man pages will be put.
# If a relative path is entered the value of OUTPUT_DIRECTORY will be
# put in front of it. If left blank `man' will be used as the default path.
MAN_OUTPUT = man
# The MAN_EXTENSION tag determines the extension that is added to
# the generated man pages (default is the subroutine's section .3)
MAN_EXTENSION = .3
#---------------------------------------------------------------------------
# configuration options related to the XML output
#---------------------------------------------------------------------------
# If the GENERATE_XML tag is set to YES Doxygen will
# generate an XML file that captures the structure of
# the code including all documentation. Warning: This feature
# is still experimental and very incomplete.
GENERATE_XML = NO
#---------------------------------------------------------------------------
# Configuration options related to the preprocessor
#---------------------------------------------------------------------------
# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will
# evaluate all C-preprocessor directives found in the sources and include
# files.
ENABLE_PREPROCESSING = YES
# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro
# names in the source code. If set to NO (the default) only conditional
# compilation will be performed. Macro expansion can be done in a controlled
# way by setting EXPAND_ONLY_PREDEF to YES.
MACRO_EXPANSION = NO
# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES
# then the macro expansion is limited to the macros specified with the
# PREDEFINED and EXPAND_AS_PREDEFINED tags.
EXPAND_ONLY_PREDEF = NO
# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files
# in the INCLUDE_PATH (see below) will be search if a #include is found.
SEARCH_INCLUDES = YES
# The INCLUDE_PATH tag can be used to specify one or more directories that
# contain include files that are not input files but should be processed by
# the preprocessor.
INCLUDE_PATH = ../../generic/extern
# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
# patterns (like *.h and *.hpp) to filter out the header-files in the
# directories. If left blank, the patterns specified with FILE_PATTERNS will
# be used.
INCLUDE_FILE_PATTERNS =
# The PREDEFINED tag can be used to specify one or more macro names that
# are defined before the preprocessor is started (similar to the -D option of
# gcc). The argument of the tag is a list of macros of the form: name
# or name=definition (no spaces). If the definition and the = are
# omitted =1 is assumed.
PREDEFINED =
# If the MACRO_EXPANSION and EXPAND_PREDEF_ONLY tags are set to YES then
# this tag can be used to specify a list of macro names that should be expanded.
# The macro definition that is found in the sources will be used.
# Use the PREDEFINED tag if you want to use a different macro definition.
EXPAND_AS_DEFINED =
#---------------------------------------------------------------------------
# Configuration::addtions related to external references
#---------------------------------------------------------------------------
# The TAGFILES tag can be used to specify one or more tagfiles.
TAGFILES =
# When a file name is specified after GENERATE_TAGFILE, doxygen will create
# a tag file that is based on the input files it reads.
GENERATE_TAGFILE =
# If the ALLEXTERNALS tag is set to YES all external classes will be listed
# in the class index. If set to NO only the inherited external classes
# will be listed.
ALLEXTERNALS = NO
# The PERL_PATH should be the absolute path and name of the perl script
# interpreter (i.e. the result of `which perl').
PERL_PATH = /usr/bin/perl
#---------------------------------------------------------------------------
# Configuration options related to the dot tool
#---------------------------------------------------------------------------
# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
# available from the path. This tool is part of Graphviz, a graph visualization
# toolkit from AT&T and Lucent Bell Labs. The other options in this section
# have no effect if this option is set to NO (the default)
HAVE_DOT = YES
# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen
# will generate a graph for each documented class showing the direct and
# indirect inheritance relations. Setting this tag to YES will force the
# the CLASS_DIAGRAMS tag to NO.
CLASS_GRAPH = YES
# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen
# will generate a graph for each documented class showing the direct and
# indirect implementation dependencies (inheritance, containment, and
# class references variables) of the class with other documented classes.
COLLABORATION_GRAPH = YES
# If the ENABLE_PREPROCESSING, INCLUDE_GRAPH, and HAVE_DOT tags are set to
# YES then doxygen will generate a graph for each documented file showing
# the direct and indirect include dependencies of the file with other
# documented files.
INCLUDE_GRAPH = YES
# If the ENABLE_PREPROCESSING, INCLUDED_BY_GRAPH, and HAVE_DOT tags are set to
# YES then doxygen will generate a graph for each documented header file showing
# the documented files that directly or indirectly include this file
INCLUDED_BY_GRAPH = YES
# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen
# will graphical hierarchy of all classes instead of a textual one.
GRAPHICAL_HIERARCHY = YES
# The tag DOT_PATH can be used to specify the path where the dot tool can be
# found. If left blank, it is assumed the dot tool can be found on the path.
DOT_PATH =
# The MAX_DOT_GRAPH_WIDTH tag can be used to set the maximum allowed width
# (in pixels) of the graphs generated by dot. If a graph becomes larger than
# this value, doxygen will try to truncate the graph, so that it fits within
# the specified constraint. Beware that most browsers cannot cope with very
# large images.
MAX_DOT_GRAPH_WIDTH = 1024
# The MAX_DOT_GRAPH_HEIGHT tag can be used to set the maximum allows height
# (in pixels) of the graphs generated by dot. If a graph becomes larger than
# this value, doxygen will try to truncate the graph, so that it fits within
# the specified constraint. Beware that most browsers cannot cope with very
# large images.
MAX_DOT_GRAPH_HEIGHT = 1024
# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will
# generate a legend page explaining the meaning of the various boxes and
# arrows in the dot generated graphs.
GENERATE_LEGEND = YES
#---------------------------------------------------------------------------
# Configuration::addtions related to the search engine
#---------------------------------------------------------------------------
# The SEARCHENGINE tag specifies whether or not a search engine should be
# used. If set to NO the values of all tags below this one will be ignored.
SEARCHENGINE = NO
# The CGI_NAME tag should be the name of the CGI script that
# starts the search engine (doxysearch) with the correct parameters.
# A script with this name will be generated by doxygen.
CGI_NAME = search.cgi
# The CGI_URL tag should be the absolute URL to the directory where the
# cgi binaries are located. See the documentation of your http daemon for
# details.
CGI_URL =
# The DOC_URL tag should be the absolute URL to the directory where the
# documentation is located. If left blank the absolute path to the
# documentation, with file:// prepended to it, will be used.
DOC_URL =
# The DOC_ABSPATH tag should be the absolute path to the directory where the
# documentation is located. If left blank the directory on the local machine
# will be used.
DOC_ABSPATH =
# The BIN_ABSPATH tag must point to the directory where the doxysearch binary
# is installed.
BIN_ABSPATH = c:\program files\doxygen\bin
# The EXT_DOC_PATHS tag can be used to specify one or more paths to
# documentation generated for other projects. This allows doxysearch to search
# the documentation for these projects as well.
EXT_DOC_PATHS =

14
extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h vendored
View File

@@ -404,12 +404,12 @@ SIMD_FORCE_INLINE void SEGMENT_COLLISION(
CLASS_POINT & vPointA,
CLASS_POINT & vPointB)
{
CLASS_POINT _AD,_BD,_N;
CLASS_POINT _AD,_BD,n;
vec4f _M;//plane
VEC_DIFF(_AD,vA2,vA1);
VEC_DIFF(_BD,vB2,vB1);
VEC_CROSS(_N,_AD,_BD);
GREAL _tp = VEC_DOT(_N,_N);
VEC_CROSS(n,_AD,_BD);
GREAL _tp = VEC_DOT(n,n);
if(_tp<G_EPSILON)//ARE PARALELE
{
//project B over A
@@ -424,10 +424,10 @@ SIMD_FORCE_INLINE void SEGMENT_COLLISION(
_M[2] = VEC_DOT(vA1,_AD);
_M[3] = VEC_DOT(vA2,_AD);
//mid points
_N[0] = (_M[0]+_M[1])*0.5f;
_N[1] = (_M[2]+_M[3])*0.5f;
n[0] = (_M[0]+_M[1])*0.5f;
n[1] = (_M[2]+_M[3])*0.5f;
if(_N[0]<_N[1])
if(n[0]<n[1])
{
if(_M[1]<_M[2])
{
@@ -467,7 +467,7 @@ SIMD_FORCE_INLINE void SEGMENT_COLLISION(
}
VEC_CROSS(_M,_N,_BD);
VEC_CROSS(_M,n,_BD);
_M[3] = VEC_DOT(_M,vB1);
LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));

369
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h vendored Normal file
View File

@@ -0,0 +1,369 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2014 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_GJK_EPA_PENETATION_CONVEX_COLLISION_H
#define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision...
#include "btGjkEpa3.h"
#include "btGjkCollisionDescription.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
template <typename btConvexTemplate>
bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b,
const btGjkCollisionDescription& colDesc,
btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB)
{
(void)v;
// const btScalar radialmargin(btScalar(0.));
btVector3 guessVector(b.getWorldTransform().getOrigin()-a.getWorldTransform().getOrigin());//?? why not use the GJK input?
btGjkEpaSolver3::sResults results;
if(btGjkEpaSolver3_Penetration(a,b,guessVector,results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1];
v = results.normal;
return true;
} else
{
if(btGjkEpaSolver3_Distance(a,b,guessVector,results))
{
wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1];
v = results.normal;
return false;
}
}
return false;
}
template <typename btConvexTemplate, typename btGjkDistanceTemplate>
int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo)
{
bool m_catchDegeneracies = true;
btScalar m_cachedSeparatingDistance = 0.f;
btScalar distance=btScalar(0.);
btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 pointOnA,pointOnB;
btTransform localTransA = a.getWorldTransform();
btTransform localTransB = b.getWorldTransform();
btScalar marginA = a.getMargin();
btScalar marginB = b.getMargin();
int m_curIter = 0;
int gGjkMaxIter = colDesc.m_maxGjkIterations;//this is to catch invalid input, perhaps check for #NaN?
btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir;
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
int m_degenerateSimplex = 0;
int m_lastUsedMethod = -1;
{
btScalar squaredDistance = BT_LARGE_FLOAT;
btScalar delta = btScalar(0.);
btScalar margin = marginA + marginB;
simplexSolver.reset();
for ( ; ; )
//while (true)
{
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis();
btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis();
btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA);
btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB);
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);
btVector3 w = pWorld - qWorld;
delta = m_cachedSeparatingAxis.dot(w);
// potential exit, they don't overlap
if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared))
{
m_degenerateSimplex = 10;
checkSimplex=true;
//checkPenetration = false;
break;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if (simplexSolver.inSimplex(w))
{
m_degenerateSimplex = 1;
checkSimplex = true;
break;
}
// are we getting any closer ?
btScalar f0 = squaredDistance - delta;
btScalar f1 = squaredDistance * colDesc.m_gjkRelError2;
if (f0 <= f1)
{
if (f0 <= btScalar(0.))
{
m_degenerateSimplex = 2;
} else
{
m_degenerateSimplex = 11;
}
checkSimplex = true;
break;
}
//add current vertex to simplex
simplexSolver.addVertex(w, pWorld, qWorld);
btVector3 newCachedSeparatingAxis;
//calculate the closest point to the origin (update vector v)
if (!simplexSolver.closest(newCachedSeparatingAxis))
{
m_degenerateSimplex = 3;
checkSimplex = true;
break;
}
if(newCachedSeparatingAxis.length2()<colDesc.m_gjkRelError2)
{
m_cachedSeparatingAxis = newCachedSeparatingAxis;
m_degenerateSimplex = 6;
checkSimplex = true;
break;
}
btScalar previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.length2();
#if 0
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
if (squaredDistance>previousSquaredDistance)
{
m_degenerateSimplex = 7;
squaredDistance = previousSquaredDistance;
checkSimplex = false;
break;
}
#endif //
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
//are we getting any closer ?
if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
{
// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
checkSimplex = true;
m_degenerateSimplex = 12;
break;
}
m_cachedSeparatingAxis = newCachedSeparatingAxis;
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (m_curIter++ > gGjkMaxIter)
{
#if defined(DEBUG) || defined (_DEBUG)
printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n",
m_cachedSeparatingAxis.getX(),
m_cachedSeparatingAxis.getY(),
m_cachedSeparatingAxis.getZ(),
squaredDistance);
#endif
break;
}
bool check = (!simplexSolver.fullSimplex());
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
if (!check)
{
//do we need this backup_closest here ?
// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
m_degenerateSimplex = 13;
break;
}
}
if (checkSimplex)
{
simplexSolver.compute_points(pointOnA, pointOnB);
normalInB = m_cachedSeparatingAxis;
btScalar lenSqr =m_cachedSeparatingAxis.length2();
//valid normal
if (lenSqr < 0.0001)
{
m_degenerateSimplex = 5;
}
if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
normalInB *= rlen; //normalize
btScalar s = btSqrt(squaredDistance);
btAssert(s > btScalar(0.0));
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
pointOnB += m_cachedSeparatingAxis * (marginB / s);
distance = ((btScalar(1.)/rlen) - margin);
isValid = true;
m_lastUsedMethod = 1;
} else
{
m_lastUsedMethod = 2;
}
}
bool catchDegeneratePenetrationCase =
(m_catchDegeneracies && m_degenerateSimplex && ((distance+margin) < 0.01));
//if (checkPenetration && !isValid)
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
{
//penetration case
//if there is no way to handle penetrations, bail out
// Penetration depth case.
btVector3 tmpPointOnA,tmpPointOnB;
m_cachedSeparatingAxis.setZero();
bool isValid2 = btGjkEpaCalcPenDepth(a,b,
colDesc,
m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
if (isValid2)
{
btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
btScalar lenSqr = tmpNormalInB.length2();
if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
{
tmpNormalInB = m_cachedSeparatingAxis;
lenSqr = m_cachedSeparatingAxis.length2();
}
if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
{
tmpNormalInB /= btSqrt(lenSqr);
btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
//only replace valid penetrations when the result is deeper (check)
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
isValid = true;
m_lastUsedMethod = 3;
} else
{
m_lastUsedMethod = 8;
}
} else
{
m_lastUsedMethod = 9;
}
} else
{
///this is another degenerate case, where the initial GJK calculation reports a degenerate case
///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
///reports a valid positive distance. Use the results of the second GJK instead of failing.
///thanks to Jacob.Langford for the reproduction case
///http://code.google.com/p/bullet/issues/detail?id=250
if (m_cachedSeparatingAxis.length2() > btScalar(0.))
{
btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
//only replace valid distances when the distance is less
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
pointOnA -= m_cachedSeparatingAxis * marginA ;
pointOnB += m_cachedSeparatingAxis * marginB ;
normalInB = m_cachedSeparatingAxis;
normalInB.normalize();
isValid = true;
m_lastUsedMethod = 6;
} else
{
m_lastUsedMethod = 5;
}
}
}
}
}
if (isValid && ((distance < 0) || (distance*distance < colDesc.m_maximumDistanceSquared)))
{
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
distInfo->m_distance = distance;
distInfo->m_normalBtoA = normalInB;
distInfo->m_pointOnB = pointOnB;
distInfo->m_pointOnA = pointOnB+normalInB*distance;
return 0;
}
return -m_lastUsedMethod;
}
#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H

41
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h vendored Normal file
View File

@@ -0,0 +1,41 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2014 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 GJK_COLLISION_DESCRIPTION_H
#define GJK_COLLISION_DESCRIPTION_H
#include "LinearMath/btVector3.h"
struct btGjkCollisionDescription
{
btVector3 m_firstDir;
int m_maxGjkIterations;
btScalar m_maximumDistanceSquared;
btScalar m_gjkRelError2;
btGjkCollisionDescription()
:m_firstDir(0,1,0),
m_maxGjkIterations(1000),
m_maximumDistanceSquared(1e30f),
m_gjkRelError2(1.0e-6)
{
}
virtual ~btGjkCollisionDescription()
{
}
};
#endif //GJK_COLLISION_DESCRIPTION_H

1035
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

119
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp vendored
View File

@@ -26,7 +26,6 @@ subject to the following restrictions:
#ifdef __SPU__
#include <spu_printf.h>
#define printf spu_printf
//#define DEBUG_SPU_COLLISION_DETECTION 1
#endif //__SPU__
#endif
@@ -81,17 +80,18 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
#ifdef __SPU__
void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
#else
void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw)
#endif
{
m_cachedSeparatingDistance = 0.f;
btScalar distance=btScalar(0.);
btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 pointOnA,pointOnB;
btTransform localTransA = input.m_transformA;
btTransform localTransB = input.m_transformB;
btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
btVector3 positionOffset=(localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
localTransA.getOrigin() -= positionOffset;
localTransB.getOrigin() -= positionOffset;
@@ -102,17 +102,11 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
gNumGjkChecks++;
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("inside gjk\n");
#endif
//for CCD we don't use margins
if (m_ignoreMargin)
{
marginA = btScalar(0.);
marginB = btScalar(0.);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("ignoring margin\n");
#endif
}
m_curIter = 0;
@@ -143,37 +137,13 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
#if 1
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
// btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
// btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
#else
#ifdef __SPU__
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
#else
btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
#ifdef TEST_NON_VIRTUAL
btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
btAssert((pInAv-pInA).length() < 0.0001);
btAssert((qInBv-qInB).length() < 0.0001);
#endif //
#endif //__SPU__
#endif
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("got local supporting vertices\n");
#endif
if (check2d)
{
@@ -217,14 +187,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
break;
}
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("addVertex 1\n");
#endif
//add current vertex to simplex
m_simplexSolver->addVertex(w, pWorld, qWorld);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("addVertex 2\n");
#endif
btVector3 newCachedSeparatingAxis;
//calculate the closest point to the origin (update vector v)
@@ -274,7 +238,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (m_curIter++ > gGjkMaxIter)
{
#if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
#if defined(DEBUG) || defined (_DEBUG)
printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
@@ -307,6 +271,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
{
m_simplexSolver->compute_points(pointOnA, pointOnB);
normalInB = m_cachedSeparatingAxis;
btScalar lenSqr =m_cachedSeparatingAxis.length2();
//valid normal
@@ -318,6 +283,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
normalInB *= rlen; //normalize
btScalar s = btSqrt(squaredDistance);
btAssert(s > btScalar(0.0));
@@ -373,6 +339,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
{
tmpNormalInB /= btSqrt(lenSqr);
btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
m_lastUsedMethod = 3;
//only replace valid penetrations when the result is deeper (check)
if (!isValid || (distance2 < distance))
{
@@ -380,8 +347,48 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
///todo: need to track down this EPA penetration solver degeneracy
///the penetration solver reports penetration but the contact normal
///connecting the contact points is pointing in the opposite direction
///until then, detect the issue and revert the normal
{
btScalar d1=0;
{
btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis();
btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis();
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);
btVector3 w = pWorld - qWorld;
d1 = (-normalInB).dot(w);
}
btScalar d0 = 0.f;
{
btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis();
btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis();
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);
btVector3 w = pWorld - qWorld;
d0 = normalInB.dot(w);
}
if (d1>d0)
{
m_lastUsedMethod = 10;
normalInB*=-1;
}
}
isValid = true;
m_lastUsedMethod = 3;
} else
{
m_lastUsedMethod = 8;
@@ -413,6 +420,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
pointOnB += m_cachedSeparatingAxis * marginB ;
normalInB = m_cachedSeparatingAxis;
normalInB.normalize();
isValid = true;
m_lastUsedMethod = 6;
} else
@@ -431,36 +439,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
{
#if 0
///some debugging
// if (check2d)
{
printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]);
printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex);
}
#endif
if (m_fixContactNormalDirection)
{
///@workaround for sticky convex collisions
//in some degenerate cases (usually when the use uses very small margins)
//the contact normal is pointing the wrong direction
//so fix it now (until we can deal with all degenerate cases in GJK and EPA)
//contact normals need to point from B to A in all cases, so we can simply check if the contact normal really points from B to A
//We like to use a dot product of the normal against the difference of the centroids,
//once the centroid is available in the API
//until then we use the center of the aabb to approximate the centroid
btVector3 aabbMin,aabbMax;
m_minkowskiA->getAabb(localTransA,aabbMin,aabbMax);
btVector3 posA = (aabbMax+aabbMin)*btScalar(0.5);
m_minkowskiB->getAabb(localTransB,aabbMin,aabbMax);
btVector3 posB = (aabbMin+aabbMax)*btScalar(0.5);
btVector3 diff = posA-posB;
if (diff.dot(normalInB) < 0.f)
normalInB *= -1.f;
}
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;

908
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h vendored Normal file
View File

@@ -0,0 +1,908 @@
/***
* ---------------------------------
* Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
*
* This file was ported from mpr.c file, part of libccd.
* The Minkoski Portal Refinement implementation was ported
* to OpenCL by Erwin Coumans for the Bullet 3 Physics library.
* The original MPR idea and implementation is by Gary Snethen
* in XenoCollide, see http://github.com/erwincoumans/xenocollide
*
* Distributed under the OSI-approved BSD License (the "License");
* see <http://www.opensource.org/licenses/bsd-license.php>.
* This software is distributed WITHOUT ANY WARRANTY; without even the
* implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the License for more information.
*/
///2014 Oct, Erwin Coumans, Use templates to avoid void* casts
#ifndef BT_MPR_PENETRATION_H
#define BT_MPR_PENETRATION_H
#define BT_DEBUG_MPR1
#include "LinearMath/btTransform.h"
#include "LinearMath/btAlignedObjectArray.h"
//#define MPR_AVERAGE_CONTACT_POSITIONS
struct btMprCollisionDescription
{
btVector3 m_firstDir;
int m_maxGjkIterations;
btScalar m_maximumDistanceSquared;
btScalar m_gjkRelError2;
btMprCollisionDescription()
: m_firstDir(0,1,0),
m_maxGjkIterations(1000),
m_maximumDistanceSquared(1e30f),
m_gjkRelError2(1.0e-6)
{
}
virtual ~btMprCollisionDescription()
{
}
};
struct btMprDistanceInfo
{
btVector3 m_pointOnA;
btVector3 m_pointOnB;
btVector3 m_normalBtoA;
btScalar m_distance;
};
#ifdef __cplusplus
#define BT_MPR_SQRT sqrtf
#else
#define BT_MPR_SQRT sqrt
#endif
#define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
#define BT_MPR_FABS fabs
#define BT_MPR_TOLERANCE 1E-6f
#define BT_MPR_MAX_ITERATIONS 1000
struct _btMprSupport_t
{
btVector3 v; //!< Support point in minkowski sum
btVector3 v1; //!< Support point in obj1
btVector3 v2; //!< Support point in obj2
};
typedef struct _btMprSupport_t btMprSupport_t;
struct _btMprSimplex_t
{
btMprSupport_t ps[4];
int last; //!< index of last added point
};
typedef struct _btMprSimplex_t btMprSimplex_t;
inline btMprSupport_t* btMprSimplexPointW(btMprSimplex_t *s, int idx)
{
return &s->ps[idx];
}
inline void btMprSimplexSetSize(btMprSimplex_t *s, int size)
{
s->last = size - 1;
}
#ifdef DEBUG_MPR
inline void btPrintPortalVertex(_btMprSimplex_t* portal, int index)
{
printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(),portal->ps[index].v.y(),portal->ps[index].v.z(),
portal->ps[index].v1.x(),portal->ps[index].v1.y(),portal->ps[index].v1.z(),
portal->ps[index].v2.x(),portal->ps[index].v2.y(),portal->ps[index].v2.z());
}
#endif //DEBUG_MPR
inline int btMprSimplexSize(const btMprSimplex_t *s)
{
return s->last + 1;
}
inline const btMprSupport_t* btMprSimplexPoint(const btMprSimplex_t* s, int idx)
{
// here is no check on boundaries
return &s->ps[idx];
}
inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
{
*d = *s;
}
inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
{
btMprSupportCopy(s->ps + pos, a);
}
inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
{
btMprSupport_t supp;
btMprSupportCopy(&supp, &s->ps[pos1]);
btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
btMprSupportCopy(&s->ps[pos2], &supp);
}
inline int btMprIsZero(float val)
{
return BT_MPR_FABS(val) < FLT_EPSILON;
}
inline int btMprEq(float _a, float _b)
{
float ab;
float a, b;
ab = BT_MPR_FABS(_a - _b);
if (BT_MPR_FABS(ab) < FLT_EPSILON)
return 1;
a = BT_MPR_FABS(_a);
b = BT_MPR_FABS(_b);
if (b > a){
return ab < FLT_EPSILON * b;
}else{
return ab < FLT_EPSILON * a;
}
}
inline int btMprVec3Eq(const btVector3* a, const btVector3 *b)
{
return btMprEq((*a).x(), (*b).x())
&& btMprEq((*a).y(), (*b).y())
&& btMprEq((*a).z(), (*b).z());
}
template <typename btConvexTemplate>
inline void btFindOrigin(const btConvexTemplate& a, const btConvexTemplate& b, const btMprCollisionDescription& colDesc,btMprSupport_t *center)
{
center->v1 = a.getObjectCenterInWorld();
center->v2 = b.getObjectCenterInWorld();
center->v = center->v1 - center->v2;
}
inline void btMprVec3Set(btVector3 *v, float x, float y, float z)
{
v->setValue(x,y,z);
}
inline void btMprVec3Add(btVector3 *v, const btVector3 *w)
{
*v += *w;
}
inline void btMprVec3Copy(btVector3 *v, const btVector3 *w)
{
*v = *w;
}
inline void btMprVec3Scale(btVector3 *d, float k)
{
*d *= k;
}
inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
{
float dot;
dot = btDot(*a,*b);
return dot;
}
inline float btMprVec3Len2(const btVector3 *v)
{
return btMprVec3Dot(v, v);
}
inline void btMprVec3Normalize(btVector3 *d)
{
float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d));
btMprVec3Scale(d, k);
}
inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
{
*d = btCross(*a,*b);
}
inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
{
*d = *v - *w;
}
inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
{
btVector3 v2v1, v3v1;
btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v,
&btMprSimplexPoint(portal, 1)->v);
btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v,
&btMprSimplexPoint(portal, 1)->v);
btMprVec3Cross(dir, &v2v1, &v3v1);
btMprVec3Normalize(dir);
}
inline int portalEncapsulesOrigin(const btMprSimplex_t *portal,
const btVector3 *dir)
{
float dot;
dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v);
return btMprIsZero(dot) || dot > 0.f;
}
inline int portalReachTolerance(const btMprSimplex_t *portal,
const btMprSupport_t *v4,
const btVector3 *dir)
{
float dv1, dv2, dv3, dv4;
float dot1, dot2, dot3;
// find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir);
dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir);
dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir);
dv4 = btMprVec3Dot(&v4->v, dir);
dot1 = dv4 - dv1;
dot2 = dv4 - dv2;
dot3 = dv4 - dv3;
dot1 = BT_MPR_FMIN(dot1, dot2);
dot1 = BT_MPR_FMIN(dot1, dot3);
return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE;
}
inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal,
const btMprSupport_t *v4,
const btVector3 *dir)
{
float dot;
dot = btMprVec3Dot(&v4->v, dir);
return btMprIsZero(dot) || dot > 0.f;
}
inline void btExpandPortal(btMprSimplex_t *portal,
const btMprSupport_t *v4)
{
float dot;
btVector3 v4v0;
btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v);
dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0);
if (dot > 0.f){
dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0);
if (dot > 0.f){
btMprSimplexSet(portal, 1, v4);
}else{
btMprSimplexSet(portal, 3, v4);
}
}else{
dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0);
if (dot > 0.f){
btMprSimplexSet(portal, 2, v4);
}else{
btMprSimplexSet(portal, 1, v4);
}
}
}
template <typename btConvexTemplate>
inline void btMprSupport(const btConvexTemplate& a, const btConvexTemplate& b,
const btMprCollisionDescription& colDesc,
const btVector3& dir, btMprSupport_t *supp)
{
btVector3 seperatingAxisInA = dir* a.getWorldTransform().getBasis();
btVector3 seperatingAxisInB = -dir* b.getWorldTransform().getBasis();
btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA);
btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB);
supp->v1 = a.getWorldTransform()(pInA);
supp->v2 = b.getWorldTransform()(qInB);
supp->v = supp->v1 - supp->v2;
}
template <typename btConvexTemplate>
static int btDiscoverPortal(const btConvexTemplate& a, const btConvexTemplate& b,
const btMprCollisionDescription& colDesc,
btMprSimplex_t *portal)
{
btVector3 dir, va, vb;
float dot;
int cont;
// vertex 0 is center of portal
btFindOrigin(a,b,colDesc, btMprSimplexPointW(portal, 0));
// vertex 0 is center of portal
btMprSimplexSetSize(portal, 1);
btVector3 zero = btVector3(0,0,0);
btVector3* org = &zero;
if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org)){
// Portal's center lies on origin (0,0,0) => we know that objects
// intersect but we would need to know penetration info.
// So move center little bit...
btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va);
}
// vertex 1 = support in direction of origin
btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v);
btMprVec3Scale(&dir, -1.f);
btMprVec3Normalize(&dir);
btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 1));
btMprSimplexSetSize(portal, 2);
// test if origin isn't outside of v1
dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir);
if (btMprIsZero(dot) || dot < 0.f)
return -1;
// vertex 2
btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v,
&btMprSimplexPoint(portal, 1)->v);
if (btMprIsZero(btMprVec3Len2(&dir))){
if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org)){
// origin lies on v1
return 1;
}else{
// origin lies on v0-v1 segment
return 2;
}
}
btMprVec3Normalize(&dir);
btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 2));
dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir);
if (btMprIsZero(dot) || dot < 0.f)
return -1;
btMprSimplexSetSize(portal, 3);
// vertex 3 direction
btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
&btMprSimplexPoint(portal, 0)->v);
btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
&btMprSimplexPoint(portal, 0)->v);
btMprVec3Cross(&dir, &va, &vb);
btMprVec3Normalize(&dir);
// it is better to form portal faces to be oriented "outside" origin
dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v);
if (dot > 0.f){
btMprSimplexSwap(portal, 1, 2);
btMprVec3Scale(&dir, -1.f);
}
while (btMprSimplexSize(portal) < 4){
btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 3));
dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir);
if (btMprIsZero(dot) || dot < 0.f)
return -1;
cont = 0;
// test if origin is outside (v1, v0, v3) - set v2 as v3 and
// continue
btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v,
&btMprSimplexPoint(portal, 3)->v);
dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
if (dot < 0.f && !btMprIsZero(dot)){
btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3));
cont = 1;
}
if (!cont){
// test if origin is outside (v3, v0, v2) - set v1 as v3 and
// continue
btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v,
&btMprSimplexPoint(portal, 2)->v);
dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
if (dot < 0.f && !btMprIsZero(dot)){
btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3));
cont = 1;
}
}
if (cont){
btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
&btMprSimplexPoint(portal, 0)->v);
btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
&btMprSimplexPoint(portal, 0)->v);
btMprVec3Cross(&dir, &va, &vb);
btMprVec3Normalize(&dir);
}else{
btMprSimplexSetSize(portal, 4);
}
}
return 0;
}
template <typename btConvexTemplate>
static int btRefinePortal(const btConvexTemplate& a, const btConvexTemplate& b,const btMprCollisionDescription& colDesc,
btMprSimplex_t *portal)
{
btVector3 dir;
btMprSupport_t v4;
for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++)
//while (1)
{
// compute direction outside the portal (from v0 throught v1,v2,v3
// face)
btPortalDir(portal, &dir);
// test if origin is inside the portal
if (portalEncapsulesOrigin(portal, &dir))
return 0;
// get next support point
btMprSupport(a,b,colDesc, dir, &v4);
// test if v4 can expand portal to contain origin and if portal
// expanding doesn't reach given tolerance
if (!portalCanEncapsuleOrigin(portal, &v4, &dir)
|| portalReachTolerance(portal, &v4, &dir))
{
return -1;
}
// v1-v2-v3 triangle must be rearranged to face outside Minkowski
// difference (direction from v0).
btExpandPortal(portal, &v4);
}
return -1;
}
static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
{
btVector3 zero = btVector3(0,0,0);
btVector3* origin = &zero;
btVector3 dir;
size_t i;
float b[4], sum, inv;
btVector3 vec, p1, p2;
btPortalDir(portal, &dir);
// use barycentric coordinates of tetrahedron to find origin
btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
&btMprSimplexPoint(portal, 2)->v);
b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
&btMprSimplexPoint(portal, 2)->v);
b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v,
&btMprSimplexPoint(portal, 1)->v);
b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
&btMprSimplexPoint(portal, 1)->v);
b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
sum = b[0] + b[1] + b[2] + b[3];
if (btMprIsZero(sum) || sum < 0.f){
b[0] = 0.f;
btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
&btMprSimplexPoint(portal, 3)->v);
b[1] = btMprVec3Dot(&vec, &dir);
btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
&btMprSimplexPoint(portal, 1)->v);
b[2] = btMprVec3Dot(&vec, &dir);
btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
&btMprSimplexPoint(portal, 2)->v);
b[3] = btMprVec3Dot(&vec, &dir);
sum = b[1] + b[2] + b[3];
}
inv = 1.f / sum;
btMprVec3Copy(&p1, origin);
btMprVec3Copy(&p2, origin);
for (i = 0; i < 4; i++){
btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1);
btMprVec3Scale(&vec, b[i]);
btMprVec3Add(&p1, &vec);
btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2);
btMprVec3Scale(&vec, b[i]);
btMprVec3Add(&p2, &vec);
}
btMprVec3Scale(&p1, inv);
btMprVec3Scale(&p2, inv);
#ifdef MPR_AVERAGE_CONTACT_POSITIONS
btMprVec3Copy(pos, &p1);
btMprVec3Add(pos, &p2);
btMprVec3Scale(pos, 0.5);
#else
btMprVec3Copy(pos, &p2);
#endif//MPR_AVERAGE_CONTACT_POSITIONS
}
inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
{
btVector3 ab;
btMprVec3Sub2(&ab, a, b);
return btMprVec3Len2(&ab);
}
inline float _btMprVec3PointSegmentDist2(const btVector3 *P,
const btVector3 *x0,
const btVector3 *b,
btVector3 *witness)
{
// The computation comes from solving equation of segment:
// S(t) = x0 + t.d
// where - x0 is initial point of segment
// - d is direction of segment from x0 (|d| > 0)
// - t belongs to <0, 1> interval
//
// Than, distance from a segment to some point P can be expressed:
// D(t) = |x0 + t.d - P|^2
// which is distance from any point on segment. Minimization
// of this function brings distance from P to segment.
// Minimization of D(t) leads to simple quadratic equation that's
// solving is straightforward.
//
// Bonus of this method is witness point for free.
float dist, t;
btVector3 d, a;
// direction of segment
btMprVec3Sub2(&d, b, x0);
// precompute vector from P to x0
btMprVec3Sub2(&a, x0, P);
t = -1.f * btMprVec3Dot(&a, &d);
t /= btMprVec3Len2(&d);
if (t < 0.f || btMprIsZero(t)){
dist = btMprVec3Dist2(x0, P);
if (witness)
btMprVec3Copy(witness, x0);
}else if (t > 1.f || btMprEq(t, 1.f)){
dist = btMprVec3Dist2(b, P);
if (witness)
btMprVec3Copy(witness, b);
}else{
if (witness){
btMprVec3Copy(witness, &d);
btMprVec3Scale(witness, t);
btMprVec3Add(witness, x0);
dist = btMprVec3Dist2(witness, P);
}else{
// recycling variables
btMprVec3Scale(&d, t);
btMprVec3Add(&d, &a);
dist = btMprVec3Len2(&d);
}
}
return dist;
}
inline float btMprVec3PointTriDist2(const btVector3 *P,
const btVector3 *x0, const btVector3 *B,
const btVector3 *C,
btVector3 *witness)
{
// Computation comes from analytic expression for triangle (x0, B, C)
// T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
// Then equation for distance is:
// D(s, t) = | T(s, t) - P |^2
// This leads to minimization of quadratic function of two variables.
// The solution from is taken only if s is between 0 and 1, t is
// between 0 and 1 and t + s < 1, otherwise distance from segment is
// computed.
btVector3 d1, d2, a;
float u, v, w, p, q, r;
float s, t, dist, dist2;
btVector3 witness2;
btMprVec3Sub2(&d1, B, x0);
btMprVec3Sub2(&d2, C, x0);
btMprVec3Sub2(&a, x0, P);
u = btMprVec3Dot(&a, &a);
v = btMprVec3Dot(&d1, &d1);
w = btMprVec3Dot(&d2, &d2);
p = btMprVec3Dot(&a, &d1);
q = btMprVec3Dot(&a, &d2);
r = btMprVec3Dot(&d1, &d2);
btScalar div = (w * v - r * r);
if (btMprIsZero(div))
{
s=-1;
} else
{
s = (q * r - w * p) / div;
t = (-s * r - q) / w;
}
if ((btMprIsZero(s) || s > 0.f)
&& (btMprEq(s, 1.f) || s < 1.f)
&& (btMprIsZero(t) || t > 0.f)
&& (btMprEq(t, 1.f) || t < 1.f)
&& (btMprEq(t + s, 1.f) || t + s < 1.f)){
if (witness){
btMprVec3Scale(&d1, s);
btMprVec3Scale(&d2, t);
btMprVec3Copy(witness, x0);
btMprVec3Add(witness, &d1);
btMprVec3Add(witness, &d2);
dist = btMprVec3Dist2(witness, P);
}else{
dist = s * s * v;
dist += t * t * w;
dist += 2.f * s * t * r;
dist += 2.f * s * p;
dist += 2.f * t * q;
dist += u;
}
}else{
dist = _btMprVec3PointSegmentDist2(P, x0, B, witness);
dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2);
if (dist2 < dist){
dist = dist2;
if (witness)
btMprVec3Copy(witness, &witness2);
}
dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2);
if (dist2 < dist){
dist = dist2;
if (witness)
btMprVec3Copy(witness, &witness2);
}
}
return dist;
}
template <typename btConvexTemplate>
static void btFindPenetr(const btConvexTemplate& a, const btConvexTemplate& b,
const btMprCollisionDescription& colDesc,
btMprSimplex_t *portal,
float *depth, btVector3 *pdir, btVector3 *pos)
{
btVector3 dir;
btMprSupport_t v4;
unsigned long iterations;
btVector3 zero = btVector3(0,0,0);
btVector3* origin = &zero;
iterations = 1UL;
for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++)
//while (1)
{
// compute portal direction and obtain next support point
btPortalDir(portal, &dir);
btMprSupport(a,b,colDesc, dir, &v4);
// reached tolerance -> find penetration info
if (portalReachTolerance(portal, &v4, &dir)
|| iterations ==BT_MPR_MAX_ITERATIONS)
{
*depth = btMprVec3PointTriDist2(origin,&btMprSimplexPoint(portal, 1)->v,&btMprSimplexPoint(portal, 2)->v,&btMprSimplexPoint(portal, 3)->v,pdir);
*depth = BT_MPR_SQRT(*depth);
if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z()))
{
*pdir = dir;
}
btMprVec3Normalize(pdir);
// barycentric coordinates:
btFindPos(portal, pos);
return;
}
btExpandPortal(portal, &v4);
iterations++;
}
}
static void btFindPenetrTouch(btMprSimplex_t *portal,float *depth, btVector3 *dir, btVector3 *pos)
{
// Touching contact on portal's v1 - so depth is zero and direction
// is unimportant and pos can be guessed
*depth = 0.f;
btVector3 zero = btVector3(0,0,0);
btVector3* origin = &zero;
btMprVec3Copy(dir, origin);
#ifdef MPR_AVERAGE_CONTACT_POSITIONS
btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
btMprVec3Scale(pos, 0.5);
#else
btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
#endif
}
static void btFindPenetrSegment(btMprSimplex_t *portal,
float *depth, btVector3 *dir, btVector3 *pos)
{
// Origin lies on v0-v1 segment.
// Depth is distance to v1, direction also and position must be
// computed
#ifdef MPR_AVERAGE_CONTACT_POSITIONS
btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
btMprVec3Scale(pos, 0.5f);
#else
btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
#endif//MPR_AVERAGE_CONTACT_POSITIONS
btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v);
*depth = BT_MPR_SQRT(btMprVec3Len2(dir));
btMprVec3Normalize(dir);
}
template <typename btConvexTemplate>
inline int btMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b,
const btMprCollisionDescription& colDesc,
float *depthOut, btVector3* dirOut, btVector3* posOut)
{
btMprSimplex_t portal;
// Phase 1: Portal discovery
int result = btDiscoverPortal(a,b,colDesc, &portal);
//sepAxis[pairIndex] = *pdir;//or -dir?
switch (result)
{
case 0:
{
// Phase 2: Portal refinement
result = btRefinePortal(a,b,colDesc, &portal);
if (result < 0)
return -1;
// Phase 3. Penetration info
btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut);
break;
}
case 1:
{
// Touching contact on portal's v1.
btFindPenetrTouch(&portal, depthOut, dirOut, posOut);
result=0;
break;
}
case 2:
{
btFindPenetrSegment( &portal, depthOut, dirOut, posOut);
result=0;
break;
}
default:
{
//if (res < 0)
//{
// Origin isn't inside portal - no collision.
result = -1;
//}
}
};
return result;
};
template<typename btConvexTemplate, typename btMprDistanceTemplate>
inline int btComputeMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, const
btMprCollisionDescription& colDesc, btMprDistanceTemplate* distInfo)
{
btVector3 dir,pos;
float depth;
int res = btMprPenetration(a,b,colDesc,&depth, &dir, &pos);
if (res==0)
{
distInfo->m_distance = -depth;
distInfo->m_pointOnB = pos;
distInfo->m_normalBtoA = -dir;
distInfo->m_pointOnA = pos-distInfo->m_distance*dir;
return 0;
}
return -1;
}
#endif //BT_MPR_PENETRATION_H

4
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h vendored
View File

@@ -196,10 +196,6 @@ public:
m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
m_pointCache[insertIndex].m_lifeTime = lifeTime;
#else

4
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp vendored
View File

@@ -116,7 +116,7 @@ static int gActualSATPairTests=0;
inline bool IsAlmostZero(const btVector3& v)
{
if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6) return false;
return true;
}
@@ -313,7 +313,7 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
int edgeB=-1;
btVector3 worldEdgeA;
btVector3 worldEdgeB;
btVector3 witnessPointA,witnessPointB;
btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0);
int curEdgeEdge = 0;

6
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h vendored
View File

@@ -32,10 +32,12 @@ public:
//@BP Mod - allow backface filtering and unflipped normals
enum EFlags
{
kF_None = 0,
kF_None = 0,
kF_FilterBackfaces = 1 << 0,
kF_KeepUnflippedNormal = 1 << 1, // Prevents returned face normal getting flipped when a ray hits a back-facing triangle
kF_UseSubSimplexConvexCastRaytest = 1 << 2, // Uses an approximate but faster ray versus convex intersection algorithm
///SubSimplexConvexCastRaytest is the default, even if kF_None is set.
kF_UseSubSimplexConvexCastRaytest = 1 << 2, // Uses an approximate but faster ray versus convex intersection algorithm
kF_UseGjkConvexCastRaytest = 1 << 3,
kF_Terminator = 0xFFFFFFFF
};
unsigned int m_flags;

10
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp vendored
View File

@@ -65,10 +65,10 @@ bool btSubsimplexConvexCast::calcTimeOfImpact(
btVector3 n;
n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
bool hasResult = false;
btVector3 c;
btScalar lastLambda = lambda;
btScalar dist2 = v.length2();
@@ -109,9 +109,9 @@ bool btSubsimplexConvexCast::calcTimeOfImpact(
//m_simplexSolver->reset();
//check next line
w = supVertexA-supVertexB;
lastLambda = lambda;
n = v;
hasResult = true;
}
}
///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
@@ -121,7 +121,7 @@ bool btSubsimplexConvexCast::calcTimeOfImpact(
if (m_simplexSolver->closest(v))
{
dist2 = v.length2();
hasResult = true;
//todo: check this normal for validity
//n=v;
//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);

3
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp vendored
View File

@@ -294,7 +294,10 @@ bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
#else
if (m_simplexVectorW[i] == w)
#endif
{
found = true;
break;
}
}
//check in case lastW is already removed

18
extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp vendored
View File

@@ -29,13 +29,11 @@ subject to the following restrictions:
static btVector3
getNormalizedVector(const btVector3& v)
{
btScalar l = v.length();
btVector3 n = v;
if (l < SIMD_EPSILON) {
n.setValue(0,0,0);
} else {
n /= l;
}
btVector3 n(0, 0, 0);
if (v.length() > SIMD_EPSILON) {
n = v.normalized();
}
return n;
}
@@ -383,8 +381,8 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
if (callback.hasHit())
{
// we moved only a fraction
btScalar hitDistance;
hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
//btScalar hitDistance;
//hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
// m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
@@ -638,7 +636,7 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
// printf(" dt = %f", dt);
// quick check...
if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
// printf("\n");
return; // no motion
}

16
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp vendored
View File

@@ -214,7 +214,7 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
}
// m_swingCorrection is always positive or 0
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
info->m_upperLimit[srow] = (m_bMotorEnabled && m_maxMotorImpulse >= 0.0f) ? m_maxMotorImpulse : SIMD_INFINITY;
srow += info->rowskip;
}
}
@@ -540,8 +540,8 @@ void btConeTwistConstraint::calcAngleInfo()
m_solveTwistLimit = false;
m_solveSwingLimit = false;
btVector3 b1Axis1,b1Axis2,b1Axis3;
btVector3 b2Axis1,b2Axis2;
btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0);
b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
@@ -778,8 +778,10 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
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();
m_swingCorrection = m_swingAxis.length();
if (!btFuzzyZero(m_swingCorrection))
m_swingAxis.normalize();
}
}
@@ -983,8 +985,8 @@ void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingA
void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
{
btTransform trACur = m_rbA.getCenterOfMassTransform();
btTransform trBCur = m_rbB.getCenterOfMassTransform();
//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);

66
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h vendored
View File

@@ -170,6 +170,11 @@ public:
{
m_angularOnly = angularOnly;
}
bool getAngularOnly() const
{
return m_angularOnly;
}
void setLimit(int limitIndex,btScalar limitValue)
{
@@ -196,6 +201,33 @@ public:
};
}
btScalar getLimit(int limitIndex) const
{
switch (limitIndex)
{
case 3:
{
return m_twistSpan;
break;
}
case 4:
{
return m_swingSpan2;
break;
}
case 5:
{
return m_swingSpan1;
break;
}
default:
{
btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
return 0.0;
}
};
}
// setLimit(), a few notes:
// _softness:
// 0->1, recommend ~0.8->1.
@@ -218,8 +250,8 @@ public:
m_relaxationFactor = _relaxationFactor;
}
const btTransform& getAFrame() { return m_rbAFrame; };
const btTransform& getBFrame() { return m_rbBFrame; };
const btTransform& getAFrame() const { return m_rbAFrame; };
const btTransform& getBFrame() const { return m_rbBFrame; };
inline int getSolveTwistLimit()
{
@@ -239,27 +271,43 @@ public:
void calcAngleInfo();
void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
inline btScalar getSwingSpan1()
inline btScalar getSwingSpan1() const
{
return m_swingSpan1;
}
inline btScalar getSwingSpan2()
inline btScalar getSwingSpan2() const
{
return m_swingSpan2;
}
inline btScalar getTwistSpan()
inline btScalar getTwistSpan() const
{
return m_twistSpan;
}
inline btScalar getTwistAngle()
inline btScalar getLimitSoftness() const
{
return m_limitSoftness;
}
inline btScalar getBiasFactor() const
{
return m_biasFactor;
}
inline btScalar getRelaxationFactor() const
{
return m_relaxationFactor;
}
inline btScalar getTwistAngle() const
{
return m_twistAngle;
}
bool isPastSwingLimit() { return m_solveSwingLimit; }
btScalar getDamping() const { return m_damping; }
void setDamping(btScalar damping) { m_damping = damping; }
void enableMotor(bool b) { m_bMotorEnabled = b; }
bool isMotorEnabled() const { return m_bMotorEnabled; }
btScalar getMaxMotorImpulse() const { return m_maxMotorImpulse; }
bool isMaxMotorImpulseNormalized() const { return m_bNormalizedMotorStrength; }
void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
@@ -271,6 +319,7 @@ public:
// 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);
const btQuaternion& getMotorTarget() const { return m_qTarget; }
// same as above, but q is the desired rotation of frameA wrt frameB in constraint space
void setMotorTargetInConstraintSpace(const btQuaternion &q);
@@ -297,6 +346,11 @@ public:
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
int getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)

3
extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp vendored
View File

@@ -155,8 +155,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
body2.getLinearVelocity(),
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
btScalar a;
a=jacDiagABInv;
rel_vel = normal.dot(vel);

4
extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h vendored
View File

@@ -89,7 +89,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
m_restingContactRestitutionThreshold = 2;//unused as of 2.81
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag)
m_maxGyroscopicForce = 100.f; ///it is only used for 'explicit' version of gyroscopic force
m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
}
};
@@ -111,7 +111,7 @@ struct btContactSolverInfoDoubleData
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_maxGyroscopicForce;
double m_maxGyroscopicForce;///it is only used for 'explicit' version of gyroscopic force
double m_singleAxisRollingFrictionThreshold;
int m_numIterations;

165
extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp vendored
View File

@@ -21,166 +21,17 @@ subject to the following restrictions:
btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
:btGeneric6DofSpring2Constraint(rbA,rbB,frameInA,frameInB)
{
m_frameInA = frameInA;
m_frameInB = frameInB;
setAngularLowerLimit(btVector3(0,0,0));
setAngularUpperLimit(btVector3(0,0,0));
setLinearLowerLimit(btVector3(0,0,0));
setLinearUpperLimit(btVector3(0,0,0));
}
btFixedConstraint::~btFixedConstraint ()
{
}
void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
{
info->m_numConstraintRows = 6;
info->nub = 0;
}
void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
{
//fix the 3 linear degrees of freedom
const btTransform& transA = m_rbA.getCenterOfMassTransform();
const btTransform& transB = m_rbB.getCenterOfMassTransform();
const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
btVector3 a1 = worldOrnA * m_frameInA.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);
}
if (info->m_J2linearAxis)
{
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[info->rowskip+1] = -1;
info->m_J2linearAxis[2*info->rowskip+2] = -1;
}
btVector3 a2 = worldOrnB*m_frameInB.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 for the linear dofs
btScalar k = info->fps * info->erp;
btVector3 linearError = k*(a2+worldPosB-a1-worldPosA);
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = linearError[j];
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0);
btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1);
btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2);
btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0);
btVector3 target;
btScalar x = ivB.dot(ivA);
btScalar y = ivB.dot(jvA);
btScalar z = ivB.dot(kvA);
btVector3 swingAxis(0,0,0);
{
if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
{
swingAxis = -ivB.cross(ivA);
}
}
btVector3 vTwist(1,0,0);
// compute rotation of A wrt B (in constraint space)
btQuaternion qA = transA.getRotation() * m_frameInA.getRotation();
btQuaternion qB = transB.getRotation() * m_frameInB.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();
int row = 3;
int srow = row * info->rowskip;
btVector3 ax1;
// angular limits
{
btScalar *J1 = info->m_J1angularAxis;
btScalar *J2 = info->m_J2angularAxis;
btTransform trA = transA*m_frameInA;
btVector3 twistAxis = trA.getBasis().getColumn(0);
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;
info->m_constraintError[srow] = fact * swingAxis.dot(p);
info->m_constraintError[srow1] = fact * 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;
{
btQuaternion qMinTwist = qABTwist;
btScalar twistAngle = qABTwist.getAngle();
if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qMinTwist = -(qABTwist);
twistAngle = qMinTwist.getAngle();
}
if (twistAngle > SIMD_EPSILON)
{
twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
twistAxis.normalize();
twistAxis = quatRotate(qB, -twistAxis);
}
ax1 = twistAxis;
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;
info->m_constraintError[srow] = k * twistAngle;
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
}
}

23
extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h vendored
View File

@@ -16,33 +16,18 @@ subject to the following restrictions:
#ifndef BT_FIXED_CONSTRAINT_H
#define BT_FIXED_CONSTRAINT_H
#include "btTypedConstraint.h"
#include "btGeneric6DofSpring2Constraint.h"
ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btGeneric6DofSpring2Constraint
{
btTransform m_frameInA;
btTransform m_frameInB;
public:
btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
virtual ~btFixedConstraint();
virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info);
virtual void setParam(int num, btScalar value, int axis = -1)
{
btAssert(0);
}
virtual btScalar getParam(int num, int axis = -1) const
{
btAssert(0);
return 0.f;
}
};
#endif //BT_FIXED_CONSTRAINT_H

27
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h vendored
View File

@@ -111,14 +111,14 @@ public:
//! Is limited
bool isLimited()
bool isLimited() const
{
if(m_loLimit > m_hiLimit) return false;
return true;
}
//! Need apply correction
bool needApplyTorques()
bool needApplyTorques() const
{
if(m_currentLimit == 0 && m_enableMotor == false) return false;
return true;
@@ -207,11 +207,11 @@ public:
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
inline bool isLimited(int limitIndex)
inline bool isLimited(int limitIndex) const
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
inline bool needApplyForce(int limitIndex)
inline bool needApplyForce(int limitIndex) const
{
if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
return true;
@@ -457,7 +457,7 @@ public:
m_linearLimits.m_lowerLimit = linearLower;
}
void getLinearLowerLimit(btVector3& linearLower)
void getLinearLowerLimit(btVector3& linearLower) const
{
linearLower = m_linearLimits.m_lowerLimit;
}
@@ -467,7 +467,7 @@ public:
m_linearLimits.m_upperLimit = linearUpper;
}
void getLinearUpperLimit(btVector3& linearUpper)
void getLinearUpperLimit(btVector3& linearUpper) const
{
linearUpper = m_linearLimits.m_upperLimit;
}
@@ -478,7 +478,7 @@ public:
m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
}
void getAngularLowerLimit(btVector3& angularLower)
void getAngularLowerLimit(btVector3& angularLower) const
{
for(int i = 0; i < 3; i++)
angularLower[i] = m_angularLimits[i].m_loLimit;
@@ -490,7 +490,7 @@ public:
m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
}
void getAngularUpperLimit(btVector3& angularUpper)
void getAngularUpperLimit(btVector3& angularUpper) const
{
for(int i = 0; i < 3; i++)
angularUpper[i] = m_angularLimits[i].m_hiLimit;
@@ -532,7 +532,7 @@ public:
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
bool isLimited(int limitIndex)
bool isLimited(int limitIndex) const
{
if(limitIndex<3)
{
@@ -549,8 +549,11 @@ public:
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
// access for UseFrameOffset
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
bool getUseFrameOffset() const { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
bool getUseLinearReferenceFrameA() const { return m_useLinearReferenceFrameA; }
void setUseLinearReferenceFrameA(bool linearReferenceFrameA) { m_useLinearReferenceFrameA = linearReferenceFrameA; }
///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.
@@ -560,6 +563,10 @@ public:
void setAxis( const btVector3& axis1, const btVector3& axis2);
virtual int getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;

1121
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

674
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h vendored Normal file
View File

@@ -0,0 +1,674 @@
/*
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.
*/
/*
2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer
Pros:
- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled)
- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring)
- Servo motor functionality
- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision)
- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2)
Cons:
- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation.
- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.)
*/
/// 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 BT_GENERIC_6DOF_CONSTRAINT2_H
#define BT_GENERIC_6DOF_CONSTRAINT2_H
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
#include "btTypedConstraint.h"
class btRigidBody;
#ifdef BT_USE_DOUBLE_PRECISION
#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2
#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2"
#else
#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData
#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
enum RotateOrder
{
RO_XYZ=0,
RO_XZY,
RO_YXZ,
RO_YZX,
RO_ZXY,
RO_ZYX
};
class btRotationalLimitMotor2
{
public:
// upper < lower means free
// upper == lower means locked
// upper > lower means limited
btScalar m_loLimit;
btScalar m_hiLimit;
btScalar m_bounce;
btScalar m_stopERP;
btScalar m_stopCFM;
btScalar m_motorERP;
btScalar m_motorCFM;
bool m_enableMotor;
btScalar m_targetVelocity;
btScalar m_maxMotorForce;
bool m_servoMotor;
btScalar m_servoTarget;
bool m_enableSpring;
btScalar m_springStiffness;
bool m_springStiffnessLimited;
btScalar m_springDamping;
bool m_springDampingLimited;
btScalar m_equilibriumPoint;
btScalar m_currentLimitError;
btScalar m_currentLimitErrorHi;
btScalar m_currentPosition;
int m_currentLimit;
btRotationalLimitMotor2()
{
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
m_bounce = 0.0f;
m_stopERP = 0.2f;
m_stopCFM = 0.f;
m_motorERP = 0.9f;
m_motorCFM = 0.f;
m_enableMotor = false;
m_targetVelocity = 0;
m_maxMotorForce = 0.1f;
m_servoMotor = false;
m_servoTarget = 0;
m_enableSpring = false;
m_springStiffness = 0;
m_springStiffnessLimited = false;
m_springDamping = 0;
m_springDampingLimited = false;
m_equilibriumPoint = 0;
m_currentLimitError = 0;
m_currentLimitErrorHi = 0;
m_currentPosition = 0;
m_currentLimit = 0;
}
btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot)
{
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
m_bounce = limot.m_bounce;
m_stopERP = limot.m_stopERP;
m_stopCFM = limot.m_stopCFM;
m_motorERP = limot.m_motorERP;
m_motorCFM = limot.m_motorCFM;
m_enableMotor = limot.m_enableMotor;
m_targetVelocity = limot.m_targetVelocity;
m_maxMotorForce = limot.m_maxMotorForce;
m_servoMotor = limot.m_servoMotor;
m_servoTarget = limot.m_servoTarget;
m_enableSpring = limot.m_enableSpring;
m_springStiffness = limot.m_springStiffness;
m_springStiffnessLimited = limot.m_springStiffnessLimited;
m_springDamping = limot.m_springDamping;
m_springDampingLimited = limot.m_springDampingLimited;
m_equilibriumPoint = limot.m_equilibriumPoint;
m_currentLimitError = limot.m_currentLimitError;
m_currentLimitErrorHi = limot.m_currentLimitErrorHi;
m_currentPosition = limot.m_currentPosition;
m_currentLimit = limot.m_currentLimit;
}
bool isLimited()
{
if(m_loLimit > m_hiLimit) return false;
return true;
}
void testLimitValue(btScalar test_value);
};
class btTranslationalLimitMotor2
{
public:
// upper < lower means free
// upper == lower means locked
// upper > lower means limited
btVector3 m_lowerLimit;
btVector3 m_upperLimit;
btVector3 m_bounce;
btVector3 m_stopERP;
btVector3 m_stopCFM;
btVector3 m_motorERP;
btVector3 m_motorCFM;
bool m_enableMotor[3];
bool m_servoMotor[3];
bool m_enableSpring[3];
btVector3 m_servoTarget;
btVector3 m_springStiffness;
bool m_springStiffnessLimited[3];
btVector3 m_springDamping;
bool m_springDampingLimited[3];
btVector3 m_equilibriumPoint;
btVector3 m_targetVelocity;
btVector3 m_maxMotorForce;
btVector3 m_currentLimitError;
btVector3 m_currentLimitErrorHi;
btVector3 m_currentLinearDiff;
int m_currentLimit[3];
btTranslationalLimitMotor2()
{
m_lowerLimit .setValue(0.f , 0.f , 0.f );
m_upperLimit .setValue(0.f , 0.f , 0.f );
m_bounce .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_motorERP .setValue(0.9f, 0.9f, 0.9f);
m_motorCFM .setValue(0.f , 0.f , 0.f );
m_currentLimitError .setValue(0.f , 0.f , 0.f );
m_currentLimitErrorHi.setValue(0.f , 0.f , 0.f );
m_currentLinearDiff .setValue(0.f , 0.f , 0.f );
for(int i=0; i < 3; i++)
{
m_enableMotor[i] = false;
m_servoMotor[i] = false;
m_enableSpring[i] = false;
m_servoTarget[i] = btScalar(0.f);
m_springStiffness[i] = btScalar(0.f);
m_springStiffnessLimited[i] = false;
m_springDamping[i] = btScalar(0.f);
m_springDampingLimited[i] = false;
m_equilibriumPoint[i] = btScalar(0.f);
m_targetVelocity[i] = btScalar(0.f);
m_maxMotorForce[i] = btScalar(0.f);
m_currentLimit[i] = 0;
}
}
btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other )
{
m_lowerLimit = other.m_lowerLimit;
m_upperLimit = other.m_upperLimit;
m_bounce = other.m_bounce;
m_stopERP = other.m_stopERP;
m_stopCFM = other.m_stopCFM;
m_motorERP = other.m_motorERP;
m_motorCFM = other.m_motorCFM;
m_currentLimitError = other.m_currentLimitError;
m_currentLimitErrorHi = other.m_currentLimitErrorHi;
m_currentLinearDiff = other.m_currentLinearDiff;
for(int i=0; i < 3; i++)
{
m_enableMotor[i] = other.m_enableMotor[i];
m_servoMotor[i] = other.m_servoMotor[i];
m_enableSpring[i] = other.m_enableSpring[i];
m_servoTarget[i] = other.m_servoTarget[i];
m_springStiffness[i] = other.m_springStiffness[i];
m_springStiffnessLimited[i] = other.m_springStiffnessLimited[i];
m_springDamping[i] = other.m_springDamping[i];
m_springDampingLimited[i] = other.m_springDampingLimited[i];
m_equilibriumPoint[i] = other.m_equilibriumPoint[i];
m_targetVelocity[i] = other.m_targetVelocity[i];
m_maxMotorForce[i] = other.m_maxMotorForce[i];
m_currentLimit[i] = other.m_currentLimit[i];
}
}
inline bool isLimited(int limitIndex)
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
void testLimitValue(int limitIndex, btScalar test_value);
};
enum bt6DofFlags2
{
BT_6DOF_FLAGS_CFM_STOP2 = 1,
BT_6DOF_FLAGS_ERP_STOP2 = 2,
BT_6DOF_FLAGS_CFM_MOTO2 = 4,
BT_6DOF_FLAGS_ERP_MOTO2 = 8
};
#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis
ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint
{
protected:
btTransform m_frameInA;
btTransform m_frameInB;
btJacobianEntry m_jacLinear[3];
btJacobianEntry m_jacAng[3];
btTranslationalLimitMotor2 m_linearLimits;
btRotationalLimitMotor2 m_angularLimits[3];
RotateOrder m_rotateOrder;
protected:
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;
int m_flags;
btGeneric6DofSpring2Constraint& operator=(btGeneric6DofSpring2Constraint&)
{
btAssert(0);
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 calculateLinearInfo();
void calculateAngleInfo();
void testAngularLimitMotor(int axis_index);
void calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed);
int get_limit_motor_info2(btRotationalLimitMotor2* 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);
static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
virtual void buildJacobian() {}
virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info);
virtual int calculateSerializeBufferSize() const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
btRotationalLimitMotor2* getRotationalLimitMotor(int index) { return &m_angularLimits[index]; }
btTranslationalLimitMotor2* getTranslationalLimitMotor() { return &m_linearLimits; }
// Calculates the global transform for the joint offset for body A an B, and also calculates the angle differences between the bodies.
void calculateTransforms(const btTransform& transA,const btTransform& transB);
void calculateTransforms();
// Gets the global transform of the offset for body A
const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
// Gets the global transform of the offset for body B
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; }
// Get the rotation axis in global coordinates ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
btVector3 getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; }
// Get the relative Euler angle ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
btScalar getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; }
// Get the relative position of the constraint pivot ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
btScalar getRelativePivotPosition(int axis_index) const { return m_calculatedLinearDiff[axis_index]; }
void setFrames(const btTransform & frameA, const btTransform & frameB);
void setLinearLowerLimit(const btVector3& linearLower) { m_linearLimits.m_lowerLimit = linearLower; }
void getLinearLowerLimit(btVector3& linearLower) { linearLower = m_linearLimits.m_lowerLimit; }
void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; }
void getLinearUpperLimit(btVector3& linearUpper) { linearUpper = m_linearLimits.m_upperLimit; }
void setAngularLowerLimit(const btVector3& angularLower)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
}
void setAngularLowerLimitReversed(const btVector3& angularLower)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]);
}
void getAngularLowerLimit(btVector3& angularLower)
{
for(int i = 0; i < 3; i++)
angularLower[i] = m_angularLimits[i].m_loLimit;
}
void getAngularLowerLimitReversed(btVector3& angularLower)
{
for(int i = 0; i < 3; i++)
angularLower[i] = -m_angularLimits[i].m_hiLimit;
}
void setAngularUpperLimit(const btVector3& angularUpper)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
}
void setAngularUpperLimitReversed(const btVector3& angularUpper)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]);
}
void getAngularUpperLimit(btVector3& angularUpper)
{
for(int i = 0; i < 3; i++)
angularUpper[i] = m_angularLimits[i].m_hiLimit;
}
void getAngularUpperLimitReversed(btVector3& angularUpper)
{
for(int i = 0; i < 3; i++)
angularUpper[i] = -m_angularLimits[i].m_loLimit;
}
//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;
}
}
void setLimitReversed(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_hiLimit = -lo;
m_angularLimits[axis-3].m_loLimit = -hi;
}
}
bool isLimited(int limitIndex)
{
if(limitIndex<3)
{
return m_linearLimits.isLimited(limitIndex);
}
return m_angularLimits[limitIndex-3].isLimited();
}
void setRotationOrder(RotateOrder order) { m_rotateOrder = order; }
RotateOrder getRotationOrder() { return m_rotateOrder; }
void setAxis( const btVector3& axis1, const btVector3& axis2);
void setBounce(int index, btScalar bounce);
void enableMotor(int index, bool onOff);
void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also)
void setTargetVelocity(int index, btScalar velocity);
void setServoTarget(int index, btScalar target);
void setMaxMotorForce(int index, btScalar force);
void enableSpring(int index, bool onOff);
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
void setDamping(int index, btScalar damping, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF
void setEquilibriumPoint(int index, btScalar val);
//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
//If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
virtual btScalar getParam(int num, int axis = -1) const;
};
struct btGeneric6DofSpring2ConstraintData
{
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame;
btTransformFloatData m_rbBFrame;
btVector3FloatData m_linearUpperLimit;
btVector3FloatData m_linearLowerLimit;
btVector3FloatData m_linearBounce;
btVector3FloatData m_linearStopERP;
btVector3FloatData m_linearStopCFM;
btVector3FloatData m_linearMotorERP;
btVector3FloatData m_linearMotorCFM;
btVector3FloatData m_linearTargetVelocity;
btVector3FloatData m_linearMaxMotorForce;
btVector3FloatData m_linearServoTarget;
btVector3FloatData m_linearSpringStiffness;
btVector3FloatData m_linearSpringDamping;
btVector3FloatData m_linearEquilibriumPoint;
char m_linearEnableMotor[4];
char m_linearServoMotor[4];
char m_linearEnableSpring[4];
char m_linearSpringStiffnessLimited[4];
char m_linearSpringDampingLimited[4];
char m_padding1[4];
btVector3FloatData m_angularUpperLimit;
btVector3FloatData m_angularLowerLimit;
btVector3FloatData m_angularBounce;
btVector3FloatData m_angularStopERP;
btVector3FloatData m_angularStopCFM;
btVector3FloatData m_angularMotorERP;
btVector3FloatData m_angularMotorCFM;
btVector3FloatData m_angularTargetVelocity;
btVector3FloatData m_angularMaxMotorForce;
btVector3FloatData m_angularServoTarget;
btVector3FloatData m_angularSpringStiffness;
btVector3FloatData m_angularSpringDamping;
btVector3FloatData m_angularEquilibriumPoint;
char m_angularEnableMotor[4];
char m_angularServoMotor[4];
char m_angularEnableSpring[4];
char m_angularSpringStiffnessLimited[4];
char m_angularSpringDampingLimited[4];
int m_rotateOrder;
};
struct btGeneric6DofSpring2ConstraintDoubleData2
{
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame;
btTransformDoubleData m_rbBFrame;
btVector3DoubleData m_linearUpperLimit;
btVector3DoubleData m_linearLowerLimit;
btVector3DoubleData m_linearBounce;
btVector3DoubleData m_linearStopERP;
btVector3DoubleData m_linearStopCFM;
btVector3DoubleData m_linearMotorERP;
btVector3DoubleData m_linearMotorCFM;
btVector3DoubleData m_linearTargetVelocity;
btVector3DoubleData m_linearMaxMotorForce;
btVector3DoubleData m_linearServoTarget;
btVector3DoubleData m_linearSpringStiffness;
btVector3DoubleData m_linearSpringDamping;
btVector3DoubleData m_linearEquilibriumPoint;
char m_linearEnableMotor[4];
char m_linearServoMotor[4];
char m_linearEnableSpring[4];
char m_linearSpringStiffnessLimited[4];
char m_linearSpringDampingLimited[4];
char m_padding1[4];
btVector3DoubleData m_angularUpperLimit;
btVector3DoubleData m_angularLowerLimit;
btVector3DoubleData m_angularBounce;
btVector3DoubleData m_angularStopERP;
btVector3DoubleData m_angularStopCFM;
btVector3DoubleData m_angularMotorERP;
btVector3DoubleData m_angularMotorCFM;
btVector3DoubleData m_angularTargetVelocity;
btVector3DoubleData m_angularMaxMotorForce;
btVector3DoubleData m_angularServoTarget;
btVector3DoubleData m_angularSpringStiffness;
btVector3DoubleData m_angularSpringDamping;
btVector3DoubleData m_angularEquilibriumPoint;
char m_angularEnableMotor[4];
char m_angularServoMotor[4];
char m_angularEnableSpring[4];
char m_angularSpringStiffnessLimited[4];
char m_angularSpringDampingLimited[4];
int m_rotateOrder;
};
SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const
{
return sizeof(btGeneric6DofSpring2ConstraintData2);
}
SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer;
btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
m_frameInA.serialize(dof->m_rbAFrame);
m_frameInB.serialize(dof->m_rbBFrame);
int i;
for (i=0;i<3;i++)
{
dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
dof->m_angularBounce.m_floats[i] = m_angularLimits[i].m_bounce;
dof->m_angularStopERP.m_floats[i] = m_angularLimits[i].m_stopERP;
dof->m_angularStopCFM.m_floats[i] = m_angularLimits[i].m_stopCFM;
dof->m_angularMotorERP.m_floats[i] = m_angularLimits[i].m_motorERP;
dof->m_angularMotorCFM.m_floats[i] = m_angularLimits[i].m_motorCFM;
dof->m_angularTargetVelocity.m_floats[i] = m_angularLimits[i].m_targetVelocity;
dof->m_angularMaxMotorForce.m_floats[i] = m_angularLimits[i].m_maxMotorForce;
dof->m_angularServoTarget.m_floats[i] = m_angularLimits[i].m_servoTarget;
dof->m_angularSpringStiffness.m_floats[i] = m_angularLimits[i].m_springStiffness;
dof->m_angularSpringDamping.m_floats[i] = m_angularLimits[i].m_springDamping;
dof->m_angularEquilibriumPoint.m_floats[i] = m_angularLimits[i].m_equilibriumPoint;
}
dof->m_angularLowerLimit.m_floats[3] = 0;
dof->m_angularUpperLimit.m_floats[3] = 0;
dof->m_angularBounce.m_floats[3] = 0;
dof->m_angularStopERP.m_floats[3] = 0;
dof->m_angularStopCFM.m_floats[3] = 0;
dof->m_angularMotorERP.m_floats[3] = 0;
dof->m_angularMotorCFM.m_floats[3] = 0;
dof->m_angularTargetVelocity.m_floats[3] = 0;
dof->m_angularMaxMotorForce.m_floats[3] = 0;
dof->m_angularServoTarget.m_floats[3] = 0;
dof->m_angularSpringStiffness.m_floats[3] = 0;
dof->m_angularSpringDamping.m_floats[3] = 0;
dof->m_angularEquilibriumPoint.m_floats[3] = 0;
for (i=0;i<4;i++)
{
dof->m_angularEnableMotor[i] = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0;
dof->m_angularServoMotor[i] = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0;
dof->m_angularEnableSpring[i] = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0;
dof->m_angularSpringStiffnessLimited[i] = i < 3 ? ( m_angularLimits[i].m_springStiffnessLimited ? 1 : 0 ) : 0;
dof->m_angularSpringDampingLimited[i] = i < 3 ? ( m_angularLimits[i].m_springDampingLimited ? 1 : 0 ) : 0;
}
m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit );
m_linearLimits.m_upperLimit.serialize( dof->m_linearUpperLimit );
m_linearLimits.m_bounce.serialize( dof->m_linearBounce );
m_linearLimits.m_stopERP.serialize( dof->m_linearStopERP );
m_linearLimits.m_stopCFM.serialize( dof->m_linearStopCFM );
m_linearLimits.m_motorERP.serialize( dof->m_linearMotorERP );
m_linearLimits.m_motorCFM.serialize( dof->m_linearMotorCFM );
m_linearLimits.m_targetVelocity.serialize( dof->m_linearTargetVelocity );
m_linearLimits.m_maxMotorForce.serialize( dof->m_linearMaxMotorForce );
m_linearLimits.m_servoTarget.serialize( dof->m_linearServoTarget );
m_linearLimits.m_springStiffness.serialize( dof->m_linearSpringStiffness );
m_linearLimits.m_springDamping.serialize( dof->m_linearSpringDamping );
m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint );
for (i=0;i<4;i++)
{
dof->m_linearEnableMotor[i] = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0;
dof->m_linearServoMotor[i] = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0;
dof->m_linearEnableSpring[i] = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0;
dof->m_linearSpringStiffnessLimited[i] = i < 3 ? ( m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0 ) : 0;
dof->m_linearSpringDampingLimited[i] = i < 3 ? ( m_linearLimits.m_springDampingLimited[i] ? 1 : 0 ) : 0;
}
dof->m_rotateOrder = m_rotateOrder;
return btGeneric6DofSpring2ConstraintDataName;
}
#endif //BT_GENERIC_6DOF_CONSTRAINT_H

20
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h vendored
View File

@@ -63,6 +63,26 @@ public:
void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF
void setEquilibriumPoint(int index, btScalar val);
bool isSpringEnabled(int index) const
{
return m_springEnabled[index];
}
btScalar getStiffness(int index) const
{
return m_springStiffness[index];
}
btScalar getDamping(int index) const
{
return m_springDamping[index];
}
btScalar getEquilibriumPoint(int index) const
{
return m_equilibriumPoint[index];
}
virtual void setAxis( const btVector3& axis1, const btVector3& axis2);
virtual void getInfo2 (btConstraintInfo2* info);

4
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp vendored
View File

@@ -25,7 +25,7 @@ subject to the following restrictions:
// 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),
: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(),RO_XYZ),
m_anchor(anchor),
m_axis1(axis1),
m_axis2(axis2)
@@ -59,7 +59,7 @@ btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVec
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 :-)
setStiffness(2, SIMD_PI * SIMD_PI * 4.f);
setDamping(2, 0.01f);
setEquilibriumPoint();
}

4
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h vendored
View File

@@ -20,7 +20,7 @@ subject to the following restrictions:
#include "LinearMath/btVector3.h"
#include "btTypedConstraint.h"
#include "btGeneric6DofSpringConstraint.h"
#include "btGeneric6DofSpring2Constraint.h"
@@ -29,7 +29,7 @@ subject to the following restrictions:
// 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2)
// 1 translational (along axis Z) with suspension spring
ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpringConstraint
ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpring2Constraint
{
protected:
btVector3 m_anchor;

100
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp vendored
View File

@@ -45,7 +45,11 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{
m_rbAFrame.getOrigin() = pivotInA;
@@ -101,7 +105,11 @@ 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_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{
// since no frame is given, assume this to be zero angle and just pick rb transform axis
@@ -151,7 +159,11 @@ m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
@@ -177,7 +189,11 @@ m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{
///not providing rigidbody B means implicitly using worldspace for body B
@@ -285,8 +301,60 @@ void btHingeConstraint::buildJacobian()
#endif //__SPU__
static inline btScalar btNormalizeAnglePositive(btScalar angle)
{
return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI));
}
static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
{
btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
btNormalizeAnglePositive(accAngle)));
return result;
}
static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
{
btScalar tol(0.3);
btScalar result = btShortestAngularDistance(accAngle, curAngle);
if (btFabs(result) > tol)
return curAngle;
else
return accAngle + result;
return curAngle;
}
btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
{
btScalar hingeAngle = getHingeAngle();
m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle);
return m_accumulatedAngle;
}
void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
{
m_accumulatedAngle = accAngle;
}
void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
{
//update m_accumulatedAngle
btScalar curHingeAngle = getHingeAngle();
m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle);
btHingeConstraint::getInfo1(info);
}
void btHingeConstraint::getInfo1(btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
@@ -413,7 +481,9 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// linear RHS
btScalar k = info->fps * info->erp;
btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
btScalar k = info->fps * normalErp;
if (!m_angularOnly)
{
for(i = 0; i < 3; i++)
@@ -510,7 +580,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
powered = 0;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
if(powered)
{
if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
@@ -606,6 +676,8 @@ void btHingeConstraint::updateRHS(btScalar timeStep)
}
btScalar btHingeConstraint::getHingeAngle()
{
return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
@@ -798,7 +870,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
btScalar k = info->fps * info->erp;
btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp;
btScalar k = info->fps * normalErp;
if (!m_angularOnly)
{
@@ -856,7 +929,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
// 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;
k = info->fps * normalErp;//??
btVector3 u = ax1A.cross(ax1B);
info->m_constraintError[s3] = k * u.dot(p);
info->m_constraintError[s4] = k * u.dot(q);
@@ -901,7 +975,7 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
powered = 0;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
if(powered)
{
if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
@@ -1002,6 +1076,10 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis)
m_normalCFM = value;
m_flags |= BT_HINGE_FLAGS_CFM_NORM;
break;
case BT_CONSTRAINT_ERP:
m_normalERP = value;
m_flags |= BT_HINGE_FLAGS_ERP_NORM;
break;
default :
btAssertConstrParams(0);
}
@@ -1032,6 +1110,10 @@ btScalar btHingeConstraint::getParam(int num, int axis) const
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
retVal = m_normalCFM;
break;
case BT_CONSTRAINT_ERP:
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM);
retVal = m_normalERP;
break;
default :
btAssertConstrParams(0);
}

87
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h vendored
View File

@@ -41,7 +41,8 @@ enum btHingeFlags
{
BT_HINGE_FLAGS_CFM_STOP = 1,
BT_HINGE_FLAGS_ERP_STOP = 2,
BT_HINGE_FLAGS_CFM_NORM = 4
BT_HINGE_FLAGS_CFM_NORM = 4,
BT_HINGE_FLAGS_ERP_NORM = 8
};
@@ -94,6 +95,7 @@ public:
int m_flags;
btScalar m_normalCFM;
btScalar m_normalERP;
btScalar m_stopCFM;
btScalar m_stopERP;
@@ -175,6 +177,7 @@ public:
// maintain a given angular target.
void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; }
void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
void setMotorTargetVelocity(btScalar motorTargetVelocity) { m_motorTargetVelocity = motorTargetVelocity; }
void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
void setMotorTarget(btScalar targetAngle, btScalar dt);
@@ -191,6 +194,33 @@ public:
m_relaxationFactor = _relaxationFactor;
#endif
}
btScalar getLimitSoftness() const
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getSoftness();
#else
return m_limitSoftness;
#endif
}
btScalar getLimitBiasFactor() const
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getBiasFactor();
#else
return m_biasFactor;
#endif
}
btScalar getLimitRelaxationFactor() const
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getRelaxationFactor();
#else
return m_relaxationFactor;
#endif
}
void setAxis(btVector3& axisInA)
{
@@ -217,6 +247,14 @@ public:
}
bool hasLimit() const {
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getHalfRange() > 0;
#else
return m_lowerLimit <= m_upperLimit;
#endif
}
btScalar getLowerLimit() const
{
#ifdef _BT_USE_CENTER_LIMIT_
@@ -236,6 +274,7 @@ public:
}
///The getHingeAngle gives the hinge angle in range [-PI,PI]
btScalar getHingeAngle();
btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
@@ -286,13 +325,20 @@ public:
// access for UseFrameOffset
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
// access for UseReferenceFrameA
bool getUseReferenceFrameA() const { return m_useReferenceFrameA; }
void setUseReferenceFrameA(bool useReferenceFrameA) { m_useReferenceFrameA = useReferenceFrameA; }
///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 getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;
@@ -326,6 +372,43 @@ struct btHingeConstraintDoubleData
};
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account
ATTRIBUTE_ALIGNED16(class) btHingeAccumulatedAngleConstraint : public btHingeConstraint
{
protected:
btScalar m_accumulatedAngle;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,rbB,pivotInA,pivotInB, axisInA,axisInB, useReferenceFrameA )
{
m_accumulatedAngle=getHingeAngle();
}
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA)
{
m_accumulatedAngle=getHingeAngle();
}
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA )
{
m_accumulatedAngle=getHingeAngle();
}
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,rbAFrame, useReferenceFrameA )
{
m_accumulatedAngle=getHingeAngle();
}
btScalar getAccumulatedHingeAngle();
void setAccumulatedHingeAngle(btScalar accAngle);
virtual void getInfo1 (btConstraintInfo1* info);
};
struct btHingeConstraintFloatData
{

463
extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp vendored Normal file
View File

@@ -0,0 +1,463 @@
/*
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 "btNNCGConstraintSolver.h"
btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
return val;
}
btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
{
int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
{
if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
{
for (int j=0; j<numNonContactPool; ++j) {
int tmp = m_orderNonContactConstraintPool[j];
int swapi = btRandInt2(j+1);
m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
m_orderNonContactConstraintPool[swapi] = tmp;
}
//contact/friction constraints are not solved more than
if (iteration< infoGlobal.m_numIterations)
{
for (int 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 (int 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;
}
}
}
}
btScalar deltaflengthsqr = 0;
if (infoGlobal.m_solverMode & SOLVER_SIMD)
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
{
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
m_deltafNC[j] = deltaf;
deltaflengthsqr += deltaf * deltaf;
}
}
} else
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
{
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
m_deltafNC[j] = deltaf;
deltaflengthsqr += deltaf * deltaf;
}
}
}
if (m_onlyForNoneContact)
{
if (iteration==0)
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
} else {
// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
if (beta>1)
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
} else
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
{
btScalar additionaldeltaimpulse = beta * m_pNC[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
}
}
m_deltafLengthSqrPrev = deltaflengthsqr;
}
if (infoGlobal.m_solverMode & SOLVER_SIMD)
{
if (iteration< infoGlobal.m_numIterations)
{
for (int j=0;j<numConstraints;j++)
{
if (constraints[j]->isEnabled())
{
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
}
///solve all contact constraints using SIMD, if available
if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
for (int c=0;c<numPoolConstraints;c++)
{
btScalar totalImpulse =0;
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafC[c] = deltaf;
deltaflengthsqr += deltaf*deltaf;
totalImpulse = solveManifold.m_appliedImpulse;
}
bool applyFriction = true;
if (applyFriction)
{
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[c*multiplier] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[c*multiplier] = 0;
}
}
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[c*multiplier+1] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[c*multiplier+1] = 0;
}
}
}
}
}
else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
{
//solve the friction constraints after all contact constraints, don't interleave them
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafC[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
}
///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(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[j] = 0;
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (j=0;j<numRollingFrictionPoolConstraints;j++)
{
btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
m_deltafCRF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCRF[j] = 0;
}
}
}
}
} else
{
if (iteration< infoGlobal.m_numIterations)
{
for (int j=0;j<numConstraints;j++)
{
if (constraints[j]->isEnabled())
{
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
}
///solve all contact constraints
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
for (int j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafC[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
}
///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (int 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;
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[j] = 0;
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (int j=0;j<numRollingFrictionPoolConstraints;j++)
{
btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
m_deltafCRF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCRF[j] = 0;
}
}
}
}
if (!m_onlyForNoneContact)
{
if (iteration==0)
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 )
{
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
}
} else
{
// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
if (beta>1) {
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
}
} else {
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations) {
btScalar additionaldeltaimpulse = beta * m_pNC[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
if (iteration< infoGlobal.m_numIterations) {
btScalar additionaldeltaimpulse = beta * m_pC[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pC[j] = beta * m_pC[j] + m_deltafC[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
if (iteration< infoGlobal.m_numIterations) {
btScalar additionaldeltaimpulse = beta * m_pCF[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
if (iteration< infoGlobal.m_numIterations) {
btScalar additionaldeltaimpulse = beta * m_pCRF[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
}
}
}
m_deltafLengthSqrPrev = deltaflengthsqr;
}
return deltaflengthsqr;
}
btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
{
m_pNC.resizeNoInitialize(0);
m_pC.resizeNoInitialize(0);
m_pCF.resizeNoInitialize(0);
m_pCRF.resizeNoInitialize(0);
m_deltafNC.resizeNoInitialize(0);
m_deltafC.resizeNoInitialize(0);
m_deltafCF.resizeNoInitialize(0);
m_deltafCRF.resizeNoInitialize(0);
return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
}

64
extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h vendored Normal file
View File

@@ -0,0 +1,64 @@
/*
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_NNCG_CONSTRAINT_SOLVER_H
#define BT_NNCG_CONSTRAINT_SOLVER_H
#include "btSequentialImpulseConstraintSolver.h"
ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
{
protected:
btScalar m_deltafLengthSqrPrev;
btAlignedObjectArray<btScalar> m_pNC; // p for None Contact constraints
btAlignedObjectArray<btScalar> m_pC; // p for Contact constraints
btAlignedObjectArray<btScalar> m_pCF; // p for ContactFriction constraints
btAlignedObjectArray<btScalar> m_pCRF; // p for ContactRollingFriction constraints
//These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration.
btAlignedObjectArray<btScalar> m_deltafNC; // deltaf for NoneContact constraints
btAlignedObjectArray<btScalar> m_deltafC; // deltaf for Contact constraints
btAlignedObjectArray<btScalar> m_deltafCF; // deltaf for ContactFriction constraints
btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints
protected:
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {}
virtual btConstraintSolverType getSolverType() const
{
return BT_NNCG_SOLVER;
}
bool m_onlyForNoneContact;
};
#endif //BT_NNCG_CONSTRAINT_SOLVER_H

5
extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h vendored
View File

@@ -116,6 +116,11 @@ public:
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 getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;

536
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp vendored
View File

@@ -4,8 +4,8 @@ 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,
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.
@@ -22,6 +22,8 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "LinearMath/btIDebugDraw.h"
#include "LinearMath/btCpuFeatureUtility.h"
//#include "btJacobianEntry.h"
#include "LinearMath/btMinMax.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
@@ -37,57 +39,238 @@ int gNumSplitImpulseRecoveries = 0;
#include "BulletDynamics/Dynamics/btRigidBody.h"
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
:m_btSeed2(0)
{
///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver
///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check.
static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
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_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
return deltaImpulse;
}
btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
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_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
return deltaImpulse;
}
#ifdef USE_SIMD
#include <emmintrin.h>
#define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
{
__m128 result = _mm_mul_ps( vec0, vec1);
return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
}
#endif//USE_SIMD
#if defined (BT_ALLOW_SSE4)
#include <intrin.h>
#define USE_FMA 1
#define USE_FMA3_INSTEAD_FMA4 1
#define USE_SSE4_DOT 1
#define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f)
#define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f))
#if USE_SSE4_DOT
#define DOT_PRODUCT(a, b) SSE4_DP(a, b)
#else
#define DOT_PRODUCT(a, b) btSimdDot3(a, b)
#endif
#if USE_FMA
#if USE_FMA3_INSTEAD_FMA4
// a*b + c
#define FMADD(a, b, c) _mm_fmadd_ps(a, b, c)
// -(a*b) + c
#define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c)
#else // USE_FMA3
// a*b + c
#define FMADD(a, b, c) _mm_macc_ps(a, b, c)
// -(a*b) + c
#define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c)
#endif
#else // USE_FMA
// c + a*b
#define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b))
// c - a*b
#define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b))
#endif
#endif
// Project Gauss Seidel or the equivalent Sequential Impulse
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& 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);
btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
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_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128,body2.internalGetInvMass().mVec128);
btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
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_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, body2.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse;
body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
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_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
return deltaImpulse;
}
// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
{
#if defined (BT_ALLOW_SSE4)
__m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
__m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm);
const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit);
const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum
const __m128 maskLower = _mm_cmpgt_ps(tmp, lowerLimit);
const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp);
deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower);
c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower);
body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
return deltaImpulse;
#else
return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c);
#endif
}
static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
{
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
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_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse;
body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
return deltaImpulse;
}
// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
{
#ifdef BT_ALLOW_SSE4
__m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
__m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm);
const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse);
const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit);
deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask);
c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask);
body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
return deltaImpulse;
#else
return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c);
#endif //BT_ALLOW_SSE4
}
#endif //USE_SIMD
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
#ifdef USE_SIMD
return m_resolveSingleConstraintRowGeneric(body1, body2, c);
#else
return resolveSingleConstraintRowGeneric(body1,body2,c);
#endif
@@ -96,62 +279,13 @@ btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGene
// Project Gauss Seidel or the equivalent Sequential Impulse
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
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_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
return deltaImpulse;
return gResolveSingleConstraintRowGeneric_scalar_reference(body1, body2, c);
}
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& 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);
btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
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_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse;
body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
return deltaImpulse;
return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
#else
return resolveSingleConstraintRowLowerLimit(body1,body2,c);
#endif
@@ -160,26 +294,7 @@ btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowe
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
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_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
return deltaImpulse;
return gResolveSingleConstraintRowLowerLimit_scalar_reference(body1,body2,c);
}
@@ -248,6 +363,63 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
}
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
: m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference),
m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference),
m_btSeed2(0)
{
#ifdef USE_SIMD
m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
m_resolveSingleConstraintRowLowerLimit=gResolveSingleConstraintRowLowerLimit_sse2;
#endif //USE_SIMD
#ifdef BT_ALLOW_SSE4
int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1))
{
m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
}
#endif//BT_ALLOW_SSE4
}
btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
{
}
btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()
{
return gResolveSingleConstraintRowGeneric_scalar_reference;
}
btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()
{
return gResolveSingleConstraintRowLowerLimit_scalar_reference;
}
#ifdef USE_SIMD
btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric()
{
return gResolveSingleConstraintRowGeneric_sse2;
}
btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit()
{
return gResolveSingleConstraintRowLowerLimit_sse2;
}
#ifdef BT_ALLOW_SSE4
btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric()
{
return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
}
btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit()
{
return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
}
#endif //BT_ALLOW_SSE4
#endif //USE_SIMD
unsigned long btSequentialImpulseConstraintSolver::btRand2()
{
@@ -308,7 +480,7 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
solverBody->m_angularVelocity = rb->getAngularVelocity();
solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
} else
{
solverBody->m_worldTransform.setIdentity();
@@ -340,7 +512,7 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel,
void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
{
if (colObj && colObj->hasAnisotropicFriction(frictionMode))
{
@@ -361,7 +533,7 @@ void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb
void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
{
btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
@@ -422,26 +594,26 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
}
{
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
rel_vel = vel1Dotn+vel2Dotn;
// btScalar positionalError = 0.f;
btSimdScalar velocityError = desiredVelocity - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
btScalar velocityError = desiredVelocity - rel_vel;
btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
}
}
@@ -449,7 +621,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
{
btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex;
setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
return solverConstraint;
}
@@ -457,7 +629,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity, btScalar cfmSlip)
{
@@ -503,12 +675,12 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv
}
{
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
rel_vel = vel1Dotn+vel2Dotn;
@@ -521,7 +693,7 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
}
}
@@ -536,7 +708,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConst
{
btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex;
setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
return solverConstraint;
}
@@ -564,7 +736,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
body.setCompanionId(solverBodyIdA);
} else
{
if (m_fixedBodyId<0)
{
m_fixedBodyId = m_tmpSolverBodyPool.size();
@@ -582,15 +754,15 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
#include <stdio.h>
void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
const btVector3& rel_pos1, const btVector3& rel_pos2)
{
const btVector3& pos1 = cp.getPositionWorldOnA();
const btVector3& pos2 = cp.getPositionWorldOnB();
// const btVector3& pos1 = cp.getPositionWorldOnA();
// const btVector3& pos2 = cp.getPositionWorldOnB();
btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
@@ -598,23 +770,23 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
btRigidBody* rb0 = bodyA->m_originalBody;
btRigidBody* rb1 = bodyB->m_originalBody;
// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
//rel_pos2 = pos2 - bodyB->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);
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
#else
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
@@ -628,7 +800,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
}
#endif //COMPUTE_IMPULSE_DENOM
#endif //COMPUTE_IMPULSE_DENOM
btScalar denom = relaxation/(denom0+denom1);
solverConstraint.m_jacDiagABInv = denom;
@@ -666,11 +838,11 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
btVector3 vel = vel1 - vel2;
btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
solverConstraint.m_friction = cp.m_combinedFriction;
restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= btScalar(0.))
{
@@ -700,17 +872,17 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0);
btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA)
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA)
+ solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB)
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB)
+ solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
btScalar rel_vel = vel1Dotn+vel2Dotn;
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
@@ -755,7 +927,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
{
@@ -834,7 +1006,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
btVector3 rel_pos1;
btVector3 rel_pos2;
btScalar relaxation;
int frictionIndex = m_tmpSolverContactConstraintPool.size();
btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
@@ -848,7 +1020,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
const btVector3& pos1 = cp.getPositionWorldOnA();
const btVector3& pos2 = cp.getPositionWorldOnB();
rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
@@ -856,13 +1028,13 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
btVector3 vel = vel1 - vel2;
btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
// const btVector3& pos1 = cp.getPositionWorldOnA();
// const btVector3& pos2 = cp.getPositionWorldOnB();
@@ -903,21 +1075,21 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if (axis1.length()>0.001)
addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
}
///Bullet has several options to set the friction directions
///By default, each contact has only a single friction direction that is recomputed automatically very frame
///By default, each contact has only a single friction direction that is recomputed automatically very frame
///based on the relative linear velocity.
///If the relative velocity it zero, it will automatically compute a friction direction.
///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
///
///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
///
///The user can manually override the friction directions for certain contacts using a contact callback,
///The user can manually override the friction directions for certain contacts using a contact callback,
///and set the cp.m_lateralFrictionInitialized to true
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
///this will give a conveyor belt effect
@@ -973,9 +1145,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
}
setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
}
}
@@ -1015,7 +1187,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (&constraint->getRigidBodyA()==bodies[b])
{
found = true;
@@ -1047,7 +1219,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (manifoldPtr[i]->getBody0()==bodies[b])
{
found = true;
@@ -1071,8 +1243,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
}
}
#endif //BT_ADDITIONAL_DEBUG
for (int i = 0; i < numBodies; i++)
{
bodies[i]->setCompanionId(-1);
@@ -1087,6 +1259,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
//convert all bodies
for (int i=0;i<numBodies;i++)
{
int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
@@ -1096,14 +1269,27 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
{
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
btVector3 gyroForce (0,0,0);
if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE)
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
{
gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
}
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
{
gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
solverBody.m_externalTorqueImpulse += gyroForce;
}
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
{
gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
solverBody.m_externalTorqueImpulse += gyroForce;
}
}
}
if (1)
{
int j;
@@ -1123,7 +1309,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
int totalNumRows = 0;
int i;
m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
//calculate the total number of contraint rows
for (i=0;i<numConstraints;i++)
@@ -1153,14 +1339,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
}
m_tmpSolverNonContactConstraintPool.resizeNoInitialize(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);
@@ -1268,7 +1454,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
}
{
btScalar rel_vel;
btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
@@ -1276,11 +1462,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
+ solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
+ solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
rel_vel = vel1Dotn+vel2Dotn;
@@ -1346,7 +1532,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
{
if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
@@ -1359,7 +1545,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
m_orderNonContactConstraintPool[swapi] = tmp;
}
//contact/friction constraints are not solved more than
//contact/friction constraints are not solved more than
if (iteration< infoGlobal.m_numIterations)
{
for (int j=0; j<numConstraintPool; ++j) {
@@ -1438,7 +1624,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
@@ -1463,8 +1649,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
///solve all friction constraints, using SIMD, if available
@@ -1483,7 +1669,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (j=0;j<numRollingFrictionPoolConstraints;j++)
{
@@ -1502,9 +1688,9 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
}
}
}
}
}
} else
{
@@ -1628,10 +1814,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
//for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
{
{
solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
}
}
return 0.f;
}
@@ -1673,7 +1859,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
}
constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
@@ -1694,7 +1880,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
else
m_tmpSolverBodyPool[i].writebackVelocity();
m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
m_tmpSolverBodyPool[i].m_linearVelocity+
m_tmpSolverBodyPool[i].m_externalForceImpulse);
@@ -1727,13 +1913,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
BT_PROFILE("solveGroup");
//you need to provide at least some bodies
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
return 0.f;
}
@@ -1741,5 +1927,3 @@ void btSequentialImpulseConstraintSolver::reset()
{
m_btSeed2 = 0;
}

37
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h vendored
View File

@@ -27,6 +27,8 @@ class btCollisionObject;
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
typedef btSimdScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver
{
@@ -43,6 +45,10 @@ protected:
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
int m_maxOverrideNumSolverIterations;
int m_fixedBodyId;
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
@@ -112,9 +118,7 @@ public:
virtual ~btSequentialImpulseConstraintSolver();
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
///clear internal cached data and reset random seed
virtual void reset();
@@ -136,6 +140,33 @@ public:
{
return BT_SEQUENTIAL_IMPULSE_SOLVER;
}
btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric()
{
return m_resolveSingleConstraintRowGeneric;
}
void setConstraintRowSolverGeneric(btSingleConstraintRowSolver rowSolver)
{
m_resolveSingleConstraintRowGeneric = rowSolver;
}
btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit()
{
return m_resolveSingleConstraintRowLowerLimit;
}
void setConstraintRowSolverLowerLimit(btSingleConstraintRowSolver rowSolver)
{
m_resolveSingleConstraintRowLowerLimit = rowSolver;
}
///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
};

8
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp vendored
View File

@@ -539,8 +539,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
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;
info->m_lowerLimit[srow] += -getMaxLinMotorForce() / info->fps;
info->m_upperLimit[srow] += getMaxLinMotorForce() / info->fps;
}
if(limit)
{
@@ -641,8 +641,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
}
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;
info->m_lowerLimit[srow] = -getMaxAngMotorForce() / info->fps;
info->m_upperLimit[srow] = getMaxAngMotorForce() / info->fps;
}
if(limit)
{

5
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h vendored
View File

@@ -280,6 +280,11 @@ public:
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 getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;

4
extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp vendored
View File

@@ -24,7 +24,7 @@ subject to the following restrictions:
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
:btTypedObject(type),
m_userConstraintType(-1),
m_userConstraintId(-1),
m_userConstraintPtr((void*)-1),
m_breakingImpulseThreshold(SIMD_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),
@@ -41,7 +41,7 @@ m_jointFeedback(0)
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
:btTypedObject(type),
m_userConstraintType(-1),
m_userConstraintId(-1),
m_userConstraintPtr((void*)-1),
m_breakingImpulseThreshold(SIMD_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),

2
extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h vendored
View File

@@ -44,6 +44,7 @@ enum btTypedConstraintType
D6_SPRING_CONSTRAINT_TYPE,
GEAR_CONSTRAINT_TYPE,
FIXED_CONSTRAINT_TYPE,
D6_SPRING_2_CONSTRAINT_TYPE,
MAX_CONSTRAINT_TYPE
};
@@ -65,6 +66,7 @@ enum btConstraintParams
ATTRIBUTE_ALIGNED16(struct) btJointFeedback
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 m_appliedForceBodyA;
btVector3 m_appliedTorqueBodyA;
btVector3 m_appliedForceBodyB;

196
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp vendored
View File

@@ -4,8 +4,8 @@ Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -34,6 +34,7 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
@@ -58,7 +59,7 @@ int firstHit=startHit;
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();
@@ -88,7 +89,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
int m_numConstraints;
btIDebugDraw* m_debugDrawer;
btDispatcher* m_dispatcher;
btAlignedObjectArray<btCollisionObject*> m_bodies;
btAlignedObjectArray<btPersistentManifold*> m_manifolds;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
@@ -127,7 +128,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
m_constraints.resize (0);
}
virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
{
if (islandId<0)
@@ -140,7 +141,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
btTypedConstraint** startConstraint = 0;
int numCurConstraints = 0;
int i;
//find the first constraint for this island
for (i=0;i<m_numConstraints;i++)
{
@@ -164,7 +165,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
} else
{
for (i=0;i<numBodies;i++)
m_bodies.push_back(bodies[i]);
for (i=0;i<numManifolds;i++)
@@ -187,7 +188,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher);
m_bodies.resize(0);
m_manifolds.resize(0);
@@ -206,10 +207,10 @@ m_solverIslandCallback ( NULL ),
m_constraintSolver(constraintSolver),
m_gravity(0,-10,0),
m_localTime(0),
m_fixedTimeStep(0),
m_synchronizeAllMotionStates(false),
m_applySpeculativeContactRestitution(false),
m_profileTimings(0),
m_fixedTimeStep(0),
m_latencyMotionStateInterpolation(true)
{
@@ -317,6 +318,9 @@ void btDiscreteDynamicsWorld::debugDrawWorld()
}
}
}
if (getDebugDrawer())
getDebugDrawer()->flushLines();
}
void btDiscreteDynamicsWorld::clearForces()
@@ -329,7 +333,7 @@ void btDiscreteDynamicsWorld::clearForces()
//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()
@@ -445,7 +449,7 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
applyGravity();
for (int i=0;i<clampedSimulationSteps;i++)
{
@@ -463,18 +467,18 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
#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);
@@ -487,20 +491,20 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
createPredictiveContacts(timeStep);
///perform collision detection
performDiscreteCollisionDetection();
calculateSimulationIslands();
getSolverInfo().m_timeStep = timeStep;
///solve contact and other joint constraints
solveConstraints(getSolverInfo());
///CallbackTriggers();
///integrate transforms
@@ -509,12 +513,12 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
///update vehicle simulation
updateActions(timeStep);
updateActivationState( timeStep );
if(0 != m_internalTickCallback) {
(*m_internalTickCallback)(this, timeStep);
}
}
}
void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
@@ -606,14 +610,14 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short
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");
@@ -634,7 +638,7 @@ void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
{
if (body->getActivationState() == ACTIVE_TAG)
body->setActivationState( WANTS_DEACTIVATION );
if (body->getActivationState() == ISLAND_SLEEPING)
if (body->getActivationState() == ISLAND_SLEEPING)
{
body->setAngularVelocity(btVector3(0,0,0));
body->setLinearVelocity(btVector3(0,0,0));
@@ -653,6 +657,9 @@ void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
{
m_constraints.push_back(constraint);
//Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?)
btAssert(&constraint->getRigidBodyA()!=&constraint->getRigidBodyB());
if (disableCollisionsBetweenLinkedBodies)
{
constraint->getRigidBodyA().addConstraintRef(constraint);
@@ -704,25 +711,25 @@ void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
BT_PROFILE("solveConstraints");
m_sortedConstraints.resize( m_constraints.size());
int i;
int i;
for (i=0;i<getNumConstraints();i++)
{
m_sortedConstraints[i] = m_constraints[i];
}
// btAssert(0);
m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback);
@@ -743,10 +750,10 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands()
for (int i=0;i<this->m_predictiveManifolds.size();i++)
{
btPersistentManifold* manifold = m_predictiveManifolds[i];
const btCollisionObject* colObj0 = manifold->getBody0();
const btCollisionObject* colObj1 = manifold->getBody1();
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
{
@@ -754,7 +761,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands()
}
}
}
{
int i;
int numConstraints = int(m_constraints.size());
@@ -778,7 +785,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands()
//Store the island id in each body
getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
}
@@ -794,7 +801,7 @@ public:
btDispatcher* m_dispatcher;
public:
btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
m_me(me),
m_allowedPenetration(0.0f),
@@ -874,7 +881,7 @@ int gNumClampedCcdMotions=0;
void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
{
BT_PROFILE("createPredictiveContacts");
{
BT_PROFILE("release predictive contact manifolds");
@@ -896,7 +903,7 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
{
body->predictIntegratedTransform(timeStep, predictedTrans);
btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
@@ -910,7 +917,7 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
{
public:
StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
{
}
@@ -940,14 +947,14 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
{
btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction;
btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld);
btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject);
m_predictiveManifolds.push_back(manifold);
btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec;
btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB;
@@ -980,10 +987,10 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
{
body->predictIntegratedTransform(timeStep, predictedTrans);
btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
{
@@ -996,7 +1003,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
{
public:
StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
{
}
@@ -1026,7 +1033,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
{
//printf("clamped integration to hit fraction = %f\n",fraction);
body->setHitFraction(sweepResults.m_closestHitFraction);
body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
@@ -1051,13 +1058,13 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
printf("sm2=%f\n",sm2);
}
#else
//don't apply the collision response right now, it will happen next frame
//if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution.
//btScalar appliedImpulse = 0.f;
//btScalar depth = 0.f;
//appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
#endif
@@ -1065,10 +1072,10 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
}
}
}
body->proceedToTransform( predictedTrans);
}
}
@@ -1082,7 +1089,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
btPersistentManifold* manifold = m_predictiveManifolds[i];
btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0());
btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1());
for (int p=0;p<manifold->getNumContacts();p++)
{
const btManifoldPoint& pt = manifold->getContactPoint(p);
@@ -1092,11 +1099,11 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
//if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
{
btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution;
const btVector3& pos1 = pt.getPositionWorldOnA();
const btVector3& pos2 = pt.getPositionWorldOnB();
btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin();
btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin();
btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();
if (body0)
@@ -1107,7 +1114,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
}
}
}
}
@@ -1146,7 +1153,7 @@ void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{
@@ -1166,12 +1173,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
btTransform tr;
tr.setIdentity();
btVector3 pivot = p2pC->getPivotInA();
pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
tr.setOrigin(pivot);
getDebugDrawer()->drawTransform(tr, dbgDrawSize);
// that ideally should draw the same frame
// that ideally should draw the same frame
pivot = p2pC->getPivotInB();
pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
tr.setOrigin(pivot);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
}
@@ -1190,13 +1197,13 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
break;
}
bool drawSect = true;
if(minAng > maxAng)
if(!pHinge->hasLimit())
{
minAng = btScalar(0.f);
maxAng = SIMD_2_PI;
drawSect = false;
}
if(drawLimits)
if(drawLimits)
{
btVector3& center = tr.getOrigin();
btVector3 normal = tr.getBasis().getColumn(2);
@@ -1231,7 +1238,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
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));
@@ -1259,7 +1266,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = p6DOF->getCalculatedTransformB();
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
if(drawLimits)
if(drawLimits)
{
tr = p6DOF->getCalculatedTransformA();
const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
@@ -1300,6 +1307,57 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
}
}
break;
///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage
case D6_SPRING_2_CONSTRAINT_TYPE:
{
{
btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)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;
@@ -1322,7 +1380,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
}
}
break;
default :
default :
break;
}
return;
@@ -1422,19 +1480,19 @@ void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serialize
worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm;
worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold;
worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp;
worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop;
worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor;
worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce;
worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold;
worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations;
worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode;
worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold;
worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize;
worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
#ifdef BT_USE_DOUBLE_PRECISION
const char* structType = "btDynamicsWorldDoubleData";
#else//BT_USE_DOUBLE_PRECISION
@@ -1450,10 +1508,10 @@ void btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
serializeDynamicsWorldInfo(serializer);
serializeRigidBodies(serializer);
serializeCollisionObjects(serializer);
serializeRigidBodies(serializer);
serializer->finishSerialization();
}

2
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h vendored
View File

@@ -151,7 +151,7 @@ public:
virtual void removeCollisionObject(btCollisionObject* collisionObject);
void debugDrawConstraint(btTypedConstraint* constraint);
virtual void debugDrawConstraint(btTypedConstraint* constraint);
virtual void debugDrawWorld();

174
extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp vendored
View File

@@ -87,7 +87,7 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
updateInertiaTensor();
m_rigidbodyFlags = 0;
m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY;
m_deltaLinearVelocity.setZero();
@@ -257,12 +257,41 @@ void btRigidBody::updateInertiaTensor()
}
btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
btVector3 btRigidBody::getLocalInertia() const
{
btVector3 inertiaLocal;
inertiaLocal[0] = 1.f/getInvInertiaDiagLocal()[0];
inertiaLocal[1] = 1.f/getInvInertiaDiagLocal()[1];
inertiaLocal[2] = 1.f/getInvInertiaDiagLocal()[2];
const btVector3 inertia = m_invInertiaLocal;
inertiaLocal.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));
return inertiaLocal;
}
inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt,
const btMatrix3x3 &I)
{
const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0);
return w2;
}
inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt,
const btMatrix3x3 &I)
{
btMatrix3x3 w1x, Iw1x;
const btVector3 Iwi = (I*w1);
w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]);
Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]);
const btMatrix3x3 dfw1 = I + (w1x*I - Iw1x)*dt;
return dfw1;
}
btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
{
btVector3 inertiaLocal = getLocalInertia();
btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
btVector3 tmp = inertiaTensorWorld*getAngularVelocity();
btVector3 gf = getAngularVelocity().cross(tmp);
@@ -274,6 +303,85 @@ btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
return gf;
}
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const
{
btVector3 idl = getLocalInertia();
btVector3 omega1 = getAngularVelocity();
btQuaternion q = getWorldTransform().getRotation();
// Convert to body coordinates
btVector3 omegab = quatRotate(q.inverse(), omega1);
btMatrix3x3 Ib;
Ib.setValue(idl.x(),0,0,
0,idl.y(),0,
0,0,idl.z());
btVector3 ibo = Ib*omegab;
// Residual vector
btVector3 f = step * omegab.cross(ibo);
btMatrix3x3 skew0;
omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]);
btVector3 om = Ib*omegab;
btMatrix3x3 skew1;
om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]);
// Jacobian
btMatrix3x3 J = Ib + (skew0*Ib - skew1)*step;
// btMatrix3x3 Jinv = J.inverse();
// btVector3 omega_div = Jinv*f;
btVector3 omega_div = J.solve33(f);
// Single Newton-Raphson update
omegab = omegab - omega_div;//Solve33(J, f);
// Back to world coordinates
btVector3 omega2 = quatRotate(q,omegab);
btVector3 gf = omega2-omega1;
return gf;
}
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const
{
// use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior.
// calculate using implicit euler step so it's stable.
const btVector3 inertiaLocal = getLocalInertia();
const btVector3 w0 = getAngularVelocity();
btMatrix3x3 I;
I = m_worldTransform.getBasis().scaled(inertiaLocal) *
m_worldTransform.getBasis().transpose();
// use newtons method to find implicit solution for new angular velocity (w')
// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0
// df/dw' = I + 1xIw'*step + w'xI*step
btVector3 w1 = w0;
// one step of newton's method
{
const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);
btVector3 dw;
dw = dfw.solve33(fw);
//const btMatrix3x3 dfw_inv = dfw.inverse();
//dw = dfw_inv*fw;
w1 -= dw;
}
btVector3 gf = (w1 - w0);
return gf;
}
void btRigidBody::integrateVelocities(btScalar step)
{
if (isStaticOrKinematicObject())
@@ -317,38 +425,50 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
}
bool btRigidBody::checkCollideWithOverride(const btCollisionObject* co) const
{
const btRigidBody* otherRb = btRigidBody::upcast(co);
if (!otherRb)
return true;
for (int i = 0; i < m_constraintRefs.size(); ++i)
{
const btTypedConstraint* c = m_constraintRefs[i];
if (c->isEnabled())
if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
return false;
}
return true;
}
void btRigidBody::addConstraintRef(btTypedConstraint* c)
{
int index = m_constraintRefs.findLinearSearch(c);
if (index == m_constraintRefs.size())
m_constraintRefs.push_back(c);
///disable collision with the 'other' body
m_checkCollideWith = true;
int index = m_constraintRefs.findLinearSearch(c);
//don't add constraints that are already referenced
//btAssert(index == m_constraintRefs.size());
if (index == m_constraintRefs.size())
{
m_constraintRefs.push_back(c);
btCollisionObject* colObjA = &c->getRigidBodyA();
btCollisionObject* colObjB = &c->getRigidBodyB();
if (colObjA == this)
{
colObjA->setIgnoreCollisionCheck(colObjB, true);
}
else
{
colObjB->setIgnoreCollisionCheck(colObjA, true);
}
}
}
void btRigidBody::removeConstraintRef(btTypedConstraint* c)
{
m_constraintRefs.remove(c);
m_checkCollideWith = m_constraintRefs.size() > 0;
int index = m_constraintRefs.findLinearSearch(c);
//don't remove constraints that are not referenced
if(index < m_constraintRefs.size())
{
m_constraintRefs.remove(c);
btCollisionObject* colObjA = &c->getRigidBodyA();
btCollisionObject* colObjB = &c->getRigidBodyB();
if (colObjA == this)
{
colObjA->setIgnoreCollisionCheck(colObjB, false);
}
else
{
colObjB->setIgnoreCollisionCheck(colObjA, false);
}
}
}
int btRigidBody::calculateSerializeBufferSize() const

28
extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h vendored
View File

@@ -41,10 +41,13 @@ extern bool gDisableDeactivation;
enum btRigidBodyFlags
{
BT_DISABLE_WORLD_GRAVITY = 1,
///The BT_ENABLE_GYROPSCOPIC_FORCE can easily introduce instability
///So generally it is best to not enable it.
///If really needed, run at a high frequency like 1000 Hertz: ///See Demos/GyroscopicDemo for an example use
BT_ENABLE_GYROPSCOPIC_FORCE = 2
///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
///and it BT_ENABLE_GYROPSCOPIC_FORCE becomes equivalent to BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit
BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT = 2,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD=4,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY=8,
BT_ENABLE_GYROPSCOPIC_FORCE = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY,
};
@@ -87,7 +90,7 @@ class btRigidBody : public btCollisionObject
//m_optionalMotionState allows to automatic synchronize the world transform for active objects
btMotionState* m_optionalMotionState;
//keep track of typed constraints referencing this rigid body
//keep track of typed constraints referencing this rigid body, to disable collision between linked bodies
btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
int m_rigidbodyFlags;
@@ -506,8 +509,6 @@ public:
return (getBroadphaseProxy() != 0);
}
virtual bool checkCollideWithOverride(const btCollisionObject* co) const;
void addConstraintRef(btTypedConstraint* c);
void removeConstraintRef(btTypedConstraint* c);
@@ -531,7 +532,18 @@ public:
return m_rigidbodyFlags;
}
btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;
///perform implicit force computation in world space
btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const;
///perform implicit force computation in body space (inertial frame)
btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const;
///explicit version is best avoided, it gains energy
btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const;
btVector3 getLocalInertia() const;
///////////////////////////////////////////////

1855
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp vendored
View File

File diff suppressed because it is too large Load Diff

536
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h vendored
View File

@@ -6,7 +6,8 @@
*
* COPYRIGHT:
* Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
* Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
* Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
* Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
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.
@@ -31,10 +32,23 @@
#include "LinearMath/btAlignedObjectArray.h"
///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms
#ifdef BT_USE_DOUBLE_PRECISION
#define btMultiBodyData btMultiBodyDoubleData
#define btMultiBodyDataName "btMultiBodyDoubleData"
#define btMultiBodyLinkData btMultiBodyLinkDoubleData
#define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData"
#else
#define btMultiBodyData btMultiBodyFloatData
#define btMultiBodyDataName "btMultiBodyFloatData"
#define btMultiBodyLinkData btMultiBodyLinkFloatData
#define btMultiBodyLinkDataName "btMultiBodyLinkFloatData"
#endif //BT_USE_DOUBLE_PRECISION
#include "btMultiBodyLink.h"
class btMultiBodyLinkCollider;
class btMultiBody
ATTRIBUTE_ALIGNED16(class) btMultiBody
{
public:
@@ -45,42 +59,71 @@ public:
// initialization
//
btMultiBody(int n_links, // NOT including the base
btScalar mass, // mass of base
const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal
bool fixed_base_, // whether the base is fixed (true) or can move (false)
bool can_sleep_);
btMultiBody(int n_links, // NOT including the base
btScalar mass, // mass of base
const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal
bool fixedBase, // whether the base is fixed (true) or can move (false)
bool canSleep, bool deprecatedMultiDof=true);
~btMultiBody();
virtual ~btMultiBody();
void setupPrismatic(int i, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia, // in my frame; assumed diagonal
int parent,
const btQuaternion &rot_parent_to_this, // rotate points in parent frame to my frame.
const btVector3 &joint_axis, // in my frame
const btVector3 &r_vector_when_q_zero, // vector from parent COM to my COM, in my frame, when q = 0.
bool disableParentCollision=false
);
//note: fixed link collision with parent is always disabled
void setupFixed(int linkIndex,
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisPivotOffset,
const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true);
void setupRevolute(int i, // 0 to num_links-1
void setupPrismatic(int i,
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &jointAxis,
const btVector3 &parentComToThisPivotOffset,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision);
void setupRevolute(int linkIndex, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parentIndex,
const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
const btVector3 &jointAxis, // in my frame
const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
bool disableParentCollision=false);
void setupSpherical(int linkIndex, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &zero_rot_parent_to_this, // rotate points in parent frame to this frame, when q = 0
const btVector3 &joint_axis, // in my frame
const btVector3 &parent_axis_position, // vector from parent COM to joint axis, in PARENT frame
const btVector3 &my_axis_position, // vector from joint axis to my COM, in MY frame
bool disableParentCollision=false);
const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
bool disableParentCollision=false);
void setupPlanar(int i, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
const btVector3 &rotationAxis,
const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame
bool disableParentCollision=false);
const btMultibodyLink& getLink(int index) const
{
return links[index];
return m_links[index];
}
btMultibodyLink& getLink(int index)
{
return links[index];
return m_links[index];
}
@@ -106,69 +149,98 @@ public:
//
// get number of links, masses, moments of inertia
// get number of m_links, masses, moments of inertia
//
int getNumLinks() const { return links.size(); }
btScalar getBaseMass() const { return base_mass; }
const btVector3 & getBaseInertia() const { return base_inertia; }
int getNumLinks() const { return m_links.size(); }
int getNumDofs() const { return m_dofCount; }
int getNumPosVars() const { return m_posVarCnt; }
btScalar getBaseMass() const { return m_baseMass; }
const btVector3 & getBaseInertia() const { return m_baseInertia; }
btScalar getLinkMass(int i) const;
const btVector3 & getLinkInertia(int i) const;
//
// change mass (incomplete: can only change base mass and inertia at present)
//
void setBaseMass(btScalar mass) { base_mass = mass; }
void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; }
void setBaseMass(btScalar mass) { m_baseMass = mass; }
void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }
//
// get/set pos/vel/rot/omega for the base link
//
const btVector3 & getBasePos() const { return base_pos; } // in world frame
const btVector3 & getBasePos() const { return m_basePos; } // in world frame
const btVector3 getBaseVel() const
{
return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]);
return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]);
} // in world frame
const btQuaternion & getWorldToBaseRot() const
{
return base_quat;
return m_baseQuat;
} // rotates world vectors into base frame
btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); } // in world frame
btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame
void setBasePos(const btVector3 &pos)
{
base_pos = pos;
m_basePos = pos;
}
void setBaseWorldTransform(const btTransform& tr)
{
setBasePos(tr.getOrigin());
setWorldToBaseRot(tr.getRotation().inverse());
}
btTransform getBaseWorldTransform() const
{
btTransform tr;
tr.setOrigin(getBasePos());
tr.setRotation(getWorldToBaseRot().inverse());
return tr;
}
void setBaseVel(const btVector3 &vel)
{
m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2];
m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2];
}
void setWorldToBaseRot(const btQuaternion &rot)
{
base_quat = rot;
m_baseQuat = rot; //m_baseQuat asumed to ba alias!?
}
void setBaseOmega(const btVector3 &omega)
{
m_real_buf[0]=omega[0];
m_real_buf[1]=omega[1];
m_real_buf[2]=omega[2];
m_realBuf[0]=omega[0];
m_realBuf[1]=omega[1];
m_realBuf[2]=omega[2];
}
//
// get/set pos/vel for child links (i = 0 to num_links-1)
// get/set pos/vel for child m_links (i = 0 to num_links-1)
//
btScalar getJointPos(int i) const;
btScalar getJointVel(int i) const;
btScalar * getJointVelMultiDof(int i);
btScalar * getJointPosMultiDof(int i);
const btScalar * getJointVelMultiDof(int i) const ;
const btScalar * getJointPosMultiDof(int i) const ;
void setJointPos(int i, btScalar q);
void setJointVel(int i, btScalar qdot);
void setJointPosMultiDof(int i, btScalar *q);
void setJointVelMultiDof(int i, btScalar *qdot);
//
// direct access to velocities as a vector of 6 + num_links elements.
@@ -176,7 +248,7 @@ public:
//
const btScalar * getVelocityVector() const
{
return &m_real_buf[0];
return &m_realBuf[0];
}
/* btScalar * getVelocityVector()
{
@@ -185,7 +257,7 @@ public:
*/
//
// get the frames of reference (positions and orientations) of the child links
// get the frames of reference (positions and orientations) of the child m_links
// (i = 0 to num_links-1)
//
@@ -216,22 +288,37 @@ public:
//
void clearForcesAndTorques();
void clearConstraintForces();
void clearVelocities();
void addBaseForce(const btVector3 &f)
{
base_force += f;
m_baseForce += f;
}
void addBaseTorque(const btVector3 &t) { base_torque += t; }
void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
void addLinkForce(int i, const btVector3 &f);
void addLinkTorque(int i, const btVector3 &t);
void addJointTorque(int i, btScalar Q);
const btVector3 & getBaseForce() const { return base_force; }
const btVector3 & getBaseTorque() const { return base_torque; }
void addBaseConstraintForce(const btVector3 &f)
{
m_baseConstraintForce += f;
}
void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
void addLinkConstraintForce(int i, const btVector3 &f);
void addLinkConstraintTorque(int i, const btVector3 &t);
void addJointTorque(int i, btScalar Q);
void addJointTorqueMultiDof(int i, int dof, btScalar Q);
void addJointTorqueMultiDof(int i, const btScalar *Q);
const btVector3 & getBaseForce() const { return m_baseForce; }
const btVector3 & getBaseTorque() const { return m_baseTorque; }
const btVector3 & getLinkForce(int i) const;
const btVector3 & getLinkTorque(int i) const;
btScalar getJointTorque(int i) const;
btScalar * getJointTorqueMultiDof(int i);
//
@@ -250,62 +337,82 @@ public:
// improvement, at least on Windows (where dynamic memory
// allocation appears to be fairly slow).
//
void stepVelocities(btScalar dt,
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m);
btAlignedObjectArray<btMatrix3x3> &scratch_m,
bool isConstraintPass=false
);
// calcAccelerationDeltas
///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
void stepVelocitiesMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m,
bool isConstraintPass=false)
{
computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt,scratch_r,scratch_v,scratch_m,isConstraintPass);
}
// calcAccelerationDeltasMultiDof
// input: force vector (in same format as jacobian, i.e.:
// 3 torque values, 3 force values, num_links joint torque values)
// output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
// (existing contents of output array are replaced)
// stepVelocities must have been called first.
void calcAccelerationDeltas(const btScalar *force, btScalar *output,
// calcAccelerationDeltasMultiDof must have been called first.
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v) const;
// apply a delta-vee directly. used in sequential impulses code.
void applyDeltaVee(const btScalar * delta_vee)
void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier)
{
for (int i = 0; i < 6 + getNumLinks(); ++i)
{
m_real_buf[i] += delta_vee[i];
}
}
void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier)
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
m_deltaV[dof] += delta_vee[dof] * multiplier;
}
}
void processDeltaVeeMultiDof2()
{
btScalar sum = 0;
for (int i = 0; i < 6 + getNumLinks(); ++i)
{
sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
}
btScalar l = btSqrt(sum);
/*
static btScalar maxl = -1e30f;
if (l>maxl)
{
maxl=l;
// printf("maxl=%f\n",maxl);
}
*/
if (l>m_maxAppliedImpulse)
{
// printf("exceeds 100: l=%f\n",maxl);
multiplier *= m_maxAppliedImpulse/l;
}
applyDeltaVeeMultiDof(&m_deltaV[0],1);
for (int i = 0; i < 6 + getNumLinks(); ++i)
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
m_deltaV[dof] = 0.f;
}
}
void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier)
{
//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
// printf("%.4f ", delta_vee[dof]*multiplier);
//printf("\n");
//btScalar sum = 0;
//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
//{
// sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
//}
//btScalar l = btSqrt(sum);
//if (l>m_maxAppliedImpulse)
//{
// multiplier *= m_maxAppliedImpulse/l;
//}
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
m_real_buf[i] += delta_vee[i] * multiplier;
m_realBuf[dof] += delta_vee[dof] * multiplier;
btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity);
}
}
// timestep the positions (given current velocities).
void stepPositions(btScalar dt);
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
//
@@ -315,11 +422,23 @@ public:
// This routine fills out a contact constraint jacobian for this body.
// the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
// 'normal' & 'contact_point' are both given in world coordinates.
void fillContactJacobian(int link,
void fillContactJacobianMultiDof(int link,
const btVector3 &contact_point,
const btVector3 &normal,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
//a more general version of fillContactJacobianMultiDof which does not assume..
//.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
void fillConstraintJacobianMultiDof(int link,
const btVector3 &contact_point,
const btVector3 &normal_ang,
const btVector3 &normal_lin,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
@@ -329,17 +448,22 @@ public:
//
void setCanSleep(bool canSleep)
{
can_sleep = canSleep;
m_canSleep = canSleep;
}
bool isAwake() const { return awake; }
bool getCanSleep()const
{
return m_canSleep;
}
bool isAwake() const { return m_awake; }
void wakeUp();
void goToSleep();
void checkMotionAndSleepIfRequired(btScalar timestep);
bool hasFixedBase() const
{
return fixed_base;
return m_fixedBase;
}
int getCompanionId() const
@@ -352,9 +476,9 @@ public:
m_companionId = id;
}
void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links
void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
{
links.resize(numLinks);
m_links.resize(numLinks);
}
btScalar getLinearDamping() const
@@ -369,6 +493,10 @@ public:
{
return m_angularDamping;
}
void setAngularDamping( btScalar damp)
{
m_angularDamping = damp;
}
bool getUseGyroTerm() const
{
@@ -378,6 +506,15 @@ public:
{
m_useGyroTerm = useGyro;
}
btScalar getMaxCoordinateVelocity() const
{
return m_maxCoordinateVelocity ;
}
void setMaxCoordinateVelocity(btScalar maxVel)
{
m_maxCoordinateVelocity = maxVel;
}
btScalar getMaxAppliedImpulse() const
{
return m_maxAppliedImpulse;
@@ -386,7 +523,6 @@ public:
{
m_maxAppliedImpulse = maxImp;
}
void setHasSelfCollision(bool hasSelfCollision)
{
m_hasSelfCollision = hasSelfCollision;
@@ -396,6 +532,47 @@ public:
return m_hasSelfCollision;
}
void finalizeMultiDof();
void useRK4Integration(bool use) { m_useRK4 = use; }
bool isUsingRK4Integration() const { return m_useRK4; }
void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; }
bool isPosUpdated() const
{
return __posUpdated;
}
void setPosUpdated(bool updated)
{
__posUpdated = updated;
}
//internalNeedsJointFeedback is for internal use only
bool internalNeedsJointFeedback() const
{
return m_internalNeedsJointFeedback;
}
void forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
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;
const char* getBaseName() const
{
return m_baseName;
}
///memory of setBaseName needs to be manager by user
void setBaseName(const char* name)
{
m_baseName = name;
}
private:
btMultiBody(const btMultiBody &); // not implemented
void operator=(const btMultiBody &); // not implemented
@@ -403,64 +580,181 @@ private:
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
void updateLinksDofOffsets()
{
int dofOffset = 0, cfgOffset = 0;
for(int bidx = 0; bidx < m_links.size(); ++bidx)
{
m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset;
dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount;
}
}
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
private:
btMultiBodyLinkCollider* m_baseCollider;//can be NULL
const char* m_baseName;//memory needs to be manager by user!
btVector3 base_pos; // position of COM of base (world frame)
btQuaternion base_quat; // rotates world points into base frame
btVector3 m_basePos; // position of COM of base (world frame)
btQuaternion m_baseQuat; // rotates world points into base frame
btScalar base_mass; // mass of the base
btVector3 base_inertia; // inertia of the base (in local frame; diagonal)
btScalar m_baseMass; // mass of the base
btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal)
btVector3 base_force; // external force applied to base. World frame.
btVector3 base_torque; // external torque applied to base. World frame.
btAlignedObjectArray<btMultibodyLink> links; // array of links, excluding the base. index from 0 to num_links-1.
btVector3 m_baseForce; // external force applied to base. World frame.
btVector3 m_baseTorque; // external torque applied to base. World frame.
btVector3 m_baseConstraintForce; // external force applied to base. World frame.
btVector3 m_baseConstraintTorque; // external torque applied to base. World frame.
btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1.
btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
//
// real_buf:
// realBuf:
// offset size array
// 0 6 + num_links v (base_omega; base_vel; joint_vels)
// 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
// 6+num_links num_links D
//
// vector_buf:
// vectorBuf:
// offset size array
// 0 num_links h_top
// num_links num_links h_bottom
//
// matrix_buf:
// matrixBuf:
// offset size array
// 0 num_links+1 rot_from_parent
//
btAlignedObjectArray<btScalar> m_real_buf;
btAlignedObjectArray<btVector3> vector_buf;
btAlignedObjectArray<btMatrix3x3> matrix_buf;
btAlignedObjectArray<btScalar> m_deltaV;
btAlignedObjectArray<btScalar> m_realBuf;
btAlignedObjectArray<btVector3> m_vectorBuf;
btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
//std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
btMatrix3x3 cached_inertia_top_left;
btMatrix3x3 cached_inertia_top_right;
btMatrix3x3 cached_inertia_lower_left;
btMatrix3x3 cached_inertia_lower_right;
btMatrix3x3 m_cachedInertiaTopLeft;
btMatrix3x3 m_cachedInertiaTopRight;
btMatrix3x3 m_cachedInertiaLowerLeft;
btMatrix3x3 m_cachedInertiaLowerRight;
bool fixed_base;
bool m_fixedBase;
// Sleep parameters.
bool awake;
bool can_sleep;
btScalar sleep_timer;
bool m_awake;
bool m_canSleep;
btScalar m_sleepTimer;
int m_companionId;
btScalar m_linearDamping;
btScalar m_angularDamping;
bool m_useGyroTerm;
btScalar m_maxAppliedImpulse;
btScalar m_maxCoordinateVelocity;
bool m_hasSelfCollision;
bool __posUpdated;
int m_dofCount, m_posVarCnt;
bool m_useRK4, m_useGlobalVelocities;
///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
bool m_internalNeedsJointFeedback;
};
struct btMultiBodyLinkDoubleData
{
btQuaternionDoubleData m_zeroRotParentToThis;
btVector3DoubleData m_parentComToThisComOffset;
btVector3DoubleData m_thisPivotToThisComOffset;
btVector3DoubleData m_jointAxisTop[6];
btVector3DoubleData m_jointAxisBottom[6];
char *m_linkName;
char *m_jointName;
btCollisionObjectDoubleData *m_linkCollider;
btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal)
double m_linkMass;
int m_parentIndex;
int m_jointType;
int m_dofCount;
int m_posVarCount;
double m_jointPos[7];
double m_jointVel[6];
double m_jointTorque[6];
};
struct btMultiBodyLinkFloatData
{
btQuaternionFloatData m_zeroRotParentToThis;
btVector3FloatData m_parentComToThisComOffset;
btVector3FloatData m_thisPivotToThisComOffset;
btVector3FloatData m_jointAxisTop[6];
btVector3FloatData m_jointAxisBottom[6];
char *m_linkName;
char *m_jointName;
btCollisionObjectFloatData *m_linkCollider;
btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal)
int m_dofCount;
float m_linkMass;
int m_parentIndex;
int m_jointType;
float m_jointPos[7];
float m_jointVel[6];
float m_jointTorque[6];
int m_posVarCount;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btMultiBodyDoubleData
{
char *m_baseName;
btMultiBodyLinkDoubleData *m_links;
btCollisionObjectDoubleData *m_baseCollider;
btTransformDoubleData m_baseWorldTransform;
btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal)
int m_numLinks;
double m_baseMass;
char m_padding[4];
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btMultiBodyFloatData
{
char *m_baseName;
btMultiBodyLinkFloatData *m_links;
btCollisionObjectFloatData *m_baseCollider;
btTransformFloatData m_baseWorldTransform;
btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal)
float m_baseMass;
int m_numLinks;
};
#endif

482
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp vendored
View File

@@ -1,263 +1,101 @@
#include "btMultiBodyConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
:m_bodyA(bodyA),
m_bodyB(bodyB),
m_linkA(linkA),
m_linkB(linkB),
m_num_rows(numRows),
m_numRows(numRows),
m_jacSizeA(0),
m_jacSizeBoth(0),
m_isUnilateral(isUnilateral),
m_numDofsFinalized(-1),
m_maxAppliedImpulse(100)
{
m_jac_size_A = (6 + bodyA->getNumLinks());
m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0));
m_pos_offset = ((1 + m_jac_size_both)*m_num_rows);
m_data.resize((2 + m_jac_size_both) * m_num_rows);
}
void btMultiBodyConstraint::updateJacobianSizes()
{
if(m_bodyA)
{
m_jacSizeA = (6 + m_bodyA->getNumDofs());
}
if(m_bodyB)
{
m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
}
else
m_jacSizeBoth = m_jacSizeA;
}
void btMultiBodyConstraint::allocateJacobiansMultiDof()
{
updateJacobianSizes();
m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
m_data.resize((2 + m_jacSizeBoth) * m_numRows);
}
btMultiBodyConstraint::~btMultiBodyConstraint()
{
}
btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
btMultiBodyJacobianData& data,
btScalar* jacOrgA,btScalar* jacOrgB,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity,
btScalar lowerLimit,
btScalar upperLimit)
{
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 6;
constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
if (constraintRow.m_deltaVelAindex <0)
{
constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
} else
{
btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
}
constraintRow.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
for (int i=0;i<ndofA;i++)
data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
if (constraintRow.m_deltaVelBindex <0)
{
constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
constraintRow.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
for (int i=0;i<ndofB;i++)
data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* lambdaA =0;
btScalar* lambdaB =0;
int ndofA = 0;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
jacA = &data.m_jacobians[constraintRow.m_jacAindex];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar l =lambdaA[i];
denom0 += j*l;
}
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
jacB = &data.m_jacobians[constraintRow.m_jacBindex];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
btScalar l =lambdaB[i];
denom1 += j*l;
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
btScalar d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
constraintRow.m_jacDiagABInv = 1.f/(d);
} else
{
constraintRow.m_jacDiagABInv = 1.f;
}
}
//compute rhs and remaining constraintRow fields
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
}
constraintRow.m_friction = 0.f;
constraintRow.m_appliedImpulse = 0.f;
constraintRow.m_appliedPushImpulse = 0.f;
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse)
{
//combine position and velocity into rhs
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
}
constraintRow.m_cfm = 0.f;
constraintRow.m_lowerLimit = lowerLimit;
constraintRow.m_upperLimit = upperLimit;
}
return rel_vel;
}
void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
{
for (int i = 0; i < ndof; ++i)
for (int i = 0; i < ndof; ++i)
data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
}
void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar position,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar posError,
const btContactSolverInfo& infoGlobal,
btScalar lowerLimit, btScalar upperLimit,
btScalar relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
btVector3 rel_pos1 = posAworld;
btVector3 rel_pos2 = posBworld;
solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB;
solverConstraint.m_linkA = m_linkA;
solverConstraint.m_linkB = m_linkB;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
const btVector3& pos1 = posAworld;
const btVector3& pos2 = posBworld;
btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
if (bodyA)
rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
if (bodyB)
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
relaxation = 1.f;
rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 6;
if (solverConstraint.m_linkA<0)
{
rel_pos1 = posAworld - multiBodyA->getBasePos();
} else
{
rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
}
const int ndofA = multiBodyA->getNumDofs() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
@@ -271,16 +109,35 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
}
//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
//resize..
solverConstraint.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
//copy/determine
if(jacOrgA)
{
for (int i=0;i<ndofA;i++)
data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
}
else
{
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
}
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
//resize..
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
} else
//determine..
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = contactNormalOnB;
}
else //if(rb0)
{
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
@@ -290,7 +147,15 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
if (solverConstraint.m_linkB<0)
{
rel_pos2 = posBworld - multiBodyB->getBasePos();
} else
{
rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
}
const int ndofB = multiBodyB->getNumDofs() + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
@@ -300,144 +165,136 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
//resize..
solverConstraint.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
//copy/determine..
if(jacOrgB)
{
for (int i=0;i<ndofB;i++)
data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
}
else
{
multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
}
//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
//resize..
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
//determine..
multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
} else
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormalOnB;
}
else //if(rb1)
{
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormalOnB;
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* lambdaA =0;
btScalar* lambdaB =0;
btScalar* deltaVelA = 0;
btScalar* deltaVelB = 0;
int ndofA = 0;
//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
ndofA = multiBodyA->getNumDofs() + 6;
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar l =lambdaA[i];
btScalar l = deltaVelA[i];
denom0 += j*l;
}
} else
{
if (rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
}
}
else if(rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
}
//
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
const int ndofB = multiBodyB->getNumDofs() + 6;
jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
btScalar l =lambdaB[i];
btScalar l = deltaVelB[i];
denom1 += j*l;
}
} else
}
else if(rb1)
{
if (rb1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
}
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
btScalar d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
} else
{
solverConstraint.m_jacDiagABInv = 1.f;
}
//
btScalar d = denom0+denom1;
if (d>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
}
else
{
//disable the constraint row to handle singularity/redundant constraint
solverConstraint.m_jacDiagABInv = 0.f;
}
}
//compute rhs and remaining solverConstraint fields
btScalar restitution = 0.f;
btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop;
btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
ndofA = multiBodyA->getNumDofs() + 6;
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
} else
}
else if(rb0)
{
if (rb0)
{
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
}
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
ndofB = multiBodyB->getNumDofs() + 6;
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
} else
}
else if(rb1)
{
if (rb1)
{
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= btScalar(0.))
{
restitution = 0.f;
};
}
@@ -474,18 +331,15 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
}
} else
*/
{
solverConstraint.m_appliedImpulse = 0.f;
}
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping;
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
@@ -493,15 +347,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
erp = infoGlobal.m_erp;
}
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
positionalError = -penetration * erp/infoGlobal.m_timeStep;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
@@ -520,8 +366,10 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
}
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = -m_maxAppliedImpulse;
solverConstraint.m_upperLimit = m_maxAppliedImpulse;
solverConstraint.m_lowerLimit = lowerLimit;
solverConstraint.m_upperLimit = upperLimit;
}
return rel_vel;
}

106
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h vendored
View File

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
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.
@@ -28,8 +28,8 @@ struct btSolverInfo;
struct btMultiBodyJacobianData
{
btAlignedObjectArray<btScalar> m_jacobians;
btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse;
btAlignedObjectArray<btScalar> m_deltaVelocities;
btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
btAlignedObjectArray<btScalar> m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
@@ -48,16 +48,17 @@ protected:
int m_linkA;
int m_linkB;
int m_num_rows;
int m_jac_size_A;
int m_jac_size_both;
int m_pos_offset;
int m_numRows;
int m_jacSizeA;
int m_jacSizeBoth;
int m_posOffset;
bool m_isUnilateral;
int m_numDofsFinalized;
btScalar m_maxAppliedImpulse;
// warning: the data block lay out is not consistent for all constraints
// data block laid out as follows:
// cached impulses. (one per row.)
// jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
@@ -66,40 +67,37 @@ protected:
void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar position,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
btMultiBodyJacobianData& data,
btScalar* jacOrgA,btScalar* jacOrgB,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity,
btScalar lowerLimit,
btScalar upperLimit);
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar posError,
const btContactSolverInfo& infoGlobal,
btScalar lowerLimit, btScalar upperLimit,
btScalar relaxation = 1.f,
bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
public:
btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
virtual ~btMultiBodyConstraint();
void updateJacobianSizes();
void allocateJacobiansMultiDof();
virtual void finalizeMultiDof()=0;
virtual int getIslandIdA() const =0;
virtual int getIslandIdB() const =0;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)=0;
int getNumRows() const
{
return m_num_rows;
return m_numRows;
}
btMultiBody* getMultiBodyA()
@@ -111,20 +109,33 @@ public:
return m_bodyB;
}
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
{
btAssert(dof>=0);
btAssert(dof < getNumRows());
m_data[dof] = appliedImpulse;
}
btScalar getAppliedImpulse(int dof)
{
btAssert(dof>=0);
btAssert(dof < getNumRows());
return m_data[dof];
}
// current constraint position
// constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
// NOTE: ignored position for friction rows.
btScalar getPosition(int row) const
{
return m_data[m_pos_offset + row];
btScalar getPosition(int row) const
{
return m_data[m_posOffset + row];
}
void setPosition(int row, btScalar pos)
{
m_data[m_pos_offset + row] = pos;
void setPosition(int row, btScalar pos)
{
m_data[m_posOffset + row] = pos;
}
bool isUnilateral() const
{
return m_isUnilateral;
@@ -133,21 +144,21 @@ public:
// jacobian blocks.
// each of size 6 + num_links. (jacobian2 is null if no body2.)
// format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
btScalar* jacobianA(int row)
{
return &m_data[m_num_rows + row * m_jac_size_both];
btScalar* jacobianA(int row)
{
return &m_data[m_numRows + row * m_jacSizeBoth];
}
const btScalar* jacobianA(int row) const
{
return &m_data[m_num_rows + (row * m_jac_size_both)];
const btScalar* jacobianA(int row) const
{
return &m_data[m_numRows + (row * m_jacSizeBoth)];
}
btScalar* jacobianB(int row)
{
return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A];
btScalar* jacobianB(int row)
{
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
}
const btScalar* jacobianB(int row) const
{
return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A];
const btScalar* jacobianB(int row) const
{
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
}
btScalar getMaxAppliedImpulse() const
@@ -158,7 +169,8 @@ public:
{
m_maxAppliedImpulse = maxImp;
}
virtual void debugDraw(class btIDebugDraw* drawer)=0;
};

509
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp vendored
View File

@@ -13,6 +13,7 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btMultiBodyConstraintSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "btMultiBodyLinkCollider.h"
@@ -33,9 +34,12 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
{
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
//if (iteration < constraint.m_overrideNumSolverIterations)
//resolveSingleConstraintRowGenericMultiBody(constraint);
resolveSingleConstraintRowGeneric(constraint);
if(constraint.m_multiBodyA)
constraint.m_multiBodyA->setPosUpdated(false);
if(constraint.m_multiBodyB)
constraint.m_multiBodyB->setPosUpdated(false);
}
//solve featherstone normal contact
@@ -44,6 +48,11 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
if (iteration < infoGlobal.m_numIterations)
resolveSingleConstraintRowGeneric(constraint);
if(constraint.m_multiBodyA)
constraint.m_multiBodyA->setPosUpdated(false);
if(constraint.m_multiBodyB)
constraint.m_multiBodyB->setPosUpdated(false);
}
//solve featherstone frictional contact
@@ -60,6 +69,11 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
resolveSingleConstraintRowGeneric(frictionConstraint);
if(frictionConstraint.m_multiBodyA)
frictionConstraint.m_multiBodyA->setPosUpdated(false);
if(frictionConstraint.m_multiBodyB)
frictionConstraint.m_multiBodyB->setPosUpdated(false);
}
}
}
@@ -108,10 +122,10 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
if (c.m_multiBodyA)
{
ndofA = c.m_multiBodyA->getNumLinks() + 6;
ndofA = c.m_multiBodyA->getNumDofs() + 6;
for (int i = 0; i < ndofA; ++i)
deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
} else
} else if(c.m_solverBodyIdA >= 0)
{
bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
@@ -119,10 +133,10 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
if (c.m_multiBodyB)
{
ndofB = c.m_multiBodyB->getNumLinks() + 6;
ndofB = c.m_multiBodyB->getNumDofs() + 6;
for (int i = 0; i < ndofB; ++i)
deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
} else
} else if(c.m_solverBodyIdB >= 0)
{
bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
@@ -151,8 +165,12 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
if (c.m_multiBodyA)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
} else
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
} else if(c.m_solverBodyIdA >= 0)
{
bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
@@ -160,8 +178,12 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
if (c.m_multiBodyB)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
} else
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
} else if(c.m_solverBodyIdB >= 0)
{
bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
}
@@ -169,60 +191,6 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
}
void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
btScalar deltaVelADotn=0;
btScalar deltaVelBDotn=0;
int ndofA=0;
int ndofB=0;
if (c.m_multiBodyA)
{
ndofA = c.m_multiBodyA->getNumLinks() + 6;
for (int i = 0; i < ndofA; ++i)
deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
}
if (c.m_multiBodyB)
{
ndofB = c.m_multiBodyB->getNumLinks() + 6;
for (int i = 0; i < ndofB; ++i)
deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
}
deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else if (sum > c.m_upperLimit)
{
deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
c.m_appliedImpulse = c.m_upperLimit;
}
else
{
c.m_appliedImpulse = sum;
}
if (c.m_multiBodyA)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
}
if (c.m_multiBodyB)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
}
}
void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
@@ -255,9 +223,19 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
relaxation = 1.f;
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 6;
if (solverConstraint.m_linkA<0)
{
rel_pos1 = pos1 - multiBodyA->getBasePos();
} else
{
rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
}
const int ndofA = multiBodyA->getNumDofs() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
@@ -277,20 +255,34 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = contactNormal;
} else
{
btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = contactNormal;
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
if (solverConstraint.m_linkB<0)
{
rel_pos2 = pos2 - multiBodyB->getBasePos();
} else
{
rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
}
const int ndofB = multiBodyB->getNumDofs() + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
@@ -306,14 +298,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormal;
} else
{
btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormal;
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
}
{
@@ -328,7 +326,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
int ndofA = 0;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
ndofA = multiBodyA->getNumDofs() + 6;
jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
@@ -347,7 +345,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
const int ndofB = multiBodyB->getNumDofs() + 6;
jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
@@ -366,24 +364,16 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
btScalar d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
if (d>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
solverConstraint.m_jacDiagABInv = relaxation/(d);
} else
{
solverConstraint.m_jacDiagABInv = 1.f;
//disable the constraint row to handle singularity/redundant constraint
solverConstraint.m_jacDiagABInv = 0.f;
}
}
@@ -404,7 +394,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
ndofA = multiBodyA->getNumDofs() + 6;
btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
@@ -417,7 +407,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
ndofB = multiBodyB->getNumDofs() + 6;
btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
@@ -432,17 +422,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
solverConstraint.m_friction = cp.m_combinedFriction;
restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= btScalar(0.))
if(!isFriction)
{
restitution = 0.f;
};
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)
//disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
@@ -452,7 +445,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVee(deltaV,impulse);
multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
} else
{
@@ -463,7 +457,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVee(deltaV,impulse);
multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
} else
{
@@ -479,11 +473,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping;
btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
@@ -494,7 +486,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
velocityError -= penetration / infoGlobal.m_timeStep;
} else
{
@@ -504,22 +496,33 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
if(!isFriction)
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
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
} else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
solverConstraint.m_rhsPenetration = 0.f;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
}
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
solverConstraint.m_cfm = 0.f; //why not use cfmSlip?
}
}
@@ -531,6 +534,9 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo
{
BT_PROFILE("addMultiBodyFrictionConstraint");
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
solverConstraint.m_orgConstraint = 0;
solverConstraint.m_orgDofIndex = -1;
solverConstraint.m_frictionIndex = frictionIndex;
bool isFriction = true;
@@ -575,15 +581,15 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
// btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
// btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
///avoid collision response between two static objects
// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
// return;
int rollingFriction=1;
for (int j=0;j<manifold->getNumContacts();j++)
{
@@ -599,8 +605,10 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
btRigidBody* rb0 = btRigidBody::upcast(colObj0);
btRigidBody* rb1 = btRigidBody::upcast(colObj1);
// btRigidBody* rb0 = btRigidBody::upcast(colObj0);
// btRigidBody* rb1 = btRigidBody::upcast(colObj1);
solverConstraint.m_orgConstraint = 0;
solverConstraint.m_orgDofIndex = -1;
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_multiBodyA = mbA;
@@ -624,6 +632,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
#ifdef ENABLE_FRICTION
solverConstraint.m_frictionIndex = frictionIndex;
#if ROLLING_FRICTION
int rollingFriction=1;
btVector3 angVelA(0,0,0),angVelB(0,0,0);
if (rb0)
angVelA = rb0->getAngularVelocity();
@@ -702,6 +711,10 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
{
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
@@ -709,10 +722,6 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
}
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
{
cp.m_lateralFrictionInitialized = true;
@@ -741,7 +750,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
{
btPersistentManifold* manifold = 0;
//btPersistentManifold* manifold = 0;
for (int i=0;i<numManifolds;i++)
{
@@ -779,6 +788,264 @@ btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int
return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
}
#if 0
static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
{
if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
{
//todo: get rid of those temporary memory allocations for the joint feedback
btAlignedObjectArray<btScalar> forceVector;
int numDofsPlusBase = 6+mb->getNumDofs();
forceVector.resize(numDofsPlusBase);
for (int i=0;i<numDofsPlusBase;i++)
{
forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
}
btAlignedObjectArray<btScalar> output;
output.resize(numDofsPlusBase);
bool applyJointFeedback = true;
mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
}
}
#endif
void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
{
#if 1
//bod->addBaseForce(m_gravity * bod->getBaseMass());
//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
if (c.m_orgConstraint)
{
c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
}
if (c.m_multiBodyA)
{
c.m_multiBodyA->setCompanionId(-1);
btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
if (c.m_linkA<0)
{
c.m_multiBodyA->addBaseConstraintForce(force);
c.m_multiBodyA->addBaseConstraintTorque(torque);
} else
{
c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
}
}
if (c.m_multiBodyB)
{
{
c.m_multiBodyB->setCompanionId(-1);
btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
if (c.m_linkB<0)
{
c.m_multiBodyB->addBaseConstraintForce(force);
c.m_multiBodyB->addBaseConstraintTorque(torque);
} else
{
{
c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
//b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
}
}
}
}
#endif
#ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
if (c.m_multiBodyA)
{
if(c.m_multiBodyA->isMultiDof())
{
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
}
else
{
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
}
}
if (c.m_multiBodyB)
{
if(c.m_multiBodyB->isMultiDof())
{
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
}
else
{
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
}
}
#endif
}
btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
//write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
//or as applied force, so we can measure the joint reaction forces easier
for (int i=0;i<numPoolConstraints;i++)
{
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
}
}
for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
{
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
}
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
BT_PROFILE("warm starting write back");
for (int j=0;j<numPoolConstraints;j++)
{
const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
btAssert(pt);
pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
}
//do a callback here?
}
}
#if 0
//multibody joint feedback
{
BT_PROFILE("multi body joint feedback");
for (int j=0;j<numPoolConstraints;j++)
{
const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
//apply the joint feedback into all links of the btMultiBody
//todo: double-check the signs of the applied impulse
if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
{
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
}
if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
{
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
}
#if 0
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
{
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
}
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
{
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
}
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
{
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
}
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
{
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
}
}
#endif
}
for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
{
const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
{
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
}
if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
{
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
}
}
}
numPoolConstraints = m_multiBodyNonContactConstraints.size();
#if 0
//@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
for (int i=0;i<numPoolConstraints;i++)
{
const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
btJointFeedback* fb = constr->getJointFeedback();
if (fb)
{
fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
}
constr->internalSetAppliedImpulse(c.m_appliedImpulse);
if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
{
constr->setEnabled(false);
}
}
#endif
#endif
return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
}
void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
{

8
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h vendored
View File

@@ -19,6 +19,7 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "btMultiBodySolverConstraint.h"
#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
class btMultiBody;
@@ -43,7 +44,7 @@ protected:
int m_tmpNumMultiBodyConstraints;
void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c);
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
@@ -66,14 +67,15 @@ protected:
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
};

546
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp vendored
View File

@@ -20,8 +20,8 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include "LinearMath/btQuickprof.h"
#include "btMultiBodyConstraint.h"
#include "LinearMath/btIDebugDraw.h"
#include "LinearMath/btSerializer.h"
void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
@@ -364,7 +364,9 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
m_bodies.resize(0);
@@ -392,11 +394,20 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
delete m_solverMultiBodyIslandCallback;
}
void btMultiBodyDynamicsWorld::forwardKinematics()
{
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
for (int b=0;b<m_multiBodies.size();b++)
{
btMultiBody* bod = m_multiBodies[b];
bod->forwardKinematics(world_to_local,local_origin);
}
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
forwardKinematics();
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
@@ -430,9 +441,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
BT_PROFILE("btMultiBody addForce and stepVelocities");
BT_PROFILE("btMultiBody addForce");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
@@ -451,27 +462,267 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
if (!isSleeping)
{
scratch_r.resize(bod->getNumLinks()+1);
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
scratch_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);
bod->clearForcesAndTorques();
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
}
}//if (!isSleeping)
}
}
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
scratch_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);
bool doNotUpdatePos = false;
{
if(!bod->isUsingRK4Integration())
{
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
}
else
{
//
int numDofs = bod->getNumDofs() + 6;
int numPosVars = bod->getNumPosVars() + 7;
btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
//convenience
btScalar *pMem = &scratch_r2[0];
btScalar *scratch_q0 = pMem; pMem += numPosVars;
btScalar *scratch_qx = pMem; pMem += numPosVars;
btScalar *scratch_qd0 = pMem; pMem += numDofs;
btScalar *scratch_qd1 = pMem; pMem += numDofs;
btScalar *scratch_qd2 = pMem; pMem += numDofs;
btScalar *scratch_qd3 = pMem; pMem += numDofs;
btScalar *scratch_qdd0 = pMem; pMem += numDofs;
btScalar *scratch_qdd1 = pMem; pMem += numDofs;
btScalar *scratch_qdd2 = pMem; pMem += numDofs;
btScalar *scratch_qdd3 = pMem; pMem += numDofs;
btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
/////
//copy q0 to scratch_q0 and qd0 to scratch_qd0
scratch_q0[0] = bod->getWorldToBaseRot().x();
scratch_q0[1] = bod->getWorldToBaseRot().y();
scratch_q0[2] = bod->getWorldToBaseRot().z();
scratch_q0[3] = bod->getWorldToBaseRot().w();
scratch_q0[4] = bod->getBasePos().x();
scratch_q0[5] = bod->getBasePos().y();
scratch_q0[6] = bod->getBasePos().z();
//
for(int link = 0; link < bod->getNumLinks(); ++link)
{
for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
}
//
for(int dof = 0; dof < numDofs; ++dof)
scratch_qd0[dof] = bod->getVelocityVector()[dof];
////
struct
{
btMultiBody *bod;
btScalar *scratch_qx, *scratch_q0;
void operator()()
{
for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
scratch_qx[dof] = scratch_q0[dof];
}
} pResetQx = {bod, scratch_qx, scratch_q0};
//
struct
{
void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
{
for(int i = 0; i < size; ++i)
pVal[i] = pCurVal[i] + dt * pDer[i];
}
} pEulerIntegrate;
//
struct
{
void operator()(btMultiBody *pBody, const btScalar *pData)
{
btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
pVel[i] = pData[i];
}
} pCopyToVelocityVector;
//
struct
{
void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
{
for(int i = 0; i < size; ++i)
pDst[i] = pSrc[start + i];
}
} pCopy;
//
btScalar h = solverInfo.m_timeStep;
#define output &scratch_r[bod->getNumDofs()]
//calc qdd0 from: q0 & qd0
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
pCopy(output, scratch_qdd0, 0, numDofs);
//calc q1 = q0 + h/2 * qd0
pResetQx();
bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
//calc qd1 = qd0 + h/2 * qdd0
pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
//
//calc qdd1 from: q1 & qd1
pCopyToVelocityVector(bod, scratch_qd1);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
pCopy(output, scratch_qdd1, 0, numDofs);
//calc q2 = q0 + h/2 * qd1
pResetQx();
bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
//calc qd2 = qd0 + h/2 * qdd1
pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
//
//calc qdd2 from: q2 & qd2
pCopyToVelocityVector(bod, scratch_qd2);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
pCopy(output, scratch_qdd2, 0, numDofs);
//calc q3 = q0 + h * qd2
pResetQx();
bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
//calc qd3 = qd0 + h * qdd2
pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
//
//calc qdd3 from: q3 & qd3
pCopyToVelocityVector(bod, scratch_qd3);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
pCopy(output, scratch_qdd3, 0, numDofs);
//
//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
for(int i = 0; i < numDofs; ++i)
{
delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);
//delta_q[i] = h*scratch_qd0[i];
//delta_qd[i] = h*scratch_qdd0[i];
}
//
pCopyToVelocityVector(bod, scratch_qd0);
bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
//
if(!doNotUpdatePos)
{
btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
for(int i = 0; i < numDofs; ++i)
pRealBuf[i] = delta_q[i];
//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
bod->setPosUpdated(true);
}
//ugly hack which resets the cached data to t0 (needed for constraint solver)
{
for(int link = 0; link < bod->getNumLinks(); ++link)
bod->getLink(link).updateCacheMultiDof();
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, scratch_r, scratch_v, scratch_m);
}
}
}
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
bod->clearForcesAndTorques();
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
}//if (!isSleeping)
}
}
clearMultiBodyConstraintForces();
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
scratch_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);
{
if(!bod->isUsingRK4Integration())
{
bool isConstraintPass = true;
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
}
}
}
}
}
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->processDeltaVeeMultiDof2();
}
}
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
@@ -503,60 +754,27 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
int nLinks = bod->getNumLinks();
///base + num links
///base + num m_links
{
if(!bod->isPosUpdated())
bod->stepPositionsMultiDof(timeStep);
else
{
btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
bod->stepPositionsMultiDof(1, 0, pRealBuf);
bod->setPosUpdated(false);
}
}
world_to_local.resize(nLinks+1);
local_origin.resize(nLinks+1);
bod->stepPositions(timeStep);
world_to_local[0] = bod->getWorldToBaseRot();
local_origin[0] = bod->getBasePos();
if (bod->getBaseCollider())
{
btVector3 posr = local_origin[0];
float pos[4]={posr.x(),posr.y(),posr.z(),1};
float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
bod->getBaseCollider()->setWorldTransform(tr);
}
for (int k=0;k<bod->getNumLinks();k++)
{
const int parent = bod->getParent(k);
world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1];
local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k)));
}
for (int m=0;m<bod->getNumLinks();m++)
{
btMultiBodyLinkCollider* col = bod->getLink(m).m_collider;
if (col)
{
int link = col->m_link;
btAssert(link == m);
int index = link+1;
btVector3 posr = local_origin[index];
float pos[4]={posr.x(),posr.y(),posr.z(),1};
float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
}
}
bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
} else
{
bod->clearVelocities();
@@ -576,3 +794,205 @@ void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint*
{
m_multiBodyConstraints.remove(constraint);
}
void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
{
constraint->debugDraw(getDebugDrawer());
}
void btMultiBodyDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
bool drawConstraints = false;
if (getDebugDrawer())
{
int mode = getDebugDrawer()->getDebugMode();
if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
{
drawConstraints = true;
}
if (drawConstraints)
{
BT_PROFILE("btMultiBody debugDrawWorld");
btAlignedObjectArray<btQuaternion> world_to_local1;
btAlignedObjectArray<btVector3> local_origin1;
for (int c=0;c<m_multiBodyConstraints.size();c++)
{
btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
debugDrawMultiBodyConstraint(constraint);
}
for (int b = 0; b<m_multiBodies.size(); b++)
{
btMultiBody* bod = m_multiBodies[b];
bod->forwardKinematics(world_to_local1,local_origin1);
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
for (int m = 0; m<bod->getNumLinks(); m++)
{
const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
getDebugDrawer()->drawTransform(tr, 0.1);
//draw the joint axis
if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
}
}
}
}
btDiscreteDynamicsWorld::debugDrawWorld();
}
void btMultiBodyDynamicsWorld::applyGravity()
{
btDiscreteDynamicsWorld::applyGravity();
#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
BT_PROFILE("btMultiBody addGravity");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
}//if (!isSleeping)
}
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
}
void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
{
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->clearConstraintForces();
}
}
void btMultiBodyDynamicsWorld::clearMultiBodyForces()
{
{
BT_PROFILE("clearMultiBodyForces");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
btMultiBody* bod = m_multiBodies[i];
bod->clearForcesAndTorques();
}
}
}
}
void btMultiBodyDynamicsWorld::clearForces()
{
btDiscreteDynamicsWorld::clearForces();
#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
clearMultiBodyForces();
#endif
}
void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
{
serializer->startSerialization();
serializeDynamicsWorldInfo( serializer);
serializeMultiBodies(serializer);
serializeRigidBodies(serializer);
serializeCollisionObjects(serializer);
serializer->finishSerialization();
}
void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
{
int i;
//serialize all collision objects
for (i=0;i<m_multiBodies.size();i++)
{
btMultiBody* mb = m_multiBodies[i];
{
int len = mb->calculateSerializeBufferSize();
btChunk* chunk = serializer->allocate(len,1);
const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
}
}
}

51
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h vendored
View File

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
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.
@@ -18,6 +18,7 @@ subject to the following restrictions:
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
#define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
class btMultiBody;
class btMultiBodyConstraint;
@@ -38,19 +39,61 @@ protected:
virtual void calculateSimulationIslands();
virtual void updateActivationState(btScalar timeStep);
virtual void solveConstraints(btContactSolverInfo& solverInfo);
virtual void integrateTransforms(btScalar timeStep);
virtual void serializeMultiBodies(btSerializer* serializer);
public:
btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
virtual ~btMultiBodyDynamicsWorld ();
virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
virtual void removeMultiBody(btMultiBody* body);
virtual int getNumMultibodies() const
{
return m_multiBodies.size();
}
btMultiBody* getMultiBody(int mbIndex)
{
return m_multiBodies[mbIndex];
}
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual int getNumMultiBodyConstraints() const
{
return m_multiBodyConstraints.size();
}
virtual btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex)
{
return m_multiBodyConstraints[constraintIndex];
}
virtual const btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex) const
{
return m_multiBodyConstraints[constraintIndex];
}
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void integrateTransforms(btScalar timeStep);
virtual void debugDrawWorld();
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
void forwardKinematics();
virtual void clearForces();
virtual void clearMultiBodyConstraintForces();
virtual void clearMultiBodyForces();
virtual void applyGravity();
virtual void serialize(btSerializer* serializer);
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

27
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h vendored Normal file
View File

@@ -0,0 +1,27 @@
/*
Copyright (c) 2015 Google 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 BT_MULTIBODY_JOINT_FEEDBACK_H
#define BT_MULTIBODY_JOINT_FEEDBACK_H
#include "LinearMath/btSpatialAlgebra.h"
struct btMultiBodyJointFeedback
{
btSpatialForceVector m_reactionForces;
};
#endif //BT_MULTIBODY_JOINT_FEEDBACK_H

126
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp vendored
View File

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -21,51 +21,68 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
:btMultiBodyConstraint(body,body,link,link,2,true),
//:btMultiBodyConstraint(body,0,link,-1,2,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,2,true),
m_lowerBound(lower),
m_upperBound(upper)
{
}
void btMultiBodyJointLimitConstraint::finalizeMultiDof()
{
// the data.m_jacobians never change, so may as well
// initialize them here
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
// row 0: the lower bound
jacobianA(0)[6 + link] = 1;
allocateJacobiansMultiDof();
// row 1: the upper bound
jacobianB(1)[6 + link] = -1;
unsigned int offset = 6 + m_bodyA->getLink(m_linkA).m_dofOffset;
// row 0: the lower bound
jacobianA(0)[offset] = 1;
// row 1: the upper bound
//jacobianA(1)[offset] = -1;
jacobianB(1)[offset] = -1;
m_numDofsFinalized = m_jacSizeBoth;
}
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
{
}
int btMultiBodyJointLimitConstraint::getIslandIdA() const
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if(m_bodyA)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyJointLimitConstraint::getIslandIdB() const
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if(m_bodyB)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
@@ -75,22 +92,71 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
// row 0: the lower bound
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent
// row 1: the upper bound
setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
for (int row=0;row<getNumRows();row++)
{
btScalar direction = row? -1 : 1;
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,-m_maxAppliedImpulse,m_maxAppliedImpulse);
const btScalar posError = 0; //why assume it's zero?
const btVector3 dummy(0, 0, 0);
btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
{
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
constraintRow.m_contactNormal1=prismaticAxisInWorld;
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
{
btScalar penetration = getPosition(row);
btScalar positionalError = 0.f;
@@ -127,7 +193,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
}
}

14
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h vendored
View File

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
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.
@@ -30,14 +30,20 @@ public:
btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
virtual ~btMultiBodyJointLimitConstraint();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)
}
};
#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H

98
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp vendored
View File

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
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.
@@ -22,18 +22,41 @@ subject to the following restrictions:
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
:btMultiBodyConstraint(body,body,link,link,1,true),
m_desiredVelocity(desiredVelocity)
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity)
{
m_maxAppliedImpulse = maxMotorImpulse;
// the data.m_jacobians never change, so may as well
// initialize them here
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
// row 0: the lower bound
jacobianA(0)[6 + link] = 1;
}
void btMultiBodyJointMotor::finalizeMultiDof()
{
allocateJacobiansMultiDof();
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
int linkDoF = 0;
unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
// row 0: the lower bound
// row 0: the lower bound
jacobianA(0)[offset] = 1;
m_numDofsFinalized = m_jacSizeBoth;
}
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
//:btMultiBodyConstraint(body,0,link,-1,1,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
m_maxAppliedImpulse = maxMotorImpulse;
}
btMultiBodyJointMotor::~btMultiBodyJointMotor()
{
@@ -74,16 +97,61 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
for (int row=0;row<getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
btScalar penetration = 0;
fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,m_desiredVelocity,-m_maxAppliedImpulse,m_maxAppliedImpulse);
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
constraintRow.m_contactNormal1=prismaticAxisInWorld;
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
}
}

20
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h vendored
View File

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
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.
@@ -25,13 +25,15 @@ class btMultiBodyJointMotor : public btMultiBodyConstraint
{
protected:
btScalar m_desiredVelocity;
public:
btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
virtual ~btMultiBodyJointMotor();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
@@ -39,8 +41,16 @@ public:
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void setVelocityTarget(btScalar velTarget)
{
m_desiredVelocity = velTarget;
}
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)
}
};
#endif //BT_MULTIBODY_JOINT_MOTOR_H

205
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h vendored
View File

@@ -24,6 +24,22 @@ enum btMultiBodyLinkFlags
{
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
};
//both defines are now permanently enabled
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
#define TEST_SPATIAL_ALGEBRA_LAYER
//
// Various spatial helper functions
//
//namespace {
#include "LinearMath/btSpatialAlgebra.h"
//}
//
// Link struct
//
@@ -33,75 +49,168 @@ struct btMultibodyLink
BT_DECLARE_ALIGNED_ALLOCATOR();
btScalar joint_pos; // qi
btScalar m_mass; // mass of link
btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
btScalar mass; // mass of link
btVector3 inertia; // inertia of link (local frame; diagonal)
int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
int parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
btQuaternion zero_rot_parent_to_this; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant.
//this is set to zero for planar joint (see also m_eVector comment)
// m_eVector is constant, but depends on the joint type:
// revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
// todo: fix the planar so it is consistent with the other joints
btVector3 m_eVector;
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
// for prismatic: axis_top = zero;
// axis_bottom = unit vector along the joint axis.
// for revolute: axis_top = unit vector along the rotation axis (u);
// axis_bottom = u cross d_vector.
btVector3 axis_top;
btVector3 axis_bottom;
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
btVector3 d_vector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only.
enum eFeatherstoneJointType
{
eRevolute = 0,
ePrismatic = 1,
eSpherical = 2,
ePlanar = 3,
eFixed = 4,
eInvalid
};
// e_vector is constant, but depends on the joint type
// prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
// revolute: vector from parent's COM to the pivot point, in PARENT's frame.
btVector3 e_vector;
bool is_revolute; // true = revolute, false = prismatic
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
// for prismatic: m_axesTop[0] = zero;
// m_axesBottom[0] = unit vector along the joint axis.
// for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
// m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
//
// for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
// m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
//
// for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
// m_axesTop[1][2] = zero
// m_axesBottom[0] = zero
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
btSpatialMotionVector m_axes[6];
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); }
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
btQuaternion cached_rot_parent_to_this; // rotates vectors in parent frame to vectors in local frame
btVector3 cached_r_vector; // vector from COM of parent to COM of this link, in local frame.
int m_dofOffset, m_cfgOffset;
btVector3 applied_force; // In WORLD frame
btVector3 applied_torque; // In WORLD frame
btScalar joint_torque;
btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame
btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame.
btVector3 m_appliedForce; // In WORLD frame
btVector3 m_appliedTorque; // In WORLD frame
btVector3 m_appliedConstraintForce; // In WORLD frame
btVector3 m_appliedConstraintTorque; // In WORLD frame
btScalar m_jointPos[7];
//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
//It gets set to zero after each internal stepSimulation call
btScalar m_jointTorque[6];
class btMultiBodyLinkCollider* m_collider;
int m_flags;
int m_dofCount, m_posVarCount; //redundant but handy
eFeatherstoneJointType m_jointType;
struct btMultiBodyJointFeedback* m_jointFeedback;
btTransform m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
// ctor: set some sensible defaults
btMultibodyLink()
: joint_pos(0),
mass(1),
parent(-1),
zero_rot_parent_to_this(1, 0, 0, 0),
is_revolute(false),
cached_rot_parent_to_this(1, 0, 0, 0),
joint_torque(0),
: m_mass(1),
m_parent(-1),
m_zeroRotParentToThis(0, 0, 0, 1),
m_cachedRotParentToThis(0, 0, 0, 1),
m_collider(0),
m_flags(0)
m_flags(0),
m_dofCount(0),
m_posVarCount(0),
m_jointType(btMultibodyLink::eInvalid),
m_jointFeedback(0),
m_linkName(0),
m_jointName(0)
{
inertia.setValue(1, 1, 1);
axis_top.setValue(0, 0, 0);
axis_bottom.setValue(1, 0, 0);
d_vector.setValue(0, 0, 0);
e_vector.setValue(0, 0, 0);
cached_r_vector.setValue(0, 0, 0);
applied_force.setValue( 0, 0, 0);
applied_torque.setValue(0, 0, 0);
m_inertiaLocal.setValue(1, 1, 1);
setAxisTop(0, 0., 0., 0.);
setAxisBottom(0, 1., 0., 0.);
m_dVector.setValue(0, 0, 0);
m_eVector.setValue(0, 0, 0);
m_cachedRVector.setValue(0, 0, 0);
m_appliedForce.setValue( 0, 0, 0);
m_appliedTorque.setValue(0, 0, 0);
//
m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
m_jointPos[3] = 1.f; //"quat.w"
m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
m_cachedWorldTransform.setIdentity();
}
// routine to update cached_rot_parent_to_this and cached_r_vector
void updateCache()
// routine to update m_cachedRotParentToThis and m_cachedRVector
void updateCacheMultiDof(btScalar *pq = 0)
{
if (is_revolute)
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
switch(m_jointType)
{
cached_rot_parent_to_this = btQuaternion(axis_top,-joint_pos) * zero_rot_parent_to_this;
cached_r_vector = d_vector + quatRotate(cached_rot_parent_to_this,e_vector);
} else
{
// cached_rot_parent_to_this never changes, so no need to update
cached_r_vector = e_vector + joint_pos * axis_bottom;
case eRevolute:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
case ePrismatic:
{
// m_cachedRotParentToThis never changes, so no need to update
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
break;
}
case eSpherical:
{
m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
case ePlanar:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
case eFixed:
{
m_cachedRotParentToThis = m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
default:
{
//invalid type
btAssert(0);
}
}
}
};

4
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h vendored
View File

@@ -74,14 +74,14 @@ public:
if (m_link>=0)
{
const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.parent == other->m_link)
if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link)
return false;
}
if (other->m_link>=0)
{
const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.parent == this->m_link)
if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link)
return false;
}
return true;

120
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp vendored
View File

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
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.
@@ -18,25 +18,39 @@ subject to the following restrictions:
#include "btMultiBodyPoint2Point.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btIDebugDraw.h"
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
#define BTMBP2PCONSTRAINT_DIM 3
#else
#define BTMBP2PCONSTRAINT_DIM 6
#endif
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(body,0,link,-1,3,false),
:btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB)
{
m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
}
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,3,false),
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB)
{
m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
}
void btMultiBodyPoint2Point::finalizeMultiDof()
{
//not implemented yet
btAssert(0);
}
btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
{
@@ -90,25 +104,37 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
{
// int i=1;
for (int i=0;i<3;i++)
int numDim = BTMBP2PCONSTRAINT_DIM;
for (int i=0;i<numDim;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
//memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = i;
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal1.setValue(0,0,0);
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal2.setValue(0,0,0);
constraintRow.m_angularComponentA.setValue(0,0,0);
constraintRow.m_angularComponentB.setValue(0,0,0);
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
btVector3 contactNormalOnB(0,0,0);
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
contactNormalOnB[i] = -1;
#else
contactNormalOnB[i%3] = -1;
#endif
btScalar penetration = 0;
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
if (m_rigidBodyA)
{
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
} else
@@ -125,19 +151,71 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
{
if (m_bodyB)
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
}
btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
btScalar relaxation = 1.f;
fillMultiBodyConstraintMixed(constraintRow, data,
contactNormalOnB,
pivotAworld, pivotBworld,
position,
infoGlobal,
relaxation,
false);
constraintRow.m_lowerLimit = -m_maxAppliedImpulse;
constraintRow.m_upperLimit = m_maxAppliedImpulse;
}
btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
fillMultiBodyConstraint(constraintRow, data, 0, 0,
contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
//@todo: support the case of btMultiBody versus btRigidBody,
//see btPoint2PointConstraint::getInfo2NonVirtual
#else
const btVector3 dummy(0, 0, 0);
btAssert(m_bodyA->isMultiDof());
btScalar* jac1 = jacobianA(i);
const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
fillMultiBodyConstraint(constraintRow, data, jac1, 0,
dummy, dummy, dummy, //sucks but let it be this way "for the time being"
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
#endif
}
}
void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
{
btTransform tr;
tr.setIdentity();
if (m_rigidBodyA)
{
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyA)
{
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
tr.setOrigin(pivotAworld);
drawer->drawTransform(tr, 0.1);
}
if (m_rigidBodyB)
{
// that ideally should draw the same frame
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyB)
{
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
tr.setOrigin(pivotBworld);
drawer->drawTransform(tr, 0.1);
}
}

13
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h vendored
View File

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
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.
@@ -20,6 +20,8 @@ subject to the following restrictions:
#include "btMultiBodyConstraint.h"
//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
class btMultiBodyPoint2Point : public btMultiBodyConstraint
{
protected:
@@ -28,7 +30,7 @@ protected:
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
public:
@@ -37,6 +39,8 @@ public:
virtual ~btMultiBodyPoint2Point();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
@@ -54,7 +58,8 @@ public:
m_pivotInB = pivotInB;
}
virtual void debugDraw(class btIDebugDraw* drawer);
};
#endif //BT_MULTIBODY_POINT2POINT_H

16
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h vendored
View File

@@ -20,6 +20,7 @@ subject to the following restrictions:
#include "LinearMath/btAlignedObjectArray.h"
class btMultiBody;
class btMultiBodyConstraint;
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
@@ -28,16 +29,19 @@ ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_orgConstraint(0), m_orgDofIndex(-1)
{}
int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
btVector3 m_relpos1CrossNormal;
btVector3 m_contactNormal1;
int m_jacAindex;
int m_deltaVelBindex;
int m_jacBindex;
btVector3 m_relpos1CrossNormal;
btVector3 m_contactNormal1;
btVector3 m_relpos2CrossNormal;
btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
int m_jacBindex;
btVector3 m_angularComponentA;
btVector3 m_angularComponentB;
@@ -70,6 +74,10 @@ ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
btMultiBody* m_multiBodyB;
int m_linkB;
//for writing back applied impulses
btMultiBodyConstraint* m_orgConstraint;
int m_orgDofIndex;
enum btSolverConstraintType
{
BT_SOLVER_CONTACT_1D = 0,

17
extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp vendored
View File

@@ -821,14 +821,15 @@ void btSolveL1T (const btScalar *L, btScalar *B, int n, int lskip1)
/* declare variables - Z matrix, p and q vectors, etc */
btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex;
const btScalar *ell;
int lskip2,lskip3,i,j;
int lskip2,i,j;
// int lskip3;
/* special handling for L and B because we're solving L1 *transpose* */
L = L + (n-1)*(lskip1+1);
B = B + n-1;
lskip1 = -lskip1;
/* compute lskip values */
lskip2 = 2*lskip1;
lskip3 = 3*lskip1;
//lskip3 = 3*lskip1;
/* compute all 4 x 1 blocks of X */
for (i=0; i <= n-4; i+=4) {
/* compute all 4 x 1 block of X, from rows i..i+4-1 */
@@ -1199,9 +1200,9 @@ struct btLCP
int *const m_findex, *const m_p, *const m_C;
btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
btScalar *_lo, btScalar *_hi, btScalar *_L, btScalar *_d,
btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows);
bool *_state, int *_findex, int *p, int *c, btScalar **Arows);
int getNub() const { return m_nub; }
void transfer_i_to_C (int i);
void transfer_i_to_N (int i) { m_nN++; } // because we can assume C and N span 1:i-1
@@ -1224,9 +1225,9 @@ struct btLCP
btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
btScalar *_lo, btScalar *_hi, btScalar *_L, btScalar *_d,
btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows):
bool *_state, int *_findex, int *p, int *c, btScalar **Arows):
m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0),
# ifdef BTROWPTRS
m_A(Arows),
@@ -1234,8 +1235,8 @@ btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btSc
m_A(_Adata),
#endif
m_x(_x), m_b(_b), m_w(_w), m_lo(_lo), m_hi(_hi),
m_L(_L), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp),
m_state(_state), m_findex(_findex), m_p(_p), m_C(_C)
m_L(l), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp),
m_state(_state), m_findex(_findex), m_p(p), m_C(c)
{
{
btSetZero (m_x,m_n);

371
extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp vendored Normal file
View File

@@ -0,0 +1,371 @@
/* Copyright (C) 2004-2013 MBSim Development Team
Code was converted for the Bullet Continuous Collision Detection and Physics Library
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.
*/
//The original version is here
//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
//This file is re-distributed under the ZLib license, with permission of the original author
//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
//STL/std::vector replaced by btAlignedObjectArray
#include "btLemkeAlgorithm.h"
#undef BT_DEBUG_OSTREAM
#ifdef BT_DEBUG_OSTREAM
using namespace std;
#endif //BT_DEBUG_OSTREAM
btScalar btMachEps()
{
static bool calculated=false;
static btScalar machEps = btScalar(1.);
if (!calculated)
{
do {
machEps /= btScalar(2.0);
// If next epsilon yields 1, then break, because current
// epsilon is the machine epsilon.
}
while ((btScalar)(1.0 + (machEps/btScalar(2.0))) != btScalar(1.0));
// printf( "\nCalculated Machine epsilon: %G\n", machEps );
calculated=true;
}
return machEps;
}
btScalar btEpsRoot() {
static btScalar epsroot = 0.;
static bool alreadyCalculated = false;
if (!alreadyCalculated) {
epsroot = btSqrt(btMachEps());
alreadyCalculated = true;
}
return epsroot;
}
btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
{
steps = 0;
int dim = m_q.size();
#ifdef BT_DEBUG_OSTREAM
if(DEBUGLEVEL >= 1) {
cout << "Dimension = " << dim << endl;
}
#endif //BT_DEBUG_OSTREAM
btVectorXu solutionVector(2 * dim);
solutionVector.setZero();
//, INIT, 0.);
btMatrixXu ident(dim, dim);
ident.setIdentity();
#ifdef BT_DEBUG_OSTREAM
cout << m_M << std::endl;
#endif
btMatrixXu mNeg = m_M.negative();
btMatrixXu A(dim, 2 * dim + 2);
//
A.setSubMatrix(0, 0, dim - 1, dim - 1,ident);
A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1,mNeg);
A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f);
A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1,m_q);
#ifdef BT_DEBUG_OSTREAM
cout << A << std::endl;
#endif //BT_DEBUG_OSTREAM
// btVectorXu q_;
// q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
btAlignedObjectArray<int> basis;
//At first, all w-values are in the basis
for (int i = 0; i < dim; i++)
basis.push_back(i);
int pivotRowIndex = -1;
btScalar minValue = 1e30f;
bool greaterZero = true;
for (int i=0;i<dim;i++)
{
btScalar v =A(i,2*dim+1);
if (v<minValue)
{
minValue=v;
pivotRowIndex = i;
}
if (v<0)
greaterZero = false;
}
// int pivotRowIndex = q_.minIndex();//minIndex(q_); // first row is that with lowest q-value
int z0Row = pivotRowIndex; // remember the col of z0 for ending algorithm afterwards
int pivotColIndex = 2 * dim; // first col is that of z0
#ifdef BT_DEBUG_OSTREAM
if (DEBUGLEVEL >= 3)
{
// cout << "A: " << A << endl;
cout << "pivotRowIndex " << pivotRowIndex << endl;
cout << "pivotColIndex " << pivotColIndex << endl;
cout << "Basis: ";
for (int i = 0; i < basis.size(); i++)
cout << basis[i] << " ";
cout << endl;
}
#endif //BT_DEBUG_OSTREAM
if (!greaterZero)
{
if (maxloops == 0) {
maxloops = 100;
// maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now...
}
/*start looping*/
for(steps = 0; steps < maxloops; steps++) {
GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
#ifdef BT_DEBUG_OSTREAM
if (DEBUGLEVEL >= 3) {
// cout << "A: " << A << endl;
cout << "pivotRowIndex " << pivotRowIndex << endl;
cout << "pivotColIndex " << pivotColIndex << endl;
cout << "Basis: ";
for (int i = 0; i < basis.size(); i++)
cout << basis[i] << " ";
cout << endl;
}
#endif //BT_DEBUG_OSTREAM
int pivotColIndexOld = pivotColIndex;
/*find new column index */
if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value
pivotColIndex = basis[pivotRowIndex] + dim;
else
//else do it the other way round and get in the corresponding w-value
pivotColIndex = basis[pivotRowIndex] - dim;
/*the column becomes part of the basis*/
basis[pivotRowIndex] = pivotColIndexOld;
pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
if(z0Row == pivotRowIndex) { //if z0 leaves the basis the solution is found --> one last elimination step is necessary
GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
basis[pivotRowIndex] = pivotColIndex; //update basis
break;
}
}
#ifdef BT_DEBUG_OSTREAM
if(DEBUGLEVEL >= 1) {
cout << "Number of loops: " << steps << endl;
cout << "Number of maximal loops: " << maxloops << endl;
}
#endif //BT_DEBUG_OSTREAM
if(!validBasis(basis)) {
info = -1;
#ifdef BT_DEBUG_OSTREAM
if(DEBUGLEVEL >= 1)
cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
#endif //BT_DEBUG_OSTREAM
return solutionVector;
}
}
#ifdef BT_DEBUG_OSTREAM
if (DEBUGLEVEL >= 2) {
// cout << "A: " << A << endl;
cout << "pivotRowIndex " << pivotRowIndex << endl;
cout << "pivotColIndex " << pivotColIndex << endl;
}
#endif //BT_DEBUG_OSTREAM
for (int i = 0; i < basis.size(); i++)
{
solutionVector[basis[i]] = A(i,2*dim+1);//q_[i];
}
info = 0;
return solutionVector;
}
int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int & pivotColIndex) {
int RowIndex = 0;
int dim = A.rows();
btAlignedObjectArray<btVectorXu> Rows;
for (int row = 0; row < dim; row++)
{
btVectorXu vec(dim + 1);
vec.setZero();//, INIT, 0.)
Rows.push_back(vec);
btScalar a = A(row, pivotColIndex);
if (a > 0) {
Rows[row][0] = A(row, 2 * dim + 1) / a;
Rows[row][1] = A(row, 2 * dim) / a;
for (int j = 2; j < dim + 1; j++)
Rows[row][j] = A(row, j - 1) / a;
#ifdef BT_DEBUG_OSTREAM
// if (DEBUGLEVEL) {
// cout << "Rows(" << row << ") = " << Rows[row] << endl;
// }
#endif
}
}
for (int i = 0; i < Rows.size(); i++)
{
if (Rows[i].nrm2() > 0.) {
int j = 0;
for (; j < Rows.size(); j++)
{
if(i != j)
{
if(Rows[j].nrm2() > 0.)
{
btVectorXu test(dim + 1);
for (int ii=0;ii<dim+1;ii++)
{
test[ii] = Rows[j][ii] - Rows[i][ii];
}
//=Rows[j] - Rows[i]
if (! LexicographicPositive(test))
break;
}
}
}
if (j == Rows.size())
{
RowIndex += i;
break;
}
}
}
return RowIndex;
}
bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu & v)
{
int i = 0;
// if (DEBUGLEVEL)
// cout << "v " << v << endl;
while(i < v.size()-1 && fabs(v[i]) < btMachEps())
i++;
if (v[i] > 0)
return true;
return false;
}
void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)
{
btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
#ifdef BT_DEBUG_OSTREAM
cout << A << std::endl;
#endif
for (int i = 0; i < A.rows(); i++)
{
if (i != pivotRowIndex)
{
for (int j = 0; j < A.cols(); j++)
{
if (j != pivotColumnIndex)
{
btScalar v = A(i, j);
v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
A.setElem(i, j, v);
}
}
}
}
#ifdef BT_DEBUG_OSTREAM
cout << A << std::endl;
#endif //BT_DEBUG_OSTREAM
for (int i = 0; i < A.cols(); i++)
{
A.mulElem(pivotRowIndex, i,-a);
}
#ifdef BT_DEBUG_OSTREAM
cout << A << std::endl;
#endif //#ifdef BT_DEBUG_OSTREAM
for (int i = 0; i < A.rows(); i++)
{
if (i != pivotRowIndex)
{
A.setElem(i, pivotColumnIndex,0);
}
}
#ifdef BT_DEBUG_OSTREAM
cout << A << std::endl;
#endif //#ifdef BT_DEBUG_OSTREAM
}
bool btLemkeAlgorithm::greaterZero(const btVectorXu & vector)
{
bool isGreater = true;
for (int i = 0; i < vector.size(); i++) {
if (vector[i] < 0) {
isGreater = false;
break;
}
}
return isGreater;
}
bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis)
{
bool isValid = true;
for (int i = 0; i < basis.size(); i++) {
if (basis[i] >= basis.size() * 2) { //then z0 is in the base
isValid = false;
break;
}
}
return isValid;
}

Some files were not shown because too many files have changed in this diff Show More