diff options
| author | Brecht Van Lommel <brechtvanlommel@gmail.com> | 2016-01-17 23:35:32 +0300 |
|---|---|---|
| committer | Brecht Van Lommel <brechtvanlommel@gmail.com> | 2016-01-26 00:14:46 +0300 |
| commit | b64d5809e7e3b832e2a011869db68e70b4b4e6fc (patch) | |
| tree | aa4f6714da9f546eeee7dffed9236f9c8309524b /extern/bullet2 | |
| parent | 3c72e302e1eb25de43dd9d077f0c730cc02b5674 (diff) | |
Upgrade Bullet to version 2.83.
I tried to carefully preserve all patches since the last upgrade.
Improves T47195, cloth collision detection bug.
Differential Revision: https://developer.blender.org/D1739
Diffstat (limited to 'extern/bullet2')
133 files changed, 13922 insertions, 3805 deletions
diff --git a/extern/bullet2/CMakeLists.txt b/extern/bullet2/CMakeLists.txt index 2b2c18c0685..949a8b01bd2 100644 --- a/extern/bullet2/CMakeLists.txt +++ b/extern/bullet2/CMakeLists.txt @@ -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 diff --git a/extern/bullet2/patches/blender.patch b/extern/bullet2/patches/blender.patch new file mode 100644 index 00000000000..d8e52ec32e8 --- /dev/null +++ b/extern/bullet2/patches/blender.patch @@ -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; diff --git a/extern/bullet2/patches/convex_hull.patch b/extern/bullet2/patches/convex_hull.patch deleted file mode 100644 index 1b2978221fb..00000000000 --- a/extern/bullet2/patches/convex_hull.patch +++ /dev/null @@ -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; diff --git a/extern/bullet2/readme.txt b/extern/bullet2/readme.txt index 3b286afa579..ec99abf71cf 100644 --- a/extern/bullet2/readme.txt +++ b/extern/bullet2/readme.txt @@ -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 diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index 95443af5070..2ca20cdd8b8 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -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; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h index b64936844d5..db4e482f292 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -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]); } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index ae22dadc73a..ad69fcbd712 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -34,7 +34,6 @@ int gFindPairs =0; btHashedOverlappingPairCache::btHashedOverlappingPairCache(): m_overlapFilterCallback(0), - m_blockedForChanges(false), m_ghostPairCallback(0) { int initialAllocatedSize= 2; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index eee90e473a9..14614270476 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -94,7 +94,6 @@ class btHashedOverlappingPairCache : public btOverlappingPairCache { btBroadphasePairArray m_overlappingPairArray; btOverlapFilterCallback* m_overlapFilterCallback; - bool m_blockedForChanges; protected: diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 669d0b6b55e..3b6913c0e1e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -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 ; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index d0924100058..395df3a550f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -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.)), diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 89cad168210..c68402418f7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -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; + } + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index d739a2a08d7..c505ed5d508 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -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++) + btIDebugDraw::DefaultColors defaultColors = getDebugDrawer()->getDefaultColors(); + + if ( getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) { - 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++) + 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; + 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; - btVector3 minAabb2,maxAabb2; + 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); - } + 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); + 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); + } + } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index 0ac5563d06e..ec40c969f51 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -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. diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp new file mode 100644 index 00000000000..57eb81703e7 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp @@ -0,0 +1,1143 @@ +/* +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. +*/ + +#include "btCollisionWorldImporter.h" +#include "btBulletCollisionCommon.h" +#include "LinearMath/btSerializer.h" //for btBulletSerializedArrays definition + +#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT +#include "BulletCollision/Gimpact/btGImpactShape.h" +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT + +btCollisionWorldImporter::btCollisionWorldImporter(btCollisionWorld* world) +:m_collisionWorld(world), +m_verboseMode(0) +{ + +} + +btCollisionWorldImporter::~btCollisionWorldImporter() +{ +} + + + + + +bool btCollisionWorldImporter::convertAllObjects( btBulletSerializedArrays* arrays) +{ + + m_shapeMap.clear(); + m_bodyMap.clear(); + + int i; + + for (i=0;i<arrays->m_bvhsDouble.size();i++) + { + btOptimizedBvh* bvh = createOptimizedBvh(); + btQuantizedBvhDoubleData* bvhData = arrays->m_bvhsDouble[i]; + bvh->deSerializeDouble(*bvhData); + m_bvhMap.insert(arrays->m_bvhsDouble[i],bvh); + } + for (i=0;i<arrays->m_bvhsFloat.size();i++) + { + btOptimizedBvh* bvh = createOptimizedBvh(); + btQuantizedBvhFloatData* bvhData = arrays->m_bvhsFloat[i]; + bvh->deSerializeFloat(*bvhData); + m_bvhMap.insert(arrays->m_bvhsFloat[i],bvh); + } + + + + + + for (i=0;i<arrays->m_colShapeData.size();i++) + { + btCollisionShapeData* shapeData = arrays->m_colShapeData[i]; + btCollisionShape* shape = convertCollisionShape(shapeData); + if (shape) + { + // printf("shapeMap.insert(%x,%x)\n",shapeData,shape); + m_shapeMap.insert(shapeData,shape); + } + + if (shape&& shapeData->m_name) + { + char* newname = duplicateName(shapeData->m_name); + m_objectNameMap.insert(shape,newname); + m_nameShapeMap.insert(newname,shape); + } + } + + + for (i=0;i<arrays->m_collisionObjectDataDouble.size();i++) + { + btCollisionObjectDoubleData* colObjData = arrays->m_collisionObjectDataDouble[i]; + btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); + if (shapePtr && *shapePtr) + { + btTransform startTransform; + colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; + startTransform.deSerializeDouble(colObjData->m_worldTransform); + + btCollisionShape* shape = (btCollisionShape*)*shapePtr; + btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); + body->setFriction(btScalar(colObjData->m_friction)); + body->setRestitution(btScalar(colObjData->m_restitution)); + +#ifdef USE_INTERNAL_EDGE_UTILITY + if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; + if (trimesh->getTriangleInfoMap()) + { + body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + } + } +#endif //USE_INTERNAL_EDGE_UTILITY + m_bodyMap.insert(colObjData,body); + } else + { + printf("error: no shape found\n"); + } + } + for (i=0;i<arrays->m_collisionObjectDataFloat.size();i++) + { + btCollisionObjectFloatData* colObjData = arrays->m_collisionObjectDataFloat[i]; + btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); + if (shapePtr && *shapePtr) + { + btTransform startTransform; + colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; + startTransform.deSerializeFloat(colObjData->m_worldTransform); + + btCollisionShape* shape = (btCollisionShape*)*shapePtr; + btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); + +#ifdef USE_INTERNAL_EDGE_UTILITY + if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; + if (trimesh->getTriangleInfoMap()) + { + body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + } + } +#endif //USE_INTERNAL_EDGE_UTILITY + m_bodyMap.insert(colObjData,body); + } else + { + printf("error: no shape found\n"); + } + } + + return true; +} + + + +void btCollisionWorldImporter::deleteAllData() +{ + int i; + + for (i=0;i<m_allocatedCollisionObjects.size();i++) + { + if(m_collisionWorld) + m_collisionWorld->removeCollisionObject(m_allocatedCollisionObjects[i]); + delete m_allocatedCollisionObjects[i]; + } + + m_allocatedCollisionObjects.clear(); + + + for (i=0;i<m_allocatedCollisionShapes.size();i++) + { + delete m_allocatedCollisionShapes[i]; + } + m_allocatedCollisionShapes.clear(); + + + for (i=0;i<m_allocatedBvhs.size();i++) + { + delete m_allocatedBvhs[i]; + } + m_allocatedBvhs.clear(); + + for (i=0;i<m_allocatedTriangleInfoMaps.size();i++) + { + delete m_allocatedTriangleInfoMaps[i]; + } + m_allocatedTriangleInfoMaps.clear(); + for (i=0;i<m_allocatedTriangleIndexArrays.size();i++) + { + delete m_allocatedTriangleIndexArrays[i]; + } + m_allocatedTriangleIndexArrays.clear(); + for (i=0;i<m_allocatedNames.size();i++) + { + delete[] m_allocatedNames[i]; + } + m_allocatedNames.clear(); + + for (i=0;i<m_allocatedbtStridingMeshInterfaceDatas.size();i++) + { + btStridingMeshInterfaceData* curData = m_allocatedbtStridingMeshInterfaceDatas[i]; + + for(int a = 0;a < curData->m_numMeshParts;a++) + { + btMeshPartData* curPart = &curData->m_meshPartsPtr[a]; + if(curPart->m_vertices3f) + delete [] curPart->m_vertices3f; + + if(curPart->m_vertices3d) + delete [] curPart->m_vertices3d; + + if(curPart->m_indices32) + delete [] curPart->m_indices32; + + if(curPart->m_3indices16) + delete [] curPart->m_3indices16; + + if(curPart->m_indices16) + delete [] curPart->m_indices16; + + if (curPart->m_3indices8) + delete [] curPart->m_3indices8; + + } + delete [] curData->m_meshPartsPtr; + delete curData; + } + m_allocatedbtStridingMeshInterfaceDatas.clear(); + + for (i=0;i<m_indexArrays.size();i++) + { + btAlignedFree(m_indexArrays[i]); + } + m_indexArrays.clear(); + + for (i=0;i<m_shortIndexArrays.size();i++) + { + btAlignedFree(m_shortIndexArrays[i]); + } + m_shortIndexArrays.clear(); + + for (i=0;i<m_charIndexArrays.size();i++) + { + btAlignedFree(m_charIndexArrays[i]); + } + m_charIndexArrays.clear(); + + for (i=0;i<m_floatVertexArrays.size();i++) + { + btAlignedFree(m_floatVertexArrays[i]); + } + m_floatVertexArrays.clear(); + + for (i=0;i<m_doubleVertexArrays.size();i++) + { + btAlignedFree(m_doubleVertexArrays[i]); + } + m_doubleVertexArrays.clear(); + + +} + + + +btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionShapeData* shapeData ) +{ + btCollisionShape* shape = 0; + + switch (shapeData->m_shapeType) + { + case STATIC_PLANE_PROXYTYPE: + { + btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)shapeData; + btVector3 planeNormal,localScaling; + planeNormal.deSerializeFloat(planeData->m_planeNormal); + localScaling.deSerializeFloat(planeData->m_localScaling); + shape = createPlaneShape(planeNormal,planeData->m_planeConstant); + shape->setLocalScaling(localScaling); + + break; + } + case SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE: + { + btScaledTriangleMeshShapeData* scaledMesh = (btScaledTriangleMeshShapeData*) shapeData; + btCollisionShapeData* colShapeData = (btCollisionShapeData*) &scaledMesh->m_trimeshShapeData; + colShapeData->m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; + btCollisionShape* childShape = convertCollisionShape(colShapeData); + btBvhTriangleMeshShape* meshShape = (btBvhTriangleMeshShape*)childShape; + btVector3 localScaling; + localScaling.deSerializeFloat(scaledMesh->m_localScaling); + + shape = createScaledTrangleMeshShape(meshShape, localScaling); + break; + } +#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT + case GIMPACT_SHAPE_PROXYTYPE: + { + btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*) shapeData; + if (gimpactData->m_gimpactSubType == CONST_GIMPACT_TRIMESH_SHAPE) + { + btStridingMeshInterfaceData* interfaceData = createStridingMeshInterfaceData(&gimpactData->m_meshInterface); + btTriangleIndexVertexArray* meshInterface = createMeshInterface(*interfaceData); + + + btGImpactMeshShape* gimpactShape = createGimpactShape(meshInterface); + btVector3 localScaling; + localScaling.deSerializeFloat(gimpactData->m_localScaling); + gimpactShape->setLocalScaling(localScaling); + gimpactShape->setMargin(btScalar(gimpactData->m_collisionMargin)); + gimpactShape->updateBound(); + shape = gimpactShape; + } else + { + printf("unsupported gimpact sub type\n"); + } + break; + } +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT + //The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API + //so deal with this + case CAPSULE_SHAPE_PROXYTYPE: + { + btCapsuleShapeData* capData = (btCapsuleShapeData*)shapeData; + + + switch (capData->m_upAxis) + { + case 0: + { + shape = createCapsuleShapeX(1,1); + break; + } + case 1: + { + shape = createCapsuleShapeY(1,1); + break; + } + case 2: + { + shape = createCapsuleShapeZ(1,1); + break; + } + default: + { + printf("error: wrong up axis for btCapsuleShape\n"); + } + + + }; + if (shape) + { + btCapsuleShape* cap = (btCapsuleShape*) shape; + cap->deSerializeFloat(capData); + } + break; + } + case CYLINDER_SHAPE_PROXYTYPE: + case CONE_SHAPE_PROXYTYPE: + case BOX_SHAPE_PROXYTYPE: + case SPHERE_SHAPE_PROXYTYPE: + case MULTI_SPHERE_SHAPE_PROXYTYPE: + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData; + btVector3 implicitShapeDimensions; + implicitShapeDimensions.deSerializeFloat(bsd->m_implicitShapeDimensions); + btVector3 localScaling; + localScaling.deSerializeFloat(bsd->m_localScaling); + btVector3 margin(bsd->m_collisionMargin,bsd->m_collisionMargin,bsd->m_collisionMargin); + switch (shapeData->m_shapeType) + { + case BOX_SHAPE_PROXYTYPE: + { + btBoxShape* box= (btBoxShape*)createBoxShape(implicitShapeDimensions/localScaling+margin); + //box->initializePolyhedralFeatures(); + shape = box; + + break; + } + case SPHERE_SHAPE_PROXYTYPE: + { + shape = createSphereShape(implicitShapeDimensions.getX()); + break; + } + + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShapeData* cylData = (btCylinderShapeData*) shapeData; + btVector3 halfExtents = implicitShapeDimensions+margin; + switch (cylData->m_upAxis) + { + case 0: + { + shape = createCylinderShapeX(halfExtents.getY(),halfExtents.getX()); + break; + } + case 1: + { + shape = createCylinderShapeY(halfExtents.getX(),halfExtents.getY()); + break; + } + case 2: + { + shape = createCylinderShapeZ(halfExtents.getX(),halfExtents.getZ()); + break; + } + default: + { + printf("unknown Cylinder up axis\n"); + } + + }; + + + + break; + } + case CONE_SHAPE_PROXYTYPE: + { + btConeShapeData* conData = (btConeShapeData*) shapeData; + btVector3 halfExtents = implicitShapeDimensions;//+margin; + switch (conData->m_upIndex) + { + case 0: + { + shape = createConeShapeX(halfExtents.getY(),halfExtents.getX()); + break; + } + case 1: + { + shape = createConeShapeY(halfExtents.getX(),halfExtents.getY()); + break; + } + case 2: + { + shape = createConeShapeZ(halfExtents.getX(),halfExtents.getZ()); + break; + } + default: + { + printf("unknown Cone up axis\n"); + } + + }; + + + + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + btMultiSphereShapeData* mss = (btMultiSphereShapeData*)bsd; + int numSpheres = mss->m_localPositionArraySize; + + btAlignedObjectArray<btVector3> tmpPos; + btAlignedObjectArray<btScalar> radii; + radii.resize(numSpheres); + tmpPos.resize(numSpheres); + int i; + for ( i=0;i<numSpheres;i++) + { + tmpPos[i].deSerializeFloat(mss->m_localPositionArrayPtr[i].m_pos); + radii[i] = mss->m_localPositionArrayPtr[i].m_radius; + } + shape = createMultiSphereShape(&tmpPos[0],&radii[0],numSpheres); + break; + } + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + // int sz = sizeof(btConvexHullShapeData); + // int sz2 = sizeof(btConvexInternalShapeData); + // int sz3 = sizeof(btCollisionShapeData); + btConvexHullShapeData* convexData = (btConvexHullShapeData*)bsd; + int numPoints = convexData->m_numUnscaledPoints; + + btAlignedObjectArray<btVector3> tmpPoints; + tmpPoints.resize(numPoints); + int i; + for ( i=0;i<numPoints;i++) + { +#ifdef BT_USE_DOUBLE_PRECISION + if (convexData->m_unscaledPointsDoublePtr) + tmpPoints[i].deSerialize(convexData->m_unscaledPointsDoublePtr[i]); + if (convexData->m_unscaledPointsFloatPtr) + tmpPoints[i].deSerializeFloat(convexData->m_unscaledPointsFloatPtr[i]); +#else + if (convexData->m_unscaledPointsFloatPtr) + tmpPoints[i].deSerialize(convexData->m_unscaledPointsFloatPtr[i]); + if (convexData->m_unscaledPointsDoublePtr) + tmpPoints[i].deSerializeDouble(convexData->m_unscaledPointsDoublePtr[i]); +#endif //BT_USE_DOUBLE_PRECISION + } + btConvexHullShape* hullShape = createConvexHullShape(); + for (i=0;i<numPoints;i++) + { + hullShape->addPoint(tmpPoints[i]); + } + hullShape->setMargin(bsd->m_collisionMargin); + //hullShape->initializePolyhedralFeatures(); + shape = hullShape; + break; + } + default: + { + printf("error: cannot create shape type (%d)\n",shapeData->m_shapeType); + } + } + + if (shape) + { + shape->setMargin(bsd->m_collisionMargin); + + btVector3 localScaling; + localScaling.deSerializeFloat(bsd->m_localScaling); + shape->setLocalScaling(localScaling); + + } + break; + } + case TRIANGLE_MESH_SHAPE_PROXYTYPE: + { + btTriangleMeshShapeData* trimesh = (btTriangleMeshShapeData*)shapeData; + btStridingMeshInterfaceData* interfaceData = createStridingMeshInterfaceData(&trimesh->m_meshInterface); + btTriangleIndexVertexArray* meshInterface = createMeshInterface(*interfaceData); + if (!meshInterface->getNumSubParts()) + { + return 0; + } + + btVector3 scaling; scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling); + meshInterface->setScaling(scaling); + + + btOptimizedBvh* bvh = 0; +#if 1 + if (trimesh->m_quantizedFloatBvh) + { + btOptimizedBvh** bvhPtr = m_bvhMap.find(trimesh->m_quantizedFloatBvh); + if (bvhPtr && *bvhPtr) + { + bvh = *bvhPtr; + } else + { + bvh = createOptimizedBvh(); + bvh->deSerializeFloat(*trimesh->m_quantizedFloatBvh); + } + } + if (trimesh->m_quantizedDoubleBvh) + { + btOptimizedBvh** bvhPtr = m_bvhMap.find(trimesh->m_quantizedDoubleBvh); + if (bvhPtr && *bvhPtr) + { + bvh = *bvhPtr; + } else + { + bvh = createOptimizedBvh(); + bvh->deSerializeDouble(*trimesh->m_quantizedDoubleBvh); + } + } +#endif + + + btBvhTriangleMeshShape* trimeshShape = createBvhTriangleMeshShape(meshInterface,bvh); + trimeshShape->setMargin(trimesh->m_collisionMargin); + shape = trimeshShape; + + if (trimesh->m_triangleInfoMap) + { + btTriangleInfoMap* map = createTriangleInfoMap(); + map->deSerialize(*trimesh->m_triangleInfoMap); + trimeshShape->setTriangleInfoMap(map); + +#ifdef USE_INTERNAL_EDGE_UTILITY + gContactAddedCallback = btAdjustInternalEdgeContactsCallback; +#endif //USE_INTERNAL_EDGE_UTILITY + + } + + //printf("trimesh->m_collisionMargin=%f\n",trimesh->m_collisionMargin); + break; + } + case COMPOUND_SHAPE_PROXYTYPE: + { + btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData; + btCompoundShape* compoundShape = createCompoundShape(); + + + btAlignedObjectArray<btCollisionShape*> childShapes; + for (int i=0;i<compoundData->m_numChildShapes;i++) + { + btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape; + + btCollisionShape* childShape = convertCollisionShape(cd); + if (childShape) + { + btTransform localTransform; + localTransform.deSerializeFloat(compoundData->m_childShapePtr[i].m_transform); + compoundShape->addChildShape(localTransform,childShape); + } else + { +#ifdef _DEBUG + printf("error: couldn't create childShape for compoundShape\n"); +#endif + } + + } + shape = compoundShape; + + break; + } + case SOFTBODY_SHAPE_PROXYTYPE: + { + return 0; + } + default: + { +#ifdef _DEBUG + printf("unsupported shape type (%d)\n",shapeData->m_shapeType); +#endif + } + } + + return shape; + +} + + + +char* btCollisionWorldImporter::duplicateName(const char* name) +{ + if (name) + { + int l = (int)strlen(name); + char* newName = new char[l+1]; + memcpy(newName,name,l); + newName[l] = 0; + m_allocatedNames.push_back(newName); + return newName; + } + return 0; +} + + + + + + + + + + + +btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStridingMeshInterfaceData& meshData) +{ + btTriangleIndexVertexArray* meshInterface = createTriangleMeshContainer(); + + for (int i=0;i<meshData.m_numMeshParts;i++) + { + btIndexedMesh meshPart; + meshPart.m_numTriangles = meshData.m_meshPartsPtr[i].m_numTriangles; + meshPart.m_numVertices = meshData.m_meshPartsPtr[i].m_numVertices; + + + if (meshData.m_meshPartsPtr[i].m_indices32) + { + meshPart.m_indexType = PHY_INTEGER; + meshPart.m_triangleIndexStride = 3*sizeof(int); + int* indexArray = (int*)btAlignedAlloc(sizeof(int)*3*meshPart.m_numTriangles,16); + m_indexArrays.push_back(indexArray); + for (int j=0;j<3*meshPart.m_numTriangles;j++) + { + indexArray[j] = meshData.m_meshPartsPtr[i].m_indices32[j].m_value; + } + meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; + } else + { + if (meshData.m_meshPartsPtr[i].m_3indices16) + { + meshPart.m_indexType = PHY_SHORT; + meshPart.m_triangleIndexStride = sizeof(short int)*3;//sizeof(btShortIntIndexTripletData); + + short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int)*3*meshPart.m_numTriangles,16); + m_shortIndexArrays.push_back(indexArray); + + for (int j=0;j<meshPart.m_numTriangles;j++) + { + indexArray[3*j] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[0]; + indexArray[3*j+1] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[1]; + indexArray[3*j+2] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[2]; + } + + meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; + } + if (meshData.m_meshPartsPtr[i].m_indices16) + { + meshPart.m_indexType = PHY_SHORT; + meshPart.m_triangleIndexStride = 3*sizeof(short int); + short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int)*3*meshPart.m_numTriangles,16); + m_shortIndexArrays.push_back(indexArray); + for (int j=0;j<3*meshPart.m_numTriangles;j++) + { + indexArray[j] = meshData.m_meshPartsPtr[i].m_indices16[j].m_value; + } + + meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; + } + + if (meshData.m_meshPartsPtr[i].m_3indices8) + { + meshPart.m_indexType = PHY_UCHAR; + meshPart.m_triangleIndexStride = sizeof(unsigned char)*3; + + unsigned char* indexArray = (unsigned char*)btAlignedAlloc(sizeof(unsigned char)*3*meshPart.m_numTriangles,16); + m_charIndexArrays.push_back(indexArray); + + for (int j=0;j<meshPart.m_numTriangles;j++) + { + indexArray[3*j] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[0]; + indexArray[3*j+1] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[1]; + indexArray[3*j+2] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[2]; + } + + meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; + } + } + + if (meshData.m_meshPartsPtr[i].m_vertices3f) + { + meshPart.m_vertexType = PHY_FLOAT; + meshPart.m_vertexStride = sizeof(btVector3FloatData); + btVector3FloatData* vertices = (btVector3FloatData*) btAlignedAlloc(sizeof(btVector3FloatData)*meshPart.m_numVertices,16); + m_floatVertexArrays.push_back(vertices); + + for (int j=0;j<meshPart.m_numVertices;j++) + { + vertices[j].m_floats[0] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[0]; + vertices[j].m_floats[1] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[1]; + vertices[j].m_floats[2] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[2]; + vertices[j].m_floats[3] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[3]; + } + meshPart.m_vertexBase = (const unsigned char*)vertices; + } else + { + meshPart.m_vertexType = PHY_DOUBLE; + meshPart.m_vertexStride = sizeof(btVector3DoubleData); + + + btVector3DoubleData* vertices = (btVector3DoubleData*) btAlignedAlloc(sizeof(btVector3DoubleData)*meshPart.m_numVertices,16); + m_doubleVertexArrays.push_back(vertices); + + for (int j=0;j<meshPart.m_numVertices;j++) + { + vertices[j].m_floats[0] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[0]; + vertices[j].m_floats[1] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[1]; + vertices[j].m_floats[2] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[2]; + vertices[j].m_floats[3] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[3]; + } + meshPart.m_vertexBase = (const unsigned char*)vertices; + } + + if (meshPart.m_triangleIndexBase && meshPart.m_vertexBase) + { + meshInterface->addIndexedMesh(meshPart,meshPart.m_indexType); + } + } + + return meshInterface; +} + + +btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData) +{ + //create a new btStridingMeshInterfaceData that is an exact copy of shapedata and store it in the WorldImporter + btStridingMeshInterfaceData* newData = new btStridingMeshInterfaceData; + + newData->m_scaling = interfaceData->m_scaling; + newData->m_numMeshParts = interfaceData->m_numMeshParts; + newData->m_meshPartsPtr = new btMeshPartData[newData->m_numMeshParts]; + + for(int i = 0;i < newData->m_numMeshParts;i++) + { + btMeshPartData* curPart = &interfaceData->m_meshPartsPtr[i]; + btMeshPartData* curNewPart = &newData->m_meshPartsPtr[i]; + + curNewPart->m_numTriangles = curPart->m_numTriangles; + curNewPart->m_numVertices = curPart->m_numVertices; + + if(curPart->m_vertices3f) + { + curNewPart->m_vertices3f = new btVector3FloatData[curNewPart->m_numVertices]; + memcpy(curNewPart->m_vertices3f,curPart->m_vertices3f,sizeof(btVector3FloatData) * curNewPart->m_numVertices); + } + else + curNewPart->m_vertices3f = NULL; + + if(curPart->m_vertices3d) + { + curNewPart->m_vertices3d = new btVector3DoubleData[curNewPart->m_numVertices]; + memcpy(curNewPart->m_vertices3d,curPart->m_vertices3d,sizeof(btVector3DoubleData) * curNewPart->m_numVertices); + } + else + curNewPart->m_vertices3d = NULL; + + int numIndices = curNewPart->m_numTriangles * 3; + ///the m_3indices8 was not initialized in some Bullet versions, this can cause crashes at loading time + ///we catch it by only dealing with m_3indices8 if none of the other indices are initialized + bool uninitialized3indices8Workaround =false; + + if(curPart->m_indices32) + { + uninitialized3indices8Workaround=true; + curNewPart->m_indices32 = new btIntIndexData[numIndices]; + memcpy(curNewPart->m_indices32,curPart->m_indices32,sizeof(btIntIndexData) * numIndices); + } + else + curNewPart->m_indices32 = NULL; + + if(curPart->m_3indices16) + { + uninitialized3indices8Workaround=true; + curNewPart->m_3indices16 = new btShortIntIndexTripletData[curNewPart->m_numTriangles]; + memcpy(curNewPart->m_3indices16,curPart->m_3indices16,sizeof(btShortIntIndexTripletData) * curNewPart->m_numTriangles); + } + else + curNewPart->m_3indices16 = NULL; + + if(curPart->m_indices16) + { + uninitialized3indices8Workaround=true; + curNewPart->m_indices16 = new btShortIntIndexData[numIndices]; + memcpy(curNewPart->m_indices16,curPart->m_indices16,sizeof(btShortIntIndexData) * numIndices); + } + else + curNewPart->m_indices16 = NULL; + + if(!uninitialized3indices8Workaround && curPart->m_3indices8) + { + curNewPart->m_3indices8 = new btCharIndexTripletData[curNewPart->m_numTriangles]; + memcpy(curNewPart->m_3indices8,curPart->m_3indices8,sizeof(btCharIndexTripletData) * curNewPart->m_numTriangles); + } + else + curNewPart->m_3indices8 = NULL; + + } + + m_allocatedbtStridingMeshInterfaceDatas.push_back(newData); + + return(newData); +} + +#ifdef USE_INTERNAL_EDGE_UTILITY +extern ContactAddedCallback gContactAddedCallback; + +static bool btAdjustInternalEdgeContactsCallback(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1) +{ + + btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1); + //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_BACKFACE_MODE); + //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_DOUBLE_SIDED+BT_TRIANGLE_CONCAVE_DOUBLE_SIDED); + return true; +} +#endif //USE_INTERNAL_EDGE_UTILITY + + +/* +btRigidBody* btWorldImporter::createRigidBody(bool isDynamic, btScalar mass, const btTransform& startTransform,btCollisionShape* shape,const char* bodyName) +{ + btVector3 localInertia; + localInertia.setZero(); + + if (mass) + shape->calculateLocalInertia(mass,localInertia); + + btRigidBody* body = new btRigidBody(mass,0,shape,localInertia); + body->setWorldTransform(startTransform); + + if (m_dynamicsWorld) + m_dynamicsWorld->addRigidBody(body); + + if (bodyName) + { + char* newname = duplicateName(bodyName); + m_objectNameMap.insert(body,newname); + m_nameBodyMap.insert(newname,body); + } + m_allocatedRigidBodies.push_back(body); + return body; + +} +*/ + +btCollisionObject* btCollisionWorldImporter::getCollisionObjectByName(const char* name) +{ + btCollisionObject** bodyPtr = m_nameColObjMap.find(name); + if (bodyPtr && *bodyPtr) + { + return *bodyPtr; + } + return 0; +} + +btCollisionObject* btCollisionWorldImporter::createCollisionObject(const btTransform& startTransform,btCollisionShape* shape, const char* bodyName) +{ + btCollisionObject* colObj = new btCollisionObject(); + colObj->setWorldTransform(startTransform); + colObj->setCollisionShape(shape); + m_collisionWorld->addCollisionObject(colObj);//todo: flags etc + + if (bodyName) + { + char* newname = duplicateName(bodyName); + m_objectNameMap.insert(colObj,newname); + m_nameColObjMap.insert(newname,colObj); + } + m_allocatedCollisionObjects.push_back(colObj); + + return colObj; +} + + + +btCollisionShape* btCollisionWorldImporter::createPlaneShape(const btVector3& planeNormal,btScalar planeConstant) +{ + btStaticPlaneShape* shape = new btStaticPlaneShape(planeNormal,planeConstant); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} +btCollisionShape* btCollisionWorldImporter::createBoxShape(const btVector3& halfExtents) +{ + btBoxShape* shape = new btBoxShape(halfExtents); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} +btCollisionShape* btCollisionWorldImporter::createSphereShape(btScalar radius) +{ + btSphereShape* shape = new btSphereShape(radius); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + + +btCollisionShape* btCollisionWorldImporter::createCapsuleShapeX(btScalar radius, btScalar height) +{ + btCapsuleShapeX* shape = new btCapsuleShapeX(radius,height); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createCapsuleShapeY(btScalar radius, btScalar height) +{ + btCapsuleShape* shape = new btCapsuleShape(radius,height); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createCapsuleShapeZ(btScalar radius, btScalar height) +{ + btCapsuleShapeZ* shape = new btCapsuleShapeZ(radius,height); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createCylinderShapeX(btScalar radius,btScalar height) +{ + btCylinderShapeX* shape = new btCylinderShapeX(btVector3(height,radius,radius)); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createCylinderShapeY(btScalar radius,btScalar height) +{ + btCylinderShape* shape = new btCylinderShape(btVector3(radius,height,radius)); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createCylinderShapeZ(btScalar radius,btScalar height) +{ + btCylinderShapeZ* shape = new btCylinderShapeZ(btVector3(radius,radius,height)); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createConeShapeX(btScalar radius,btScalar height) +{ + btConeShapeX* shape = new btConeShapeX(radius,height); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createConeShapeY(btScalar radius,btScalar height) +{ + btConeShape* shape = new btConeShape(radius,height); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createConeShapeZ(btScalar radius,btScalar height) +{ + btConeShapeZ* shape = new btConeShapeZ(radius,height); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btTriangleIndexVertexArray* btCollisionWorldImporter::createTriangleMeshContainer() +{ + btTriangleIndexVertexArray* in = new btTriangleIndexVertexArray(); + m_allocatedTriangleIndexArrays.push_back(in); + return in; +} + +btOptimizedBvh* btCollisionWorldImporter::createOptimizedBvh() +{ + btOptimizedBvh* bvh = new btOptimizedBvh(); + m_allocatedBvhs.push_back(bvh); + return bvh; +} + + +btTriangleInfoMap* btCollisionWorldImporter::createTriangleInfoMap() +{ + btTriangleInfoMap* tim = new btTriangleInfoMap(); + m_allocatedTriangleInfoMaps.push_back(tim); + return tim; +} + +btBvhTriangleMeshShape* btCollisionWorldImporter::createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh) +{ + if (bvh) + { + btBvhTriangleMeshShape* bvhTriMesh = new btBvhTriangleMeshShape(trimesh,bvh->isQuantized(), false); + bvhTriMesh->setOptimizedBvh(bvh); + m_allocatedCollisionShapes.push_back(bvhTriMesh); + return bvhTriMesh; + } + + btBvhTriangleMeshShape* ts = new btBvhTriangleMeshShape(trimesh,true); + m_allocatedCollisionShapes.push_back(ts); + return ts; + +} +btCollisionShape* btCollisionWorldImporter::createConvexTriangleMeshShape(btStridingMeshInterface* trimesh) +{ + return 0; +} +#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT +btGImpactMeshShape* btCollisionWorldImporter::createGimpactShape(btStridingMeshInterface* trimesh) +{ + btGImpactMeshShape* shape = new btGImpactMeshShape(trimesh); + m_allocatedCollisionShapes.push_back(shape); + return shape; + +} +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT + +btConvexHullShape* btCollisionWorldImporter::createConvexHullShape() +{ + btConvexHullShape* shape = new btConvexHullShape(); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCompoundShape* btCollisionWorldImporter::createCompoundShape() +{ + btCompoundShape* shape = new btCompoundShape(); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + + +btScaledBvhTriangleMeshShape* btCollisionWorldImporter::createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScaling) +{ + btScaledBvhTriangleMeshShape* shape = new btScaledBvhTriangleMeshShape(meshShape,localScaling); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btMultiSphereShape* btCollisionWorldImporter::createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres) +{ + btMultiSphereShape* shape = new btMultiSphereShape(positions, radi, numSpheres); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + + + + // query for data +int btCollisionWorldImporter::getNumCollisionShapes() const +{ + return m_allocatedCollisionShapes.size(); +} + +btCollisionShape* btCollisionWorldImporter::getCollisionShapeByIndex(int index) +{ + return m_allocatedCollisionShapes[index]; +} + +btCollisionShape* btCollisionWorldImporter::getCollisionShapeByName(const char* name) +{ + btCollisionShape** shapePtr = m_nameShapeMap.find(name); + if (shapePtr&& *shapePtr) + { + return *shapePtr; + } + return 0; +} + + +const char* btCollisionWorldImporter::getNameForPointer(const void* ptr) const +{ + const char*const * namePtr = m_objectNameMap.find(ptr); + if (namePtr && *namePtr) + return *namePtr; + return 0; +} + + +int btCollisionWorldImporter::getNumRigidBodies() const +{ + return m_allocatedRigidBodies.size(); +} + +btCollisionObject* btCollisionWorldImporter::getRigidBodyByIndex(int index) const +{ + return m_allocatedRigidBodies[index]; +} + + +int btCollisionWorldImporter::getNumBvhs() const +{ + return m_allocatedBvhs.size(); +} + btOptimizedBvh* btCollisionWorldImporter::getBvhByIndex(int index) const +{ + return m_allocatedBvhs[index]; +} + +int btCollisionWorldImporter::getNumTriangleInfoMaps() const +{ + return m_allocatedTriangleInfoMaps.size(); +} + +btTriangleInfoMap* btCollisionWorldImporter::getTriangleInfoMapByIndex(int index) const +{ + return m_allocatedTriangleInfoMaps[index]; +} + + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h new file mode 100644 index 00000000000..9a6d16fbea7 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h @@ -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 diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 286e6c31f2f..a80c438d12a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -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 ; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp index 95780fb2d27..1d64d84b87b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -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) { } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp index 4ec9ae71386..1cb3d2e7a14 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp @@ -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; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h index 18d9385a180..24d13367786 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h @@ -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); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index e23f5f7a88d..912a5285568 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -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); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index c3cacec4a4f..d42f00a637f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -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) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp index cfcca5654de..8c8a7c3c1e3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp @@ -28,9 +28,7 @@ int gFindSimplePairs =0; -btHashedSimplePairCache::btHashedSimplePairCache(): - m_blockedForChanges(false) -{ +btHashedSimplePairCache::btHashedSimplePairCache() { int initialAllocatedSize= 2; m_overlappingPairArray.reserve(initialAllocatedSize); growTables(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h index e88ef97e968..186964d723e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -55,9 +55,7 @@ extern int gFindSimplePairs; class btHashedSimplePairCache { btSimplePairArray m_overlappingPairArray; - - bool m_blockedForChanges; - + protected: diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index 73fa4e87ea4..6cba442ca5d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -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; - - } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index 493d635539e..1fa4995d160 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -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: diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h index 7578bb258df..f8c55ace4eb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -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 diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h index ff017a20671..5e86568005e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -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; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index 0aa75f2bff3..e8c8c336cd2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -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) ); } } } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index 141034a8e8c..4eef8dba306 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -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(); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 02ea5033b15..0623e351a97 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -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" diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index f4324c1f401..4f45319a83a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -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; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp index fb0d9fdb75a..b56d72917de 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -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; } } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h index 290cd9fd13c..875f2ac195a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -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 diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 26322791d04..441a89c6bb6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -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 diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index 6abfdffa677..a7362ea01f4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -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" diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h index 2b92ab7d1b7..5ebaede4a88 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h @@ -25,7 +25,6 @@ subject to the following restrictions: ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape { btAlignedObjectArray <btMaterial*> m_materialList; - int ** m_triangleMaterials; public: diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 9095c592d87..4854f370f73 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -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" diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp index 38ef8f03706..d17141e3f20 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp @@ -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.)) ); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp index 5fbed334ec1..e4de7320930 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp @@ -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) { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h index 0afc2321ff0..ac4afa7f6b2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -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); diff --git a/extern/bullet2/src/BulletCollision/Doxyfile b/extern/bullet2/src/BulletCollision/Doxyfile deleted file mode 100644 index 4ecb6acb62f..00000000000 --- a/extern/bullet2/src/BulletCollision/Doxyfile +++ /dev/null @@ -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 = diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h index 915277404d6..d98051da3d6 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h @@ -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)); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h new file mode 100644 index 00000000000..9eb880b8dfc --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h @@ -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 diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h new file mode 100644 index 00000000000..0b49b0ecc65 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h @@ -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 + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h new file mode 100644 index 00000000000..ce1f24bc50d --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h @@ -0,0 +1,1035 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2014 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. +*/ + +/* +Initial GJK-EPA collision solver by Nathanael Presson, 2008 +Improvements and refactoring by Erwin Coumans, 2008-2014 +*/ +#ifndef BT_GJK_EPA3_H +#define BT_GJK_EPA3_H + +#include "LinearMath/btTransform.h" +#include "btGjkCollisionDescription.h" + + + +struct btGjkEpaSolver3 +{ +struct sResults + { + enum eStatus + { + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } status; + btVector3 witnesses[2]; + btVector3 normal; + btScalar distance; + }; + + +}; + + + +#if defined(DEBUG) || defined (_DEBUG) +#include <stdio.h> //for debug printf +#ifdef __SPU__ +#include <spu_printf.h> +#define printf spu_printf +#endif //__SPU__ +#endif + + + + // Config + + /* GJK */ +#define GJK_MAX_ITERATIONS 128 +#define GJK_ACCURARY ((btScalar)0.0001) +#define GJK_MIN_DISTANCE ((btScalar)0.0001) +#define GJK_DUPLICATED_EPS ((btScalar)0.0001) +#define GJK_SIMPLEX2_EPS ((btScalar)0.0) +#define GJK_SIMPLEX3_EPS ((btScalar)0.0) +#define GJK_SIMPLEX4_EPS ((btScalar)0.0) + + /* EPA */ +#define EPA_MAX_VERTICES 64 +#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) +#define EPA_MAX_ITERATIONS 255 +#define EPA_ACCURACY ((btScalar)0.0001) +#define EPA_FALLBACK (10*EPA_ACCURACY) +#define EPA_PLANE_EPS ((btScalar)0.00001) +#define EPA_INSIDE_EPS ((btScalar)0.01) + + + // Shorthands + typedef unsigned int U; + typedef unsigned char U1; + + // MinkowskiDiff + template <typename btConvexTemplate> + struct MinkowskiDiff + { + const btConvexTemplate* m_convexAPtr; + const btConvexTemplate* m_convexBPtr; + + btMatrix3x3 m_toshape1; + btTransform m_toshape0; + + bool m_enableMargin; + + + MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b) + :m_convexAPtr(&a), + m_convexBPtr(&b) + { + } + + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline btVector3 Support0(const btVector3& d) const + { + return m_convexAPtr->getLocalSupportWithMargin(d); + } + inline btVector3 Support1(const btVector3& d) const + { + return m_toshape0*m_convexBPtr->getLocalSupportWithMargin(m_toshape1*d); + } + + + inline btVector3 Support(const btVector3& d) const + { + return(Support0(d)-Support1(-d)); + } + btVector3 Support(const btVector3& d,U index) const + { + if(index) + return(Support1(d)); + else + return(Support0(d)); + } + }; + +enum eGjkStatus +{ + eGjkValid, + eGjkInside, + eGjkFailed +}; + + // GJK + template <typename btConvexTemplate> + struct GJK + { + /* Types */ + struct sSV + { + btVector3 d,w; + }; + struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; + + /* Fields */ + + MinkowskiDiff<btConvexTemplate> m_shape; + btVector3 m_ray; + btScalar m_distance; + sSimplex m_simplices[2]; + sSV m_store[4]; + sSV* m_free[4]; + U m_nfree; + U m_current; + sSimplex* m_simplex; + eGjkStatus m_status; + /* Methods */ + + GJK(const btConvexTemplate& a, const btConvexTemplate& b) + :m_shape(a,b) + { + Initialize(); + } + void Initialize() + { + m_ray = btVector3(0,0,0); + m_nfree = 0; + m_status = eGjkFailed; + m_current = 0; + m_distance = 0; + } + eGjkStatus Evaluate(const MinkowskiDiff<btConvexTemplate>& shapearg,const btVector3& guess) + { + U iterations=0; + btScalar sqdist=0; + btScalar alpha=0; + btVector3 lastw[4]; + U clastw=0; + /* Initialize solver */ + m_free[0] = &m_store[0]; + m_free[1] = &m_store[1]; + m_free[2] = &m_store[2]; + m_free[3] = &m_store[3]; + m_nfree = 4; + m_current = 0; + m_status = eGjkValid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const btScalar sqrl= m_ray.length2(); + appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do { + const U next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const btScalar rl=m_ray.length(); + if(rl<GJK_MIN_DISTANCE) + {/* Touching or inside */ + m_status=eGjkInside; + break; + } + /* Append new vertice in -'v' direction */ + appendvertice(cs,-m_ray); + const btVector3& w=cs.c[cs.rank-1]->w; + bool found=false; + for(U i=0;i<4;++i) + { + if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS) + { found=true;break; } + } + if(found) + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + else + {/* Update lastw */ + lastw[clastw=(clastw+1)&3]=w; + } + /* Check for termination */ + const btScalar omega=btDot(m_ray,w)/rl; + alpha=btMax(omega,alpha); + if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + /* Reduce simplex */ + btScalar weights[4]; + U mask=0; + switch(cs.rank) + { + case 2: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + weights,mask);break; + case 3: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights,mask);break; + case 4: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights,mask);break; + } + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = btVector3(0,0,0); + m_current = next; + for(U i=0,ni=cs.rank;i<ni;++i) + { + if(mask&(1<<i)) + { + ns.c[ns.rank] = cs.c[i]; + ns.p[ns.rank++] = weights[i]; + m_ray += cs.c[i]->w*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) m_status=eGjkInside; + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eGjkFailed; + } while(m_status==eGjkValid); + m_simplex=&m_simplices[m_current]; + switch(m_status) + { + case eGjkValid: m_distance=m_ray.length();break; + case eGjkInside: m_distance=0;break; + default: + { + } + } + return(m_status); + } + bool EncloseOrigin() + { + switch(m_simplex->rank) + { + case 1: + { + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + appendvertice(*m_simplex, axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + const btVector3 p=btCross(d,axis); + if(p.length2()>0) + { + appendvertice(*m_simplex, p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w, + m_simplex->c[2]->w-m_simplex->c[0]->w); + if(n.length2()>0) + { + appendvertice(*m_simplex,n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 4: + { + if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, + m_simplex->c[1]->w-m_simplex->c[3]->w, + m_simplex->c[2]->w-m_simplex->c[3]->w))>0) + return(true); + } + break; + } + return(false); + } + /* Internals */ + void getsupport(const btVector3& d,sSV& sv) const + { + sv.d = d/d.length(); + sv.w = m_shape.Support(sv.d); + } + void removevertice(sSimplex& simplex) + { + m_free[m_nfree++]=simplex.c[--simplex.rank]; + } + void appendvertice(sSimplex& simplex,const btVector3& v) + { + simplex.p[simplex.rank]=0; + simplex.c[simplex.rank]=m_free[--m_nfree]; + getsupport(v,*simplex.c[simplex.rank++]); + } + static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) + { + return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- + a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ + a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); + } + static btScalar projectorigin( const btVector3& a, + const btVector3& b, + btScalar* w,U& m) + { + const btVector3 d=b-a; + const btScalar l=d.length2(); + if(l>GJK_SIMPLEX2_EPS) + { + const btScalar t(l>0?-btDot(a,d)/l:0); + if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } + else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } + else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } + } + return(-1); + } + static btScalar projectorigin( const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar* w,U& m) + { + static const U imd3[]={1,2,0}; + const btVector3* vt[]={&a,&b,&c}; + const btVector3 dl[]={a-b,b-c,c-a}; + const btVector3 n=btCross(dl[0],dl[1]); + const btScalar l=n.length2(); + if(l>GJK_SIMPLEX3_EPS) + { + btScalar mindist=-1; + btScalar subw[2]={0.f,0.f}; + U subm(0); + for(U i=0;i<3;++i) + { + if(btDot(*vt[i],btCross(dl[i],n))>0) + { + const U j=imd3[i]; + const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); + if((mindist<0)||(subd<mindist)) + { + mindist = subd; + m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + } + } + } + if(mindist<0) + { + const btScalar d=btDot(a,n); + const btScalar s=btSqrt(l); + const btVector3 p=n*(d/l); + mindist = p.length2(); + m = 7; + w[0] = (btCross(dl[1],b-p)).length()/s; + w[1] = (btCross(dl[2],c-p)).length()/s; + w[2] = 1-(w[0]+w[1]); + } + return(mindist); + } + return(-1); + } + static btScalar projectorigin( const btVector3& a, + const btVector3& b, + const btVector3& c, + const btVector3& d, + btScalar* w,U& m) + { + static const U imd3[]={1,2,0}; + const btVector3* vt[]={&a,&b,&c,&d}; + const btVector3 dl[]={a-d,b-d,c-d}; + const btScalar vl=det(dl[0],dl[1],dl[2]); + const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0; + if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) + { + btScalar mindist=-1; + btScalar subw[3]={0.f,0.f,0.f}; + U subm(0); + for(U i=0;i<3;++i) + { + const U j=imd3[i]; + const btScalar s=vl*btDot(d,btCross(dl[i],dl[j])); + if(s>0) + { + const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); + if((mindist<0)||(subd<mindist)) + { + mindist = subd; + m = static_cast<U>((subm&1?1<<i:0)+ + (subm&2?1<<j:0)+ + (subm&4?8:0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + w[3] = subw[2]; + } + } + } + if(mindist<0) + { + mindist = 0; + m = 15; + w[0] = det(c,b,d)/vl; + w[1] = det(a,c,d)/vl; + w[2] = det(b,a,d)/vl; + w[3] = 1-(w[0]+w[1]+w[2]); + } + return(mindist); + } + return(-1); + } + }; + + +enum eEpaStatus +{ + eEpaValid, + eEpaTouching, + eEpaDegenerated, + eEpaNonConvex, + eEpaInvalidHull, + eEpaOutOfFaces, + eEpaOutOfVertices, + eEpaAccuraryReached, + eEpaFallBack, + eEpaFailed +}; + + + // EPA +template <typename btConvexTemplate> + struct EPA + { + /* Types */ + + struct sFace + { + btVector3 n; + btScalar d; + typename GJK<btConvexTemplate>::sSV* c[3]; + sFace* f[3]; + sFace* l[2]; + U1 e[3]; + U1 pass; + }; + struct sList + { + sFace* root; + U count; + sList() : root(0),count(0) {} + }; + struct sHorizon + { + sFace* cf; + sFace* ff; + U nf; + sHorizon() : cf(0),ff(0),nf(0) {} + }; + + /* Fields */ + eEpaStatus m_status; + typename GJK<btConvexTemplate>::sSimplex m_result; + btVector3 m_normal; + btScalar m_depth; + typename GJK<btConvexTemplate>::sSV m_sv_store[EPA_MAX_VERTICES]; + sFace m_fc_store[EPA_MAX_FACES]; + U m_nextsv; + sList m_hull; + sList m_stock; + /* Methods */ + EPA() + { + Initialize(); + } + + + static inline void bind(sFace* fa,U ea,sFace* fb,U eb) + { + fa->e[ea]=(U1)eb;fa->f[ea]=fb; + fb->e[eb]=(U1)ea;fb->f[eb]=fa; + } + static inline void append(sList& list,sFace* face) + { + face->l[0] = 0; + face->l[1] = list.root; + if(list.root) list.root->l[0]=face; + list.root = face; + ++list.count; + } + static inline void remove(sList& list,sFace* face) + { + if(face->l[1]) face->l[1]->l[0]=face->l[0]; + if(face->l[0]) face->l[0]->l[1]=face->l[1]; + if(face==list.root) list.root=face->l[1]; + --list.count; + } + + + void Initialize() + { + m_status = eEpaFailed; + m_normal = btVector3(0,0,0); + m_depth = 0; + m_nextsv = 0; + for(U i=0;i<EPA_MAX_FACES;++i) + { + append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]); + } + } + eEpaStatus Evaluate(GJK<btConvexTemplate>& gjk,const btVector3& guess) + { + typename GJK<btConvexTemplate>::sSimplex& simplex=*gjk.m_simplex; + if((simplex.rank>1)&&gjk.EncloseOrigin()) + { + + /* Clean up */ + while(m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull,f); + append(m_stock,f); + } + m_status = eEpaValid; + m_nextsv = 0; + /* Orient simplex */ + if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, + simplex.c[1]->w-simplex.c[3]->w, + simplex.c[2]->w-simplex.c[3]->w)<0) + { + btSwap(simplex.c[0],simplex.c[1]); + btSwap(simplex.p[0],simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), + newface(simplex.c[1],simplex.c[0],simplex.c[3],true), + newface(simplex.c[2],simplex.c[1],simplex.c[3],true), + newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; + if(m_hull.count==4) + { + sFace* best=findbest(); + sFace outer=*best; + U pass=0; + U iterations=0; + bind(tetra[0],0,tetra[1],0); + bind(tetra[0],1,tetra[2],0); + bind(tetra[0],2,tetra[3],0); + bind(tetra[1],1,tetra[3],2); + bind(tetra[1],2,tetra[2],1); + bind(tetra[2],2,tetra[3],1); + m_status=eEpaValid; + for(;iterations<EPA_MAX_ITERATIONS;++iterations) + { + if(m_nextsv<EPA_MAX_VERTICES) + { + sHorizon horizon; + typename GJK<btConvexTemplate>::sSV* w=&m_sv_store[m_nextsv++]; + bool valid=true; + best->pass = (U1)(++pass); + gjk.getsupport(best->n,*w); + const btScalar wdist=btDot(best->n,w->w)-best->d; + if(wdist>EPA_ACCURACY) + { + for(U j=0;(j<3)&&valid;++j) + { + valid&=expand( pass,w, + best->f[j],best->e[j], + horizon); + } + if(valid&&(horizon.nf>=3)) + { + bind(horizon.cf,1,horizon.ff,2); + remove(m_hull,best); + append(m_stock,best); + best=findbest(); + outer=*best; + } else { m_status=eEpaInvalidHull;break; } + } else { m_status=eEpaAccuraryReached;break; } + } else { m_status=eEpaOutOfVertices;break; } + } + const btVector3 projection=outer.n*outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = btCross( outer.c[1]->w-projection, + outer.c[2]->w-projection).length(); + m_result.p[1] = btCross( outer.c[2]->w-projection, + outer.c[0]->w-projection).length(); + m_result.p[2] = btCross( outer.c[0]->w-projection, + outer.c[1]->w-projection).length(); + const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return(m_status); + } + } + /* Fallback */ + m_status = eEpaFallBack; + m_normal = -guess; + const btScalar nl=m_normal.length(); + if(nl>0) + m_normal = m_normal/nl; + else + m_normal = btVector3(1,0,0); + m_depth = 0; + m_result.rank=1; + m_result.c[0]=simplex.c[0]; + m_result.p[0]=1; + return(m_status); + } + bool getedgedist(sFace* face, typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, btScalar& dist) + { + const btVector3 ba = b->w - a->w; + const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required + + if(a_dot_nab < 0) + { + // Outside of edge a->b + + const btScalar ba_l2 = ba.length2(); + const btScalar a_dot_ba = btDot(a->w, ba); + const btScalar b_dot_ba = btDot(b->w, ba); + + if(a_dot_ba > 0) + { + // Pick distance vertex a + dist = a->w.length(); + } + else if(b_dot_ba < 0) + { + // Pick distance vertex b + dist = b->w.length(); + } + else + { + // Pick distance to edge a->b + const btScalar a_dot_b = btDot(a->w, b->w); + dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); + } + + return true; + } + + return false; + } + sFace* newface(typename GJK<btConvexTemplate>::sSV* a,typename GJK<btConvexTemplate>::sSV* b,typename GJK<btConvexTemplate>::sSV* c,bool forced) + { + if(m_stock.root) + { + sFace* face=m_stock.root; + remove(m_stock,face); + append(m_hull,face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = btCross(b->w-a->w,c->w-a->w); + const btScalar l=face->n.length(); + const bool v=l>EPA_ACCURACY; + + if(v) + { + if(!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) + { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = btDot(a->w, face->n) / l; + } + + face->n /= l; + if(forced || (face->d >= -EPA_PLANE_EPS)) + { + return face; + } + else + m_status=eEpaNonConvex; + } + else + m_status=eEpaDegenerated; + + remove(m_hull, face); + append(m_stock, face); + return 0; + + } + m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces; + return 0; + } + sFace* findbest() + { + sFace* minf=m_hull.root; + btScalar mind=minf->d*minf->d; + for(sFace* f=minf->l[1];f;f=f->l[1]) + { + const btScalar sqd=f->d*f->d; + if(sqd<mind) + { + minf=f; + mind=sqd; + } + } + return(minf); + } + bool expand(U pass,typename GJK<btConvexTemplate>::sSV* w,sFace* f,U e,sHorizon& horizon) + { + static const U i1m3[]={1,2,0}; + static const U i2m3[]={2,0,1}; + if(f->pass!=pass) + { + const U e1=i1m3[e]; + if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + { + sFace* nf=newface(f->c[e1],f->c[e],w,false); + if(nf) + { + bind(nf,0,f,e); + if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; + horizon.cf=nf; + ++horizon.nf; + return(true); + } + } + else + { + const U e2=i2m3[e]; + f->pass = (U1)pass; + if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& + expand(pass,w,f->f[e2],f->e[e2],horizon)) + { + remove(m_hull,f); + append(m_stock,f); + return(true); + } + } + } + return(false); + } + + }; + + template <typename btConvexTemplate> + static void Initialize( const btConvexTemplate& a, const btConvexTemplate& b, + btGjkEpaSolver3::sResults& results, + MinkowskiDiff<btConvexTemplate>& shape) + { + /* Results */ + results.witnesses[0] = + results.witnesses[1] = btVector3(0,0,0); + results.status = btGjkEpaSolver3::sResults::Separated; + /* Shape */ + + shape.m_toshape1 = b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis()); + shape.m_toshape0 = a.getWorldTransform().inverseTimes(b.getWorldTransform()); + + } + + +// +// Api +// + + + +// +template <typename btConvexTemplate> +bool btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b, + const btVector3& guess, + btGjkEpaSolver3::sResults& results) +{ + MinkowskiDiff<btConvexTemplate> shape(a,b); + Initialize(a,b,results,shape); + GJK<btConvexTemplate> gjk(a,b); + eGjkStatus gjk_status=gjk.Evaluate(shape,guess); + if(gjk_status==eGjkValid) + { + btVector3 w0=btVector3(0,0,0); + btVector3 w1=btVector3(0,0,0); + for(U i=0;i<gjk.m_simplex->rank;++i) + { + const btScalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = a.getWorldTransform()*w0; + results.witnesses[1] = a.getWorldTransform()*w1; + results.normal = w0-w1; + results.distance = results.normal.length(); + results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; + return(true); + } + else + { + results.status = gjk_status==eGjkInside? + btGjkEpaSolver3::sResults::Penetrating : + btGjkEpaSolver3::sResults::GJK_Failed ; + return(false); + } +} + + +template <typename btConvexTemplate> +bool btGjkEpaSolver3_Penetration(const btConvexTemplate& a, + const btConvexTemplate& b, + const btVector3& guess, + btGjkEpaSolver3::sResults& results) +{ + MinkowskiDiff<btConvexTemplate> shape(a,b); + Initialize(a,b,results,shape); + GJK<btConvexTemplate> gjk(a,b); + eGjkStatus gjk_status=gjk.Evaluate(shape,-guess); + switch(gjk_status) + { + case eGjkInside: + { + EPA<btConvexTemplate> epa; + eEpaStatus epa_status=epa.Evaluate(gjk,-guess); + if(epa_status!=eEpaFailed) + { + btVector3 w0=btVector3(0,0,0); + for(U i=0;i<epa.m_result.rank;++i) + { + w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i]; + } + results.status = btGjkEpaSolver3::sResults::Penetrating; + results.witnesses[0] = a.getWorldTransform()*w0; + results.witnesses[1] = a.getWorldTransform()*(w0-epa.m_normal*epa.m_depth); + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return(true); + } else results.status=btGjkEpaSolver3::sResults::EPA_Failed; + } + break; + case eGjkFailed: + results.status=btGjkEpaSolver3::sResults::GJK_Failed; + break; + default: + { + } + } + return(false); +} + +#if 0 +int btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanceInfo* distInfo) +{ + btGjkEpaSolver3::sResults results; + btVector3 guess = colDesc.m_firstDir; + + bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB, + colDesc.m_transformA,colDesc.m_transformB, + colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB, + guess, + results); + if (res) + { + if ((results.status==btGjkEpaSolver3::sResults::Penetrating) || results.status==GJK::eStatus::Inside) + { + //normal could be 'swapped' + + distInfo->m_distance = results.distance; + distInfo->m_normalBtoA = results.normal; + btVector3 tmpNormalInB = results.witnesses[1]-results.witnesses[0]; + btScalar lenSqr = tmpNormalInB.length2(); + if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) + { + tmpNormalInB = results.normal; + lenSqr = results.normal.length2(); + } + + if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) + { + tmpNormalInB /= btSqrt(lenSqr); + btScalar distance2 = -(results.witnesses[0]-results.witnesses[1]).length(); + //only replace valid penetrations when the result is deeper (check) + //if ((distance2 < results.distance)) + { + distInfo->m_distance = distance2; + distInfo->m_pointOnA= results.witnesses[0]; + distInfo->m_pointOnB= results.witnesses[1]; + distInfo->m_normalBtoA= tmpNormalInB; + return 0; + } + } + } + + } + + return -1; +} +#endif + +template <typename btConvexTemplate, typename btDistanceInfoTemplate> +int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b, + const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo) +{ + btGjkEpaSolver3::sResults results; + btVector3 guess = colDesc.m_firstDir; + + bool isSeparated = btGjkEpaSolver3_Distance( a,b, + guess, + results); + if (isSeparated) + { + distInfo->m_distance = results.distance; + distInfo->m_pointOnA= results.witnesses[0]; + distInfo->m_pointOnB= results.witnesses[1]; + distInfo->m_normalBtoA= results.normal; + return 0; + } + + return -1; +} + +/* Symbols cleanup */ + +#undef GJK_MAX_ITERATIONS +#undef GJK_ACCURARY +#undef GJK_MIN_DISTANCE +#undef GJK_DUPLICATED_EPS +#undef GJK_SIMPLEX2_EPS +#undef GJK_SIMPLEX3_EPS +#undef GJK_SIMPLEX4_EPS + +#undef EPA_MAX_VERTICES +#undef EPA_MAX_FACES +#undef EPA_MAX_ITERATIONS +#undef EPA_ACCURACY +#undef EPA_FALLBACK +#undef EPA_PLANE_EPS +#undef EPA_INSIDE_EPS + + + +#endif //BT_GJK_EPA3_H + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 8877579496b..759443a9613 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -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; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h new file mode 100644 index 00000000000..a22a0bae66b --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h @@ -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 diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 2ceaab750fa..5026397f67f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -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 diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp index b08205e84aa..d5f4a964bf3 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp @@ -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; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index 3999d400503..f2ed0cd39c4 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -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; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index 18eb662de2f..ec638f60ba5 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -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]); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp index a775198ab29..23b4f79cfc2 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -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 diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp index 8d940e63cd3..31faf1df5e3 100644 --- a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp +++ b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -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 } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 15a4c92de20..09b7388b63e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -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); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index 1735b524dba..b7636180c34 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -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) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 9d60d9957a5..1098d0c96b6 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -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); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index c07e9bbd806..a3a0fa6729f 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -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; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp index 3428e0b069d..75d81cc08c2 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp @@ -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) -{ - m_frameInA = frameInA; - m_frameInB = frameInB; - -} - -btFixedConstraint::~btFixedConstraint () +:btGeneric6DofSpring2Constraint(rbA,rbB,frameInA,frameInB) { + setAngularLowerLimit(btVector3(0,0,0)); + setAngularUpperLimit(btVector3(0,0,0)); + setLinearLowerLimit(btVector3(0,0,0)); + setLinearUpperLimit(btVector3(0,0,0)); } - -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; - } - } -}
\ No newline at end of file +btFixedConstraint::~btFixedConstraint () +{ +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h index e1c9430fd3c..bff2008b283 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h @@ -16,32 +16,17 @@ 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; - } + virtual ~btFixedConstraint(); }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 431a524169e..bea8629c325 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -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; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp new file mode 100644 index 00000000000..49ff78c2621 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp @@ -0,0 +1,1121 @@ +/* +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. (with PGS) +- 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 +*/ + + + +#include "btGeneric6DofSpring2Constraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" +#include <new> + + + +btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder) + : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB) + , m_frameInA(frameInA) + , m_frameInB(frameInB) + , m_rotateOrder(rotOrder) + , m_flags(0) +{ + calculateTransforms(); +} + + +btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder) + : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB) + , m_frameInB(frameInB) + , m_rotateOrder(rotOrder) + , m_flags(0) +{ + ///not providing rigidbody A means implicitly using worldspace for body A + m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; + calculateTransforms(); +} + + +btScalar btGeneric6DofSpring2Constraint::btGetMatrixElem(const btMatrix3x3& mat, int index) +{ + int i = index%3; + int j = index/3; + return mat[i][j]; +} + +// MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html + +bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz -cy*sz sy + // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx + // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy + + btScalar fi = btGetMatrixElem(mat,2); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); + xyz[1] = btAsin(btGetMatrixElem(mat,2)); + xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + return true; + } + else + { + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = -SIMD_HALF_PI; + xyz[2] = btScalar(0.0); + return false; + } + } + else + { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = SIMD_HALF_PI; + xyz[2] = 0.0; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz -sz sy*cz + // cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx + // cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy + + btScalar fi = btGetMatrixElem(mat,1); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,4)); + xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); + xyz[2] = btAsin(-btGetMatrixElem(mat,1)); + return true; + } + else + { + xyz[0] = -btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); + xyz[1] = btScalar(0.0); + xyz[2] = SIMD_HALF_PI; + return false; + } + } + else + { + xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); + xyz[1] = 0.0; + xyz[2] = -SIMD_HALF_PI; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy + // cx*sz cx*cz -sx + // cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx + + btScalar fi = btGetMatrixElem(mat,5); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAsin(-btGetMatrixElem(mat,5)); + xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,8)); + xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + return true; + } + else + { + xyz[0] = SIMD_HALF_PI; + xyz[1] = -btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + xyz[2] = btScalar(0.0); + return false; + } + } + else + { + xyz[0] = -SIMD_HALF_PI; + xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + xyz[2] = 0.0; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx + // sz cz*cx -cz*sx + // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx + + btScalar fi = btGetMatrixElem(mat,3); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,4)); + xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,0)); + xyz[2] = btAsin(btGetMatrixElem(mat,3)); + return true; + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = -btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8)); + xyz[2] = -SIMD_HALF_PI; + return false; + } + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8)); + xyz[2] = SIMD_HALF_PI; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx + // cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx + // -cx*sy sx cx*cy + + btScalar fi = btGetMatrixElem(mat,7); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAsin(btGetMatrixElem(mat,7)); + xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); + xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,4)); + return true; + } + else + { + xyz[0] = -SIMD_HALF_PI; + xyz[1] = btScalar(0.0); + xyz[2] = -btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); + return false; + } + } + else + { + xyz[0] = SIMD_HALF_PI; + xyz[1] = btScalar(0.0); + xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy + // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx + // -sy cy*sx cy*cx + + btScalar fi = btGetMatrixElem(mat,6); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(btGetMatrixElem(mat,7), btGetMatrixElem(mat,8)); + xyz[1] = btAsin(-btGetMatrixElem(mat,6)); + xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,0)); + return true; + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = SIMD_HALF_PI; + xyz[2] = -btAtan2(btGetMatrixElem(mat,1),btGetMatrixElem(mat,2)); + return false; + } + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = -SIMD_HALF_PI; + xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2)); + } + return false; +} + +void btGeneric6DofSpring2Constraint::calculateAngleInfo() +{ + btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); + switch (m_rotateOrder) + { + case RO_XYZ : matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_XZY : matrixToEulerXZY(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_YXZ : matrixToEulerYXZ(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_YZX : matrixToEulerYZX(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_ZXY : matrixToEulerZXY(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_ZYX : matrixToEulerZYX(relative_frame,m_calculatedAxisAngleDiff); break; + default : btAssert(false); + } + // in euler angle mode we do not actually constrain the angular velocity + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + switch (m_rotateOrder) + { + case RO_XYZ : + { + //Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles) + //The two planes are non-homologous, so this is a Tait–Bryan angle formalism and not a proper Euler + //Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation) + //that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait–Bryan angles) + // x' = Nperp = N.cross(axis2) + // y' = N = axis2.cross(axis0) + // z' = z + // + // x" = X + // y" = y' + // z" = ?? + //in other words: + //first rotate around z + //second rotate around y'= z.cross(X) + //third rotate around x" = X + //Original XYZ extrinsic rotation order. + //Planes: xy and YZ normals: z, X. Plane intersection (N) is z.cross(X) + btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); + btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); + m_calculatedAxis[1] = axis2.cross(axis0); + m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); + m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); + break; + } + case RO_XZY : + { + //planes: xz,ZY normals: y, X + //first rotate around y + //second rotate around z'= y.cross(X) + //third rotate around x" = X + btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); + btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1); + m_calculatedAxis[2] = axis0.cross(axis1); + m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]); + m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0); + break; + } + case RO_YXZ : + { + //planes: yx,XZ normals: z, Y + //first rotate around z + //second rotate around x'= z.cross(Y) + //third rotate around y" = Y + btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1); + btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); + m_calculatedAxis[0] = axis1.cross(axis2); + m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]); + m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1); + break; + } + case RO_YZX : + { + //planes: yz,ZX normals: x, Y + //first rotate around x + //second rotate around z'= x.cross(Y) + //third rotate around y" = Y + btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0); + btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1); + m_calculatedAxis[2] = axis0.cross(axis1); + m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]); + m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0); + break; + } + case RO_ZXY : + { + //planes: zx,XY normals: y, Z + //first rotate around y + //second rotate around x'= y.cross(Z) + //third rotate around z" = Z + btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1); + btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2); + m_calculatedAxis[0] = axis1.cross(axis2); + m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]); + m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1); + break; + } + case RO_ZYX : + { + //planes: zy,YX normals: x, Z + //first rotate around x + //second rotate around y' = x.cross(Z) + //third rotate around z" = Z + btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0); + btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2); + m_calculatedAxis[1] = axis2.cross(axis0); + m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); + m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); + break; + } + default: + btAssert(false); + } + + m_calculatedAxis[0].normalize(); + m_calculatedAxis[1].normalize(); + m_calculatedAxis[2].normalize(); + +} + +void btGeneric6DofSpring2Constraint::calculateTransforms() +{ + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); +} + +void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA,const btTransform& transB) +{ + m_calculatedTransformA = transA * m_frameInA; + m_calculatedTransformB = transB * m_frameInB; + calculateLinearInfo(); + calculateAngleInfo(); + + btScalar miA = getRigidBodyA().getInvMass(); + btScalar miB = getRigidBodyB().getInvMass(); + m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); + btScalar miS = miA + miB; + if(miS > btScalar(0.f)) + { + m_factA = miB / miS; + } + else + { + m_factA = btScalar(0.5f); + } + m_factB = btScalar(1.0f) - m_factA; +} + + +void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index) +{ + btScalar angle = m_calculatedAxisAngleDiff[axis_index]; + angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit); + m_angularLimits[axis_index].m_currentPosition = angle; + m_angularLimits[axis_index].testLimitValue(angle); +} + + +void btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info) +{ + //prepare constraint + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + info->m_numConstraintRows = 0; + info->nub = 0; + int i; + //test linear limits + for(i = 0; i < 3; i++) + { + if (m_linearLimits.m_currentLimit[i]==4) info->m_numConstraintRows += 2; + else if (m_linearLimits.m_currentLimit[i]!=0) info->m_numConstraintRows += 1; + if (m_linearLimits.m_enableMotor[i] ) info->m_numConstraintRows += 1; + if (m_linearLimits.m_enableSpring[i]) info->m_numConstraintRows += 1; + } + //test angular limits + for (i=0;i<3 ;i++ ) + { + testAngularLimitMotor(i); + if (m_angularLimits[i].m_currentLimit==4) info->m_numConstraintRows += 2; + else if (m_angularLimits[i].m_currentLimit!=0) info->m_numConstraintRows += 1; + if (m_angularLimits[i].m_enableMotor ) info->m_numConstraintRows += 1; + if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1; + } +} + + +void btGeneric6DofSpring2Constraint::getInfo2 (btConstraintInfo2* info) +{ + const btTransform& transA = m_rbA.getCenterOfMassTransform(); + const btTransform& transB = m_rbB.getCenterOfMassTransform(); + const btVector3& linVelA = m_rbA.getLinearVelocity(); + const btVector3& linVelB = m_rbB.getLinearVelocity(); + const btVector3& angVelA = m_rbA.getAngularVelocity(); + const btVector3& angVelB = m_rbB.getAngularVelocity(); + + // for stability better to solve angular limits first + int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); + setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); +} + + +int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) +{ + //solve linear limits + btRotationalLimitMotor2 limot; + for (int i=0;i<3 ;i++ ) + { + if(m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i]) + { // re-use rotational motor code + limot.m_bounce = m_linearLimits.m_bounce[i]; + limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; + limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i]; + limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; + limot.m_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i]; + limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; + limot.m_servoMotor = m_linearLimits.m_servoMotor[i]; + limot.m_servoTarget = m_linearLimits.m_servoTarget[i]; + limot.m_enableSpring = m_linearLimits.m_enableSpring[i]; + limot.m_springStiffness = m_linearLimits.m_springStiffness[i]; + limot.m_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i]; + limot.m_springDamping = m_linearLimits.m_springDamping[i]; + limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[i]; + limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i]; + limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; + limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; + limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; + limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; + btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); + int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2); + limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; + limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp; + limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0]; + limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp; + + //rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible. + int indx1 = (i + 1) % 3; + int indx2 = (i + 2) % 3; + int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static) + #define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3 + bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 || + m_angularLimits[indx1].m_currentLimit == 2 || + ( m_angularLimits[indx1].m_currentLimit == 3 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) || + ( m_angularLimits[indx1].m_currentLimit == 4 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ); + bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 || + m_angularLimits[indx2].m_currentLimit == 2 || + ( m_angularLimits[indx2].m_currentLimit == 3 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) || + ( m_angularLimits[indx2].m_currentLimit == 4 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ); + if( indx1Violated && indx2Violated ) + { + rotAllowed = 0; + } + row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed); + + } + } + return row; +} + + + +int btGeneric6DofSpring2Constraint::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 row = row_offset; + + //order of rotational constraint rows + int cIdx[] = {0, 1, 2}; + switch(m_rotateOrder) + { + case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break; + case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break; + case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break; + case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break; + case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break; + case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break; + default : btAssert(false); + } + + for (int ii = 0; ii < 3 ; ii++ ) + { + int i = cIdx[ii]; + if(m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring) + { + btVector3 axis = getAxis(i); + int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2); + if(!(flags & BT_6DOF_FLAGS_CFM_STOP2)) + { + m_angularLimits[i].m_stopCFM = info->cfm[0]; + } + if(!(flags & BT_6DOF_FLAGS_ERP_STOP2)) + { + m_angularLimits[i].m_stopERP = info->erp; + } + if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2)) + { + m_angularLimits[i].m_motorCFM = info->cfm[0]; + } + if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2)) + { + m_angularLimits[i].m_motorERP = info->erp; + } + row += get_limit_motor_info2(&m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1); + } + } + + return row; +} + + +void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB) +{ + m_frameInA = frameA; + m_frameInB = frameB; + buildJacobian(); + calculateTransforms(); +} + + +void btGeneric6DofSpring2Constraint::calculateLinearInfo() +{ + m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin(); + m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff; + for(int i = 0; i < 3; i++) + { + m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i]; + m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]); + } +} + +void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2 *info, int srow, btVector3& ax1, int rotational, int rotAllowed) +{ + btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; + btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis; + + 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]; + + if(!rotational) + { + btVector3 tmpA, tmpB, relA, relB; + // get vector from bodyB to frameB in WCS + relB = m_calculatedTransformB.getOrigin() - transB.getOrigin(); + // same for bodyA + relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); + tmpA = relA.cross(ax1); + tmpB = relB.cross(ax1); + if(m_hasStaticBody && (!rotAllowed)) + { + tmpA *= m_factA; + tmpB *= m_factB; + } + int i; + for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i]; + } +} + + +int btGeneric6DofSpring2Constraint::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) +{ + int count = 0; + int srow = row * info->rowskip; + + if (limot->m_currentLimit==4) + { + btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1); + + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1); + if (rotational) { + if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } else { + if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } + info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY; + info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0; + info->cfm[srow] = limot->m_stopCFM; + srow += info->rowskip; + ++count; + + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1); + if (rotational) { + if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } else { + if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } + info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0; + info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY; + info->cfm[srow] = limot->m_stopCFM; + srow += info->rowskip; + ++count; + } else + if (limot->m_currentLimit==3) + { + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1); + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + info->cfm[srow] = limot->m_stopCFM; + srow += info->rowskip; + ++count; + } + + if (limot->m_enableMotor && !limot->m_servoMotor) + { + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; + btScalar mot_fact = getMotorFactor(limot->m_currentPosition, + limot->m_loLimit, + limot->m_hiLimit, + tag_vel, + info->fps * limot->m_motorERP); + info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce; + info->m_upperLimit[srow] = limot->m_maxMotorForce; + info->cfm[srow] = limot->m_motorCFM; + srow += info->rowskip; + ++count; + } + + if (limot->m_enableMotor && limot->m_servoMotor) + { + btScalar error = limot->m_currentPosition - limot->m_servoTarget; + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity; + btScalar tag_vel = -targetvelocity; + btScalar mot_fact; + if(error != 0) + { + btScalar lowLimit; + btScalar hiLimit; + if(limot->m_loLimit > limot->m_hiLimit) + { + lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY; + hiLimit = error < 0 ? limot->m_servoTarget : SIMD_INFINITY; + } + else + { + lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit; + hiLimit = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit; + } + mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP); + } + else + { + mot_fact = 0; + } + info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1); + info->m_lowerLimit[srow] = -limot->m_maxMotorForce; + info->m_upperLimit[srow] = limot->m_maxMotorForce; + info->cfm[srow] = limot->m_motorCFM; + srow += info->rowskip; + ++count; + } + + if (limot->m_enableSpring) + { + btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint; + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + + //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping); + //if(cfm > 0.99999) + // cfm = 0.99999; + //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping); + //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0); + //info->m_lowerLimit[srow] = -SIMD_INFINITY; + //info->m_upperLimit[srow] = SIMD_INFINITY; + + btScalar dt = BT_ONE / info->fps; + btScalar kd = limot->m_springDamping; + btScalar ks = limot->m_springStiffness; + btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1); +// btScalar erp = 0.1; + btScalar cfm = BT_ZERO; + btScalar mA = BT_ONE / m_rbA.getInvMass(); + btScalar mB = BT_ONE / m_rbB.getInvMass(); + btScalar m = mA > mB ? mB : mA; + btScalar angularfreq = sqrt(ks / m); + + + //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency) + if(limot->m_springStiffnessLimited && 0.25 < angularfreq * dt) + { + ks = BT_ONE / dt / dt / btScalar(16.0) * m; + } + //avoid damping that would blow up the spring + if(limot->m_springDampingLimited && kd * dt > m) + { + kd = m / dt; + } + btScalar fs = ks * error * dt; + btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt; + btScalar f = (fs+fd); + + info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ; + + btScalar minf = f < fd ? f : fd; + btScalar maxf = f < fd ? fd : f; + if(!rotational) + { + info->m_lowerLimit[srow] = minf > 0 ? 0 : minf; + info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf; + } + else + { + info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf; + info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf; + } + + info->cfm[srow] = cfm; + srow += info->rowskip; + ++count; + } + + return count; +} + + +//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +//If no axis is provided, it uses the default axis for this constraint. +void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis) +{ + if((axis >= 0) && (axis < 3)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + m_linearLimits.m_stopERP[axis] = value; + m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_STOP_CFM : + m_linearLimits.m_stopCFM[axis] = value; + m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_ERP : + m_linearLimits.m_motorERP[axis] = value; + m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_CFM : + m_linearLimits.m_motorCFM[axis] = value; + m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + default : + btAssertConstrParams(0); + } + } + else if((axis >=3) && (axis < 6)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + m_angularLimits[axis - 3].m_stopERP = value; + m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_STOP_CFM : + m_angularLimits[axis - 3].m_stopCFM = value; + m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_ERP : + m_angularLimits[axis - 3].m_motorERP = value; + m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_CFM : + m_angularLimits[axis - 3].m_motorCFM = value; + m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + default : + btAssertConstrParams(0); + } + } + else + { + btAssertConstrParams(0); + } +} + +//return the local value of parameter +btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const +{ + btScalar retVal = 0; + if((axis >= 0) && (axis < 3)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_stopERP[axis]; + break; + case BT_CONSTRAINT_STOP_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_stopCFM[axis]; + break; + case BT_CONSTRAINT_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_motorERP[axis]; + break; + case BT_CONSTRAINT_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_motorCFM[axis]; + break; + default : + btAssertConstrParams(0); + } + } + else if((axis >=3) && (axis < 6)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_stopERP; + break; + case BT_CONSTRAINT_STOP_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_stopCFM; + break; + case BT_CONSTRAINT_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_motorERP; + break; + case BT_CONSTRAINT_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_motorCFM; + break; + default : + btAssertConstrParams(0); + } + } + else + { + btAssertConstrParams(0); + } + return retVal; +} + + + +void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2) +{ + btVector3 zAxis = axis1.normalized(); + btVector3 yAxis = axis2.normalized(); + btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + + btTransform frameInW; + frameInW.setIdentity(); + frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); + + // now get constraint frame in local coordinate systems + m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; + m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; + + calculateTransforms(); +} + +void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_bounce[index] = bounce; + else + m_angularLimits[index - 3].m_bounce = bounce; +} + +void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_enableMotor[index] = onOff; + else + m_angularLimits[index - 3].m_enableMotor = onOff; +} + +void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_servoMotor[index] = onOff; + else + m_angularLimits[index - 3].m_servoMotor = onOff; +} + +void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_targetVelocity[index] = velocity; + else + m_angularLimits[index - 3].m_targetVelocity = velocity; +} + +void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_servoTarget[index] = target; + else + m_angularLimits[index - 3].m_servoTarget = target; +} + +void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_maxMotorForce[index] = force; + else + m_angularLimits[index - 3].m_maxMotorForce = force; +} + +void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_enableSpring[index] = onOff; + else + m_angularLimits[index - 3] .m_enableSpring = onOff; +} + +void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) { + m_linearLimits.m_springStiffness[index] = stiffness; + m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded; + } else { + m_angularLimits[index - 3].m_springStiffness = stiffness; + m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded; + } +} + +void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) { + m_linearLimits.m_springDamping[index] = damping; + m_linearLimits.m_springDampingLimited[index] = limitIfNeeded; + } else { + m_angularLimits[index - 3].m_springDamping = damping; + m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded; + } +} + +void btGeneric6DofSpring2Constraint::setEquilibriumPoint() +{ + calculateTransforms(); + int i; + for( i = 0; i < 3; i++) + m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; + for(i = 0; i < 3; i++) + m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i]; +} + +void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index) +{ + btAssert((index >= 0) && (index < 6)); + calculateTransforms(); + if (index<3) + m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; + else + m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3]; +} + +void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_equilibriumPoint[index] = val; + else + m_angularLimits[index - 3] .m_equilibriumPoint = val; +} + + +//////////////////////////// btRotationalLimitMotor2 //////////////////////////////////// + +void btRotationalLimitMotor2::testLimitValue(btScalar test_value) +{ + //we can't normalize the angles here because we would lost the sign that we use later, but it doesn't seem to be a problem + if(m_loLimit > m_hiLimit) { + m_currentLimit = 0; + m_currentLimitError = btScalar(0.f); + } + else if(m_loLimit == m_hiLimit) { + m_currentLimitError = test_value - m_loLimit; + m_currentLimit = 3; + } else { + m_currentLimitError = test_value - m_loLimit; + m_currentLimitErrorHi = test_value - m_hiLimit; + m_currentLimit = 4; + } +} + +//////////////////////////// btTranslationalLimitMotor2 //////////////////////////////////// + +void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_value) +{ + btScalar loLimit = m_lowerLimit[limitIndex]; + btScalar hiLimit = m_upperLimit[limitIndex]; + if(loLimit > hiLimit) { + m_currentLimitError[limitIndex] = 0; + m_currentLimit[limitIndex] = 0; + } + else if(loLimit == hiLimit) { + m_currentLimitError[limitIndex] = test_value - loLimit; + m_currentLimit[limitIndex] = 3; + } else { + m_currentLimitError[limitIndex] = test_value - loLimit; + m_currentLimitErrorHi[limitIndex] = test_value - hiLimit; + m_currentLimit[limitIndex] = 4; + } +} + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h new file mode 100644 index 00000000000..ace4b3c29bf --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h @@ -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 diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h index 1b2e0f62ca8..dac59c6889d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h @@ -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); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp index 29123d526b4..4be2aabe4d3 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp @@ -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(); } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h index 9a004986911..06a8e3ecd14 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h @@ -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; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index c1897413078..76a1509471e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -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); } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 7c33ac24e05..f26e72105ba 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -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 { diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp new file mode 100644 index 00000000000..f110cd48081 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp @@ -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); +} + + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h new file mode 100644 index 00000000000..a300929cd5c --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h @@ -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 + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index 91218949498..8fa03d719d7 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -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; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index f855581c712..8da572bf7d8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -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,121 +39,253 @@ 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 resolveSingleConstraintRowGeneric(body1,body2,c); + return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c); #endif } -// 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; - } +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; +} - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,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 +} + +// Project Gauss Seidel or the equivalent Sequential Impulse +btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +{ + 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; } - - diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 47dbbe3393d..a6029180983 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -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(); }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index aff9f27f594..f8f81bfe6fa 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -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) { diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 57ebb47d8e7..628ada4cfb1 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -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; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 27fdd9d3df8..736a64a1c94 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -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), diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index b58f984d0fb..b55db0c1bfb 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -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; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index fb8a4068e23..361a054ec69 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -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(); } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index d8a34b7da3d..dd3d1c3660c 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -151,7 +151,7 @@ public: virtual void removeCollisionObject(btCollisionObject* collisionObject); - void debugDrawConstraint(btTypedConstraint* constraint); + virtual void debugDrawConstraint(btTypedConstraint* constraint); virtual void debugDrawWorld(); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index 222f9006687..e0e8bc70f57 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -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) { + ///disable collision with the 'other' body + 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); - - m_checkCollideWith = true; + { + 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 diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index ed90fb44115..1d177db802a 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -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; /////////////////////////////////////////////// diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp index 56a1c55d9ae..e49cf30d044 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -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. @@ -24,21 +25,20 @@ #include "btMultiBody.h" #include "btMultiBodyLink.h" #include "btMultiBodyLinkCollider.h" - +#include "btMultiBodyJointFeedback.h" +#include "LinearMath/btTransformUtil.h" +#include "LinearMath/btSerializer.h" // #define INCLUDE_GYRO_TERM +///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals +bool gJointFeedbackInWorldSpace = false; +bool gJointFeedbackInJointFrame = false; + namespace { const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds } - - - -// -// Various spatial helper functions -// - namespace { void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates @@ -59,7 +59,7 @@ namespace { btVector3 &bottom_out) { top_out = rotation_matrix.transpose() * top_in; - bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in)); + bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in)); } btScalar SpatialDotProduct(const btVector3 &a_top, @@ -69,6 +69,17 @@ namespace { { return a_bottom.dot(b_top) + a_top.dot(b_bottom); } + + void SpatialCrossProduct(const btVector3 &a_top, + const btVector3 &a_bottom, + const btVector3 &b_top, + const btVector3 &b_bottom, + btVector3 &top_out, + btVector3 &bottom_out) + { + top_out = a_top.cross(b_top); + bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom); + } } @@ -79,133 +90,331 @@ namespace { btMultiBody::btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, - bool fixed_base_, - bool can_sleep_) - : base_quat(0, 0, 0, 1), - base_mass(mass), - base_inertia(inertia), + bool fixedBase, + bool canSleep, + bool /*deprecatedUseMultiDof*/) + : + m_baseCollider(0), + m_baseName(0), + m_basePos(0,0,0), + m_baseQuat(0, 0, 0, 1), + m_baseMass(mass), + m_baseInertia(inertia), - fixed_base(fixed_base_), - awake(true), - can_sleep(can_sleep_), - sleep_timer(0), - m_baseCollider(0), + m_fixedBase(fixedBase), + m_awake(true), + m_canSleep(canSleep), + m_sleepTimer(0), + m_linearDamping(0.04f), m_angularDamping(0.04f), m_useGyroTerm(true), - m_maxAppliedImpulse(1000.f), - m_hasSelfCollision(true) + m_maxAppliedImpulse(1000.f), + m_maxCoordinateVelocity(100.f), + m_hasSelfCollision(true), + __posUpdated(false), + m_dofCount(0), + m_posVarCnt(0), + m_useRK4(false), + m_useGlobalVelocities(false), + m_internalNeedsJointFeedback(false) { - links.resize(n_links); + m_links.resize(n_links); + m_matrixBuf.resize(n_links + 1); + - vector_buf.resize(2*n_links); - matrix_buf.resize(n_links + 1); - m_real_buf.resize(6 + 2*n_links); - base_pos.setValue(0, 0, 0); - base_force.setValue(0, 0, 0); - base_torque.setValue(0, 0, 0); + m_baseForce.setValue(0, 0, 0); + m_baseTorque.setValue(0, 0, 0); } btMultiBody::~btMultiBody() { } +void btMultiBody::setupFixed(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/) +{ + + m_links[i].m_mass = mass; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].m_dVector = thisPivotToThisComOffset; + m_links[i].m_eVector = parentComToThisPivotOffset; + + m_links[i].m_jointType = btMultibodyLink::eFixed; + m_links[i].m_dofCount = 0; + m_links[i].m_posVarCount = 0; + + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + + m_links[i].updateCacheMultiDof(); + + updateLinksDofOffsets(); + +} + + void btMultiBody::setupPrismatic(int i, btScalar mass, const btVector3 &inertia, int parent, - const btQuaternion &rot_parent_to_this, - const btVector3 &joint_axis, - const btVector3 &r_vector_when_q_zero, + const btQuaternion &rotParentToThis, + const btVector3 &jointAxis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, bool disableParentCollision) { - links[i].mass = mass; - links[i].inertia = inertia; - links[i].parent = parent; - links[i].zero_rot_parent_to_this = rot_parent_to_this; - links[i].axis_top.setValue(0,0,0); - links[i].axis_bottom = joint_axis; - links[i].e_vector = r_vector_when_q_zero; - links[i].is_revolute = false; - links[i].cached_rot_parent_to_this = rot_parent_to_this; - if (disableParentCollision) - links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + m_dofCount += 1; + m_posVarCnt += 1; + + m_links[i].m_mass = mass; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].setAxisTop(0, 0., 0., 0.); + m_links[i].setAxisBottom(0, jointAxis); + m_links[i].m_eVector = parentComToThisPivotOffset; + m_links[i].m_dVector = thisPivotToThisComOffset; + m_links[i].m_cachedRotParentToThis = rotParentToThis; + + m_links[i].m_jointType = btMultibodyLink::ePrismatic; + m_links[i].m_dofCount = 1; + m_links[i].m_posVarCount = 1; + m_links[i].m_jointPos[0] = 0.f; + m_links[i].m_jointTorque[0] = 0.f; - links[i].updateCache(); + if (disableParentCollision) + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // + + m_links[i].updateCacheMultiDof(); + + updateLinksDofOffsets(); } void btMultiBody::setupRevolute(int i, btScalar mass, const btVector3 &inertia, int parent, - const btQuaternion &zero_rot_parent_to_this, - const btVector3 &joint_axis, - const btVector3 &parent_axis_position, - const btVector3 &my_axis_position, + const btQuaternion &rotParentToThis, + const btVector3 &jointAxis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, bool disableParentCollision) { - links[i].mass = mass; - links[i].inertia = inertia; - links[i].parent = parent; - links[i].zero_rot_parent_to_this = zero_rot_parent_to_this; - links[i].axis_top = joint_axis; - links[i].axis_bottom = joint_axis.cross(my_axis_position); - links[i].d_vector = my_axis_position; - links[i].e_vector = parent_axis_position; - links[i].is_revolute = true; + m_dofCount += 1; + m_posVarCnt += 1; + + m_links[i].m_mass = mass; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].setAxisTop(0, jointAxis); + m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset)); + m_links[i].m_dVector = thisPivotToThisComOffset; + m_links[i].m_eVector = parentComToThisPivotOffset; + + m_links[i].m_jointType = btMultibodyLink::eRevolute; + m_links[i].m_dofCount = 1; + m_links[i].m_posVarCount = 1; + m_links[i].m_jointPos[0] = 0.f; + m_links[i].m_jointTorque[0] = 0.f; + + if (disableParentCollision) + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // + m_links[i].updateCacheMultiDof(); + // + updateLinksDofOffsets(); +} + + + +void btMultiBody::setupSpherical(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, + bool disableParentCollision) +{ + + m_dofCount += 3; + m_posVarCnt += 4; + + m_links[i].m_mass = mass; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].m_dVector = thisPivotToThisComOffset; + m_links[i].m_eVector = parentComToThisPivotOffset; + + m_links[i].m_jointType = btMultibodyLink::eSpherical; + m_links[i].m_dofCount = 3; + m_links[i].m_posVarCount = 4; + m_links[i].setAxisTop(0, 1.f, 0.f, 0.f); + m_links[i].setAxisTop(1, 0.f, 1.f, 0.f); + m_links[i].setAxisTop(2, 0.f, 0.f, 1.f); + m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset)); + m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset)); + m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset)); + m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointPos[3] = 1.f; + m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f; + + if (disableParentCollision) - links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; - links[i].updateCache(); + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // + m_links[i].updateCacheMultiDof(); + // + updateLinksDofOffsets(); } +void btMultiBody::setupPlanar(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &rotationAxis, + const btVector3 &parentComToThisComOffset, + bool disableParentCollision) +{ + + m_dofCount += 3; + m_posVarCnt += 3; + + m_links[i].m_mass = mass; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].m_dVector.setZero(); + m_links[i].m_eVector = parentComToThisComOffset; + + // + static btVector3 vecNonParallelToRotAxis(1, 0, 0); + if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999) + vecNonParallelToRotAxis.setValue(0, 1, 0); + // + + m_links[i].m_jointType = btMultibodyLink::ePlanar; + m_links[i].m_dofCount = 3; + m_links[i].m_posVarCount = 3; + btVector3 n=rotationAxis.normalized(); + m_links[i].setAxisTop(0, n[0],n[1],n[2]); + m_links[i].setAxisTop(1,0,0,0); + m_links[i].setAxisTop(2,0,0,0); + m_links[i].setAxisBottom(0,0,0,0); + btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis); + m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]); + cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0)); + m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]); + m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; + m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f; + if (disableParentCollision) + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // + m_links[i].updateCacheMultiDof(); + // + updateLinksDofOffsets(); +} +void btMultiBody::finalizeMultiDof() +{ + m_deltaV.resize(0); + m_deltaV.resize(6 + m_dofCount); + m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") + m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) + updateLinksDofOffsets(); +} int btMultiBody::getParent(int i) const { - return links[i].parent; + return m_links[i].m_parent; } btScalar btMultiBody::getLinkMass(int i) const { - return links[i].mass; + return m_links[i].m_mass; } const btVector3 & btMultiBody::getLinkInertia(int i) const { - return links[i].inertia; + return m_links[i].m_inertiaLocal; } btScalar btMultiBody::getJointPos(int i) const { - return links[i].joint_pos; + return m_links[i].m_jointPos[0]; } btScalar btMultiBody::getJointVel(int i) const { - return m_real_buf[6 + i]; + return m_realBuf[6 + m_links[i].m_dofOffset]; } +btScalar * btMultiBody::getJointPosMultiDof(int i) +{ + return &m_links[i].m_jointPos[0]; +} + +btScalar * btMultiBody::getJointVelMultiDof(int i) +{ + return &m_realBuf[6 + m_links[i].m_dofOffset]; +} + +const btScalar * btMultiBody::getJointPosMultiDof(int i) const +{ + return &m_links[i].m_jointPos[0]; +} + +const btScalar * btMultiBody::getJointVelMultiDof(int i) const +{ + return &m_realBuf[6 + m_links[i].m_dofOffset]; +} + + void btMultiBody::setJointPos(int i, btScalar q) { - links[i].joint_pos = q; - links[i].updateCache(); + m_links[i].m_jointPos[0] = q; + m_links[i].updateCacheMultiDof(); +} + +void btMultiBody::setJointPosMultiDof(int i, btScalar *q) +{ + for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos) + m_links[i].m_jointPos[pos] = q[pos]; + + m_links[i].updateCacheMultiDof(); } void btMultiBody::setJointVel(int i, btScalar qdot) { - m_real_buf[6 + i] = qdot; + m_realBuf[6 + m_links[i].m_dofOffset] = qdot; +} + +void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot) +{ + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof]; } const btVector3 & btMultiBody::getRVector(int i) const { - return links[i].cached_r_vector; + return m_links[i].m_cachedRVector; } const btQuaternion & btMultiBody::getParentToLocalRot(int i) const { - return links[i].cached_rot_parent_to_this; + return m_links[i].m_cachedRotParentToThis; } btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const @@ -260,20 +469,20 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const { int num_links = getNumLinks(); // Calculates the velocities of each link (and the base) in its local frame - omega[0] = quatRotate(base_quat ,getBaseOmega()); - vel[0] = quatRotate(base_quat ,getBaseVel()); + omega[0] = quatRotate(m_baseQuat ,getBaseOmega()); + vel[0] = quatRotate(m_baseQuat ,getBaseVel()); for (int i = 0; i < num_links; ++i) { - const int parent = links[i].parent; + const int parent = m_links[i].m_parent; // transform parent vel into this frame, store in omega[i+1], vel[i+1] - SpatialTransform(btMatrix3x3(links[i].cached_rot_parent_to_this), links[i].cached_r_vector, + SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector, omega[parent+1], vel[parent+1], omega[i+1], vel[i+1]); // now add qidot * shat_i - omega[i+1] += getJointVel(i) * links[i].axis_top; - vel[i+1] += getJointVel(i) * links[i].axis_bottom; + omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0); + vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0); } } @@ -286,12 +495,12 @@ btScalar btMultiBody::getKineticEnergy() const compTreeLinkVelocities(&omega[0], &vel[0]); // we will do the factor of 0.5 at the end - btScalar result = base_mass * vel[0].dot(vel[0]); - result += omega[0].dot(base_inertia * omega[0]); + btScalar result = m_baseMass * vel[0].dot(vel[0]); + result += omega[0].dot(m_baseInertia * omega[0]); for (int i = 0; i < num_links; ++i) { - result += links[i].mass * vel[i+1].dot(vel[i+1]); - result += omega[i+1].dot(links[i].inertia * omega[i+1]); + result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]); + result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]); } return 0.5f * result; @@ -306,27 +515,38 @@ btVector3 btMultiBody::getAngularMomentum() const btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1); compTreeLinkVelocities(&omega[0], &vel[0]); - rot_from_world[0] = base_quat; - btVector3 result = quatRotate(rot_from_world[0].inverse() , (base_inertia * omega[0])); + rot_from_world[0] = m_baseQuat; + btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0])); for (int i = 0; i < num_links; ++i) { - rot_from_world[i+1] = links[i].cached_rot_parent_to_this * rot_from_world[links[i].parent+1]; - result += (quatRotate(rot_from_world[i+1].inverse() , (links[i].inertia * omega[i+1]))); + rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1]; + result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1]))); } return result; } +void btMultiBody::clearConstraintForces() +{ + m_baseConstraintForce.setValue(0, 0, 0); + m_baseConstraintTorque.setValue(0, 0, 0); + + for (int i = 0; i < getNumLinks(); ++i) { + m_links[i].m_appliedConstraintForce.setValue(0, 0, 0); + m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0); + } +} void btMultiBody::clearForcesAndTorques() { - base_force.setValue(0, 0, 0); - base_torque.setValue(0, 0, 0); + m_baseForce.setValue(0, 0, 0); + m_baseTorque.setValue(0, 0, 0); + for (int i = 0; i < getNumLinks(); ++i) { - links[i].applied_force.setValue(0, 0, 0); - links[i].applied_torque.setValue(0, 0, 0); - links[i].joint_torque = 0; + m_links[i].m_appliedForce.setValue(0, 0, 0); + m_links[i].m_appliedTorque.setValue(0, 0, 0); + m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f; } } @@ -334,54 +554,81 @@ void btMultiBody::clearVelocities() { for (int i = 0; i < 6 + getNumLinks(); ++i) { - m_real_buf[i] = 0.f; + m_realBuf[i] = 0.f; } } void btMultiBody::addLinkForce(int i, const btVector3 &f) { - links[i].applied_force += f; + m_links[i].m_appliedForce += f; } void btMultiBody::addLinkTorque(int i, const btVector3 &t) { - links[i].applied_torque += t; + m_links[i].m_appliedTorque += t; } +void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f) +{ + m_links[i].m_appliedConstraintForce += f; +} + +void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t) +{ + m_links[i].m_appliedConstraintTorque += t; +} + + + void btMultiBody::addJointTorque(int i, btScalar Q) { - links[i].joint_torque += Q; + m_links[i].m_jointTorque[0] += Q; +} + +void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q) +{ + m_links[i].m_jointTorque[dof] += Q; +} + +void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q) +{ + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + m_links[i].m_jointTorque[dof] = Q[dof]; } const btVector3 & btMultiBody::getLinkForce(int i) const { - return links[i].applied_force; + return m_links[i].m_appliedForce; } const btVector3 & btMultiBody::getLinkTorque(int i) const { - return links[i].applied_torque; + return m_links[i].m_appliedTorque; } btScalar btMultiBody::getJointTorque(int i) const { - return links[i].joint_torque; + return m_links[i].m_jointTorque[0]; } +btScalar * btMultiBody::getJointTorqueMultiDof(int i) +{ + return &m_links[i].m_jointTorque[0]; +} -inline btMatrix3x3 vecMulVecTranspose(const btVector3& v0, const btVector3& v1Transposed) +inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross? { btVector3 row0 = btVector3( - v0.x() * v1Transposed.x(), - v0.x() * v1Transposed.y(), - v0.x() * v1Transposed.z()); + v0.x() * v1.x(), + v0.x() * v1.y(), + v0.x() * v1.z()); btVector3 row1 = btVector3( - v0.y() * v1Transposed.x(), - v0.y() * v1Transposed.y(), - v0.y() * v1Transposed.z()); + v0.y() * v1.x(), + v0.y() * v1.y(), + v0.y() * v1.z()); btVector3 row2 = btVector3( - v0.z() * v1Transposed.x(), - v0.z() * v1Transposed.y(), - v0.z() * v1Transposed.z()); + v0.z() * v1.x(), + v0.z() * v1.y(), + v0.z() * v1.z()); btMatrix3x3 m(row0[0],row0[1],row0[2], row1[0],row1[1],row1[2], @@ -389,11 +636,14 @@ inline btMatrix3x3 vecMulVecTranspose(const btVector3& v0, const btVector3& v1Tr return m; } +#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed) +// -void btMultiBody::stepVelocities(btScalar dt, +void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v, - btAlignedObjectArray<btMatrix3x3> &scratch_m) + btAlignedObjectArray<btMatrix3x3> &scratch_m, + bool isConstraintPass) { // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) // and the base linear & angular accelerations. @@ -405,6 +655,12 @@ void btMultiBody::stepVelocities(btScalar dt, // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), // num_links joint acceleration values. + // We added support for multi degree of freedom (multi dof) joints. + // In addition we also can compute the joint reaction forces. This is performed in a second pass, + // so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver) + + m_internalNeedsJointFeedback = false; + int num_links = getNumLinks(); const btScalar DAMPING_K1_LINEAR = m_linearDamping; @@ -419,261 +675,513 @@ void btMultiBody::stepVelocities(btScalar dt, // Temporary matrices/vectors -- use scratch space from caller // so that we don't have to keep reallocating every frame - scratch_r.resize(2*num_links + 6); + scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount scratch_v.resize(8*num_links + 6); scratch_m.resize(4*num_links + 4); - btScalar * r_ptr = &scratch_r[0]; - btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results + //btScalar * r_ptr = &scratch_r[0]; + btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results btVector3 * v_ptr = &scratch_v[0]; - // vhat_i (top = angular, bottom = linear part) - btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1; - btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1; - - // zhat_i^A - btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1; - btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1; - - // chat_i (note NOT defined for the base) - btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links; - btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links; - - // top left, top right and bottom left blocks of Ihat_i^A. - // bottom right block = transpose of top left block and is not stored. - // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently. - btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1]; - btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2]; - btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3]; + // vhat_i (top = angular, bottom = linear part) + btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr; + v_ptr += num_links * 2 + 2; + // + // zhat_i^A + btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; + v_ptr += num_links * 2 + 2; + // + // chat_i (note NOT defined for the base) + btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr; + v_ptr += num_links * 2; + // + // Ihat_i^A. + btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1]; // Cached 3x3 rotation matrices from parent frame to this frame. - btMatrix3x3 * rot_from_parent = &matrix_buf[0]; + btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; btMatrix3x3 * rot_from_world = &scratch_m[0]; // hhat_i, ahat_i - // hhat is NOT stored for the base (but ahat is) - btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0; - btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0; - btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; - btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; - - // Y_i, D_i - btScalar * Y = r_ptr; r_ptr += num_links; - btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0; + // hhat is NOT stored for the base (but ahat is) + btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); + btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; + v_ptr += num_links * 2 + 2; + // + // Y_i, invD_i + btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; + btScalar * Y = &scratch_r[0]; + // + //aux variables + static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence) + static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do + static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies + static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) + static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough + static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors + static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child + static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp + static btSpatialTransformationMatrix fromWorld; + fromWorld.m_trnVec.setZero(); + ///////////////// // ptr to the joint accel part of the output btScalar * joint_accel = output + 6; - // Start of the algorithm proper. // First 'upward' loop. // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. - rot_from_parent[0] = btMatrix3x3(base_quat); + rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? - vel_top_angular[0] = rot_from_parent[0] * base_omega; - vel_bottom_linear[0] = rot_from_parent[0] * base_vel; - - if (fixed_base) { - zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0); - } else { - zero_acc_top_angular[0] = - (rot_from_parent[0] * (base_force - - base_mass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel)); - - zero_acc_bottom_linear[0] = - - (rot_from_parent[0] * base_torque); + //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates + spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel); + if (m_fixedBase) + { + zeroAccSpatFrc[0].setZero(); + } + else + { + btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce; + btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque; + //external forces + zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce)); + + //adding damping terms (only) + btScalar linDampMult = 1., angDampMult = 1.; + zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()), + linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm())); + + // + //p += vhat x Ihat vhat - done in a simpler way if (m_useGyroTerm) - zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( base_inertia * vel_top_angular[0] ); + zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular())); + // + zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear())); + } - zero_acc_bottom_linear[0] += base_inertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm()); - } + //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) + spatInertia[0].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0), + // + btMatrix3x3(m_baseMass, 0, 0, + 0, m_baseMass, 0, + 0, 0, m_baseMass), + // + btMatrix3x3(m_baseInertia[0], 0, 0, + 0, m_baseInertia[1], 0, + 0, 0, m_baseInertia[2]) + ); + rot_from_world[0] = rot_from_parent[0]; + // + for (int i = 0; i < num_links; ++i) { + const int parent = m_links[i].m_parent; + rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); + rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; - inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); - - - inertia_top_right[0].setValue(base_mass, 0, 0, - 0, base_mass, 0, - 0, 0, base_mass); - inertia_bottom_left[0].setValue(base_inertia[0], 0, 0, - 0, base_inertia[1], 0, - 0, 0, base_inertia[2]); + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + fromWorld.m_rotMat = rot_from_world[i+1]; + fromParent.transform(spatVel[parent+1], spatVel[i+1]); - rot_from_world[0] = rot_from_parent[0]; + // now set vhat_i to its true value by doing + // vhat_i += qidot * shat_i + if(!m_useGlobalVelocities) + { + spatJointVel.setZero(); - for (int i = 0; i < num_links; ++i) { - const int parent = links[i].parent; - rot_from_parent[i+1] = btMatrix3x3(links[i].cached_rot_parent_to_this); - + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; - rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; - - // vhat_i = i_xhat_p(i) * vhat_p(i) - SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, - vel_top_angular[parent+1], vel_bottom_linear[parent+1], - vel_top_angular[i+1], vel_bottom_linear[i+1]); - - // we can now calculate chat_i - // remember vhat_i is really vhat_p(i) (but in current frame) at this point - coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(links[i].cached_r_vector)) - + 2 * vel_top_angular[i+1].cross(links[i].axis_bottom) * getJointVel(i); - if (links[i].is_revolute) { - coriolis_top_angular[i] = vel_top_angular[i+1].cross(links[i].axis_top) * getJointVel(i); - coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom); - } else { - coriolis_top_angular[i] = btVector3(0,0,0); - } - - // now set vhat_i to its true value by doing - // vhat_i += qidot * shat_i - vel_top_angular[i+1] += getJointVel(i) * links[i].axis_top; - vel_bottom_linear[i+1] += getJointVel(i) * links[i].axis_bottom; + // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint + spatVel[i+1] += spatJointVel; - // calculate zhat_i^A - zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (links[i].applied_force)); - zero_acc_top_angular[i+1] += links[i].mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1]; + // + // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint + //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel; - zero_acc_bottom_linear[i+1] = - - (rot_from_world[i+1] * links[i].applied_torque); - if (m_useGyroTerm) + } + else { - zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( links[i].inertia * vel_top_angular[i+1] ); + fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]); + fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel); } - zero_acc_bottom_linear[i+1] += links[i].inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm()); + // we can now calculate chat_i + spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]); - // calculate Ihat_i^A - inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); - inertia_top_right[i+1].setValue(links[i].mass, 0, 0, - 0, links[i].mass, 0, - 0, 0, links[i].mass); - inertia_bottom_left[i+1].setValue(links[i].inertia[0], 0, 0, - 0, links[i].inertia[1], 0, - 0, 0, links[i].inertia[2]); - } + // calculate zhat_i^A + // + //external forces + btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce; + btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; + + zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce )); + +#if 0 + { + b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f", + i+1, + zeroAccSpatFrc[i+1].m_topVec[0], + zeroAccSpatFrc[i+1].m_topVec[1], + zeroAccSpatFrc[i+1].m_topVec[2], + zeroAccSpatFrc[i+1].m_bottomVec[0], + zeroAccSpatFrc[i+1].m_bottomVec[1], + zeroAccSpatFrc[i+1].m_bottomVec[2]); + } +#endif + // + //adding damping terms (only) + btScalar linDampMult = 1., angDampMult = 1.; + zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()), + linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm())); + + // calculate Ihat_i^A + //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) + spatInertia[i+1].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0), + // + btMatrix3x3(m_links[i].m_mass, 0, 0, + 0, m_links[i].m_mass, 0, + 0, 0, m_links[i].m_mass), + // + btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0, + 0, m_links[i].m_inertiaLocal[1], 0, + 0, 0, m_links[i].m_inertiaLocal[2]) + ); + // + //p += vhat x Ihat vhat - done in a simpler way + if(m_useGyroTerm) + zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular())); + // + zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear())); + //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); + ////clamp parent's omega + //btScalar parOmegaMod = temp.length(); + //btScalar parOmegaModMax = 1000; + //if(parOmegaMod > parOmegaModMax) + // temp *= parOmegaModMax / parOmegaMod; + //zeroAccSpatFrc[i+1].addLinear(temp); + //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); + //temp = spatCoriolisAcc[i].getLinear(); + //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length()); + + + + //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z()); + //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z()); + //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z()); + } + // 'Downward' loop. // (part of TreeForwardDynamics in Mirtich.) - for (int i = num_links - 1; i >= 0; --i) { - - h_top[i] = inertia_top_left[i+1] * links[i].axis_top + inertia_top_right[i+1] * links[i].axis_bottom; - h_bottom[i] = inertia_bottom_left[i+1] * links[i].axis_top + inertia_top_left[i+1].transpose() * links[i].axis_bottom; - btScalar val = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]); - D[i] = val; - Y[i] = links[i].joint_torque - - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]) - - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]); + for (int i = num_links - 1; i >= 0; --i) + { + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - const int parent = links[i].parent; + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + hDof = spatInertia[i+1] * m_links[i].m_axes[dof]; + // + Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] + - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) + - spatCoriolisAcc[i].dot(hDof) + ; + } - - // Ip += pXi * (Ii - hi hi' / Di) * iXp - const btScalar one_over_di = 1.0f / D[i]; + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btScalar *D_row = &D[dof * m_links[i].m_dofCount]; + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; + D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2); + } + } - + btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + switch(m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + invDi[0] = 1.0f / D[0]; + break; + } + case btMultibodyLink::eSpherical: + case btMultibodyLink::ePlanar: + { + static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); + static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse(); + + //unroll the loop? + for(int row = 0; row < 3; ++row) + { + for(int col = 0; col < 3; ++col) + { + invDi[row * 3 + col] = invD3x3[row][col]; + } + } + + break; + } + default: + { + + } + } + //determine h*D^{-1} + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + spatForceVecTemps[dof].setZero(); + + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; + // + spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof]; + } + } - const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]); - const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]); - const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]); + dyadTemp = spatInertia[i+1]; + //determine (h*D^{-1}) * h^{T} + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]); + } - btMatrix3x3 r_cross; - r_cross.setValue( - 0, -links[i].cached_r_vector[2], links[i].cached_r_vector[1], - links[i].cached_r_vector[2], 0, -links[i].cached_r_vector[0], - -links[i].cached_r_vector[1], links[i].cached_r_vector[0], 0); + fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add); - inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1]; - inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1]; - inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() * - (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1]; - - - // Zp += pXi * (Zi + Ii*ci + hi*Yi/Di) - btVector3 in_top, in_bottom, out_top, out_bottom; - const btScalar Y_over_D = Y[i] * one_over_di; - in_top = zero_acc_top_angular[i+1] - + inertia_top_left[i+1] * coriolis_top_angular[i] - + inertia_top_right[i+1] * coriolis_bottom_linear[i] - + Y_over_D * h_top[i]; - in_bottom = zero_acc_bottom_linear[i+1] - + inertia_bottom_left[i+1] * coriolis_top_angular[i] - + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i] - + Y_over_D * h_bottom[i]; - InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, - in_top, in_bottom, out_top, out_bottom); - zero_acc_top_angular[parent+1] += out_top; - zero_acc_bottom_linear[parent+1] += out_bottom; + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + invD_times_Y[dof] = 0.f; + + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; + } + } + + spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + spatForceVecTemps[0] += hDof * invD_times_Y[dof]; + } + + fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); + + zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; } // Second 'upward' loop // (part of TreeForwardDynamics in Mirtich) - if (fixed_base) + if (m_fixedBase) { - accel_top[0] = accel_bottom[0] = btVector3(0,0,0); + spatAcc[0].setZero(); } else { if (num_links > 0) { - //Matrix<btScalar, 6, 6> Imatrix; - //Imatrix.block<3,3>(0,0) = inertia_top_left[0]; - //Imatrix.block<3,3>(3,0) = inertia_bottom_left[0]; - //Imatrix.block<3,3>(0,3) = inertia_top_right[0]; - //Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose(); - //cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix)); // TODO: Avoid memory allocation here? - - cached_inertia_top_left = inertia_top_left[0]; - cached_inertia_top_right = inertia_top_right[0]; - cached_inertia_lower_left = inertia_bottom_left[0]; - cached_inertia_lower_right= inertia_top_left[0].transpose(); - - } - btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]); - btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]); - float result[6]; - - solveImatrix(rhs_top, rhs_bot, result); -// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]); - for (int i = 0; i < 3; ++i) { - accel_top[0][i] = -result[i]; - accel_bottom[0][i] = -result[i+3]; - } + m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat; + m_cachedInertiaTopRight = spatInertia[0].m_topRightMat; + m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat; + m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose(); + } + + solveImatrix(zeroAccSpatFrc[0], result); + spatAcc[0] = -result; } + + + // now do the loop over the m_links + for (int i = 0; i < num_links; ++i) + { + // qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar) + // a = apar + cor + Sqdd + //or + // qdd = D^{-1} * (Y - h^{T}*(apar+cor)) + // a = apar + Sqdd + + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + + fromParent.transform(spatAcc[parent+1], spatAcc[i+1]); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); + } + + btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + //D^{-1} * (Y - h^{T}*apar) + mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); + + spatAcc[i+1] += spatCoriolisAcc[i]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; + + if (m_links[i].m_jointFeedback) + { + m_internalNeedsJointFeedback = true; + + btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; + btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; + + if (gJointFeedbackInJointFrame) + { + //shift the reaction forces to the joint frame + //linear (force) component is the same + //shift the angular (torque, moment) component using the relative position, m_links[i].m_dVector + angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector); + } + + + if (gJointFeedbackInWorldSpace) + { + if (isConstraintPass) + { + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec; + } else + { + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec; + } + } else + { + if (isConstraintPass) + { + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec; + + } + else + { + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec; + } + } + } - // now do the loop over the links - for (int i = 0; i < num_links; ++i) { - const int parent = links[i].parent; - SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, - accel_top[parent+1], accel_bottom[parent+1], - accel_top[i+1], accel_bottom[i+1]); - joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i]; - accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * links[i].axis_top; - accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * links[i].axis_bottom; } // transform base accelerations back to the world frame. - btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0]; + btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); output[0] = omegadot_out[0]; output[1] = omegadot_out[1]; output[2] = omegadot_out[2]; - btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0]; + btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear())); output[3] = vdot_out[0]; output[4] = vdot_out[1]; output[5] = vdot_out[2]; + + ///////////////// + //printf("q = ["); + //printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z()); + //for(int link = 0; link < getNumLinks(); ++link) + // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) + // printf("%.6f ", m_links[link].m_jointPos[dof]); + //printf("]\n"); + //// + //printf("qd = ["); + //for(int dof = 0; dof < getNumDofs() + 6; ++dof) + // printf("%.6f ", m_realBuf[dof]); + //printf("]\n"); + //printf("qdd = ["); + //for(int dof = 0; dof < getNumDofs() + 6; ++dof) + // printf("%.6f ", output[dof]); + //printf("]\n"); + ///////////////// + // Final step: add the accelerations (times dt) to the velocities. - applyDeltaVee(output, dt); + if (!isConstraintPass) + { + if(dt > 0.) + applyDeltaVeeMultiDof(output, dt); + + } + ///// + //btScalar angularThres = 1; + //btScalar maxAngVel = 0.; + //bool scaleDown = 1.; + //for(int link = 0; link < m_links.size(); ++link) + //{ + // if(spatVel[link+1].getAngular().length() > maxAngVel) + // { + // maxAngVel = spatVel[link+1].getAngular().length(); + // scaleDown = angularThres / spatVel[link+1].getAngular().length(); + // break; + // } + //} + + //if(scaleDown != 1.) + //{ + // for(int link = 0; link < m_links.size(); ++link) + // { + // if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical) + // { + // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) + // getJointVelMultiDof(link)[dof] *= scaleDown; + // } + // } + //} + ///// + + ///////////////////// + if(m_useGlobalVelocities) + { + for (int i = 0; i < num_links; ++i) + { + const int parent = m_links[i].m_parent; + //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done + //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done + + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + fromWorld.m_rotMat = rot_from_world[i+1]; + + // vhat_i = i_xhat_p(i) * vhat_p(i) + fromParent.transform(spatVel[parent+1], spatVel[i+1]); + //nice alternative below (using operator *) but it generates temps + ///////////////////////////////////////////////////////////// + + // now set vhat_i to its true value by doing + // vhat_i += qidot * shat_i + spatJointVel.setZero(); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; + + // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint + spatVel[i+1] += spatJointVel; + + + fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity); + fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity); + } + } } @@ -685,24 +1193,24 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo ///solve I * x = rhs, so the result = invI * rhs if (num_links == 0) { - // in the case of 0 links (i.e. a plain rigid body, not a multibody) rhs * invI is easier - result[0] = rhs_bot[0] / base_inertia[0]; - result[1] = rhs_bot[1] / base_inertia[1]; - result[2] = rhs_bot[2] / base_inertia[2]; - result[3] = rhs_top[0] / base_mass; - result[4] = rhs_top[1] / base_mass; - result[5] = rhs_top[2] / base_mass; + // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier + result[0] = rhs_bot[0] / m_baseInertia[0]; + result[1] = rhs_bot[1] / m_baseInertia[1]; + result[2] = rhs_bot[2] / m_baseInertia[2]; + result[3] = rhs_top[0] / m_baseMass; + result[4] = rhs_top[1] / m_baseMass; + result[5] = rhs_top[2] / m_baseMass; } else { /// Special routine for calculating the inverse of a spatial inertia matrix ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices - btMatrix3x3 Binv = cached_inertia_top_right.inverse()*-1.f; - btMatrix3x3 tmp = cached_inertia_lower_right * Binv; - btMatrix3x3 invIupper_right = (tmp * cached_inertia_top_left + cached_inertia_lower_left).inverse(); - tmp = invIupper_right * cached_inertia_lower_right; + btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f; + btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv; + btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse(); + tmp = invIupper_right * m_cachedInertiaLowerRight; btMatrix3x3 invI_upper_left = (tmp * Binv); btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); - tmp = cached_inertia_top_left * invI_upper_left; + tmp = m_cachedInertiaTopLeft * invI_upper_left; tmp[0][0]-= 1.0; tmp[1][1]-= 1.0; tmp[2][2]-= 1.0; @@ -727,171 +1235,363 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo } } +void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const +{ + int num_links = getNumLinks(); + ///solve I * x = rhs, so the result = invI * rhs + if (num_links == 0) + { + // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier + result.setAngular(rhs.getAngular() / m_baseInertia); + result.setLinear(rhs.getLinear() / m_baseMass); + } else + { + /// Special routine for calculating the inverse of a spatial inertia matrix + ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices + btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f; + btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv; + btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse(); + tmp = invIupper_right * m_cachedInertiaLowerRight; + btMatrix3x3 invI_upper_left = (tmp * Binv); + btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); + tmp = m_cachedInertiaTopLeft * invI_upper_left; + tmp[0][0]-= 1.0; + tmp[1][1]-= 1.0; + tmp[2][2]-= 1.0; + btMatrix3x3 invI_lower_left = (Binv * tmp); + + //multiply result = invI * rhs + { + btVector3 vtop = invI_upper_left*rhs.getLinear(); + btVector3 tmp; + tmp = invIupper_right * rhs.getAngular(); + vtop += tmp; + btVector3 vbot = invI_lower_left*rhs.getLinear(); + tmp = invI_lower_right * rhs.getAngular(); + vbot += tmp; + result.setVector(vtop, vbot); + } + + } +} +void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const +{ + for (int row = 0; row < rowsA; row++) + { + for (int col = 0; col < colsB; col++) + { + pC[row * colsB + col] = 0.f; + for (int inner = 0; inner < rowsB; inner++) + { + pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB]; + } + } + } +} -void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output, +void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const { // Temporary matrices/vectors -- use scratch space from caller // so that we don't have to keep reallocating every frame - int num_links = getNumLinks(); - scratch_r.resize(num_links); - scratch_v.resize(4*num_links + 4); - btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0]; + + int num_links = getNumLinks(); + scratch_r.resize(m_dofCount); + scratch_v.resize(4*num_links + 4); + + btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0; btVector3 * v_ptr = &scratch_v[0]; - + // zhat_i^A (scratch space) - btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1; - btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1; + btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; + v_ptr += num_links * 2 + 2; // rot_from_parent (cached from calcAccelerations) - const btMatrix3x3 * rot_from_parent = &matrix_buf[0]; + const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; // hhat (cached), accel (scratch) - const btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0; - const btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0; - btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; - btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; - - // Y_i (scratch), D_i (cached) - btScalar * Y = r_ptr; r_ptr += num_links; - const btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0; + // hhat is NOT stored for the base (but ahat is) + const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); + btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; + v_ptr += num_links * 2 + 2; + + // Y_i (scratch), invD_i (cached) + const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; + btScalar * Y = r_ptr; + //////////////// + //aux variables + static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies + static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) + static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough + static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors + static btSpatialTransformationMatrix fromParent; + ///////////////// - btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size()); - btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); - - - // First 'upward' loop. // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. - - btVector3 input_force(force[3],force[4],force[5]); - btVector3 input_torque(force[0],force[1],force[2]); - - // Fill in zero_acc + + // Fill in zero_acc // -- set to force/torque on the base, zero otherwise - if (fixed_base) + if (m_fixedBase) { - zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0); + zeroAccSpatFrc[0].setZero(); } else - { - zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force); - zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque); + { + //test forces + fromParent.m_rotMat = rot_from_parent[0]; + fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]); } for (int i = 0; i < num_links; ++i) { - zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0); - } + zeroAccSpatFrc[i+1].setZero(); + } - // 'Downward' loop. - for (int i = num_links - 1; i >= 0; --i) + // 'Downward' loop. + // (part of TreeForwardDynamics in Mirtich.) + for (int i = num_links - 1; i >= 0; --i) { + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - Y[i] = - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); - Y[i] += force[6 + i]; // add joint torque - - const int parent = links[i].parent; - - // Zp += pXi * (Zi + hi*Yi/Di) - btVector3 in_top, in_bottom, out_top, out_bottom; - const btScalar Y_over_D = Y[i] / D[i]; - in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i]; - in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i]; - InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, - in_top, in_bottom, out_top, out_bottom); - zero_acc_top_angular[parent+1] += out_top; - zero_acc_bottom_linear[parent+1] += out_bottom; - } + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] + - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) + ; + } - // ptr to the joint accel part of the output + btVector3 in_top, in_bottom, out_top, out_bottom; + const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + invD_times_Y[dof] = 0.f; + + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; + } + } + + // Zp += pXi * (Zi + hi*Yi/Di) + spatForceVecTemps[0] = zeroAccSpatFrc[i+1]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + spatForceVecTemps[0] += hDof * invD_times_Y[dof]; + } + + + fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); + + zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; + } + + // ptr to the joint accel part of the output btScalar * joint_accel = output + 6; + // Second 'upward' loop - if (fixed_base) + // (part of TreeForwardDynamics in Mirtich) + + if (m_fixedBase) { - accel_top[0] = accel_bottom[0] = btVector3(0,0,0); - } else + spatAcc[0].setZero(); + } + else + { + solveImatrix(zeroAccSpatFrc[0], result); + spatAcc[0] = -result; + + } + + // now do the loop over the m_links + for (int i = 0; i < num_links; ++i) { - btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]); - btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]); + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + + fromParent.transform(spatAcc[parent+1], spatAcc[i+1]); - float result[6]; - solveImatrix(rhs_top,rhs_bot, result); - // printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]); + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); + } - for (int i = 0; i < 3; ++i) { - accel_top[0][i] = -result[i]; - accel_bottom[0][i] = -result[i+3]; - } + const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + mulMatrix(const_cast<btScalar*>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); - } - - // now do the loop over the links - for (int i = 0; i < num_links; ++i) { - const int parent = links[i].parent; - SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, - accel_top[parent+1], accel_bottom[parent+1], - accel_top[i+1], accel_bottom[i+1]); - joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i]; - accel_top[i+1] += joint_accel[i] * links[i].axis_top; - accel_bottom[i+1] += joint_accel[i] * links[i].axis_bottom; + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; } // transform base accelerations back to the world frame. btVector3 omegadot_out; - omegadot_out = rot_from_parent[0].transpose() * accel_top[0]; + omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); output[0] = omegadot_out[0]; output[1] = omegadot_out[1]; output[2] = omegadot_out[2]; btVector3 vdot_out; - vdot_out = rot_from_parent[0].transpose() * accel_bottom[0]; - + vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear(); output[3] = vdot_out[0]; output[4] = vdot_out[1]; output[5] = vdot_out[2]; + + ///////////////// + //printf("delta = ["); + //for(int dof = 0; dof < getNumDofs() + 6; ++dof) + // printf("%.2f ", output[dof]); + //printf("]\n"); + ///////////////// } -void btMultiBody::stepPositions(btScalar dt) -{ + + + +void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) +{ int num_links = getNumLinks(); // step position by adding dt * velocity - btVector3 v = getBaseVel(); - base_pos += dt * v; - - // "exponential map" method for the rotation - btVector3 base_omega = getBaseOmega(); - const btScalar omega_norm = base_omega.norm(); - const btScalar omega_times_dt = omega_norm * dt; - const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156 - if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE) + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + btScalar *pBasePos = (pq ? &pq[4] : m_basePos); + btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) + // + pBasePos[0] += dt * pBaseVel[0]; + pBasePos[1] += dt * pBaseVel[1]; + pBasePos[2] += dt * pBaseVel[2]; + + /////////////////////////////// + //local functor for quaternion integration (to avoid error prone redundancy) + struct { - const btScalar xsq = omega_times_dt * omega_times_dt; // |omega|^2 * dt^2 - const btScalar sin_term = dt * (xsq / 48.0f - 0.5f); // -sin(0.5*dt*|omega|) / |omega| - const btScalar cos_term = 1.0f - xsq / 8.0f; // cos(0.5*dt*|omega|) - base_quat = base_quat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term); - } else + //"exponential map" based on btTransformUtil::integrateTransform(..) + void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) + { + //baseBody => quat is alias and omega is global coor + //!baseBody => quat is alibi and omega is local coor + + btVector3 axis; + btVector3 angvel; + + if(!baseBody) + angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok + else + angvel = omega; + + btScalar fAngle = angvel.length(); + //limit the angular motion + if (fAngle * dt > ANGULAR_MOTION_THRESHOLD) + { + fAngle = btScalar(0.5)*SIMD_HALF_PI / dt; + } + + if ( fAngle < btScalar(0.001) ) + { + // use Taylor's expansions of sync function + axis = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle ); + } + else + { + // sync(fAngle) = sin(c*fAngle)/t + axis = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle ); + } + + if(!baseBody) + quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat; + else + quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) )); + //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse(); + + quat.normalize(); + } + } pQuatUpdateFun; + /////////////////////////////// + + //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); + // + btScalar *pBaseQuat = pq ? pq : m_baseQuat; + btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) + // + static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + pQuatUpdateFun(baseOmega, baseQuat, true, dt); + pBaseQuat[0] = baseQuat.x(); + pBaseQuat[1] = baseQuat.y(); + pBaseQuat[2] = baseQuat.z(); + pBaseQuat[3] = baseQuat.w(); + + + //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); + //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); + //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); + + if(pq) + pq += 7; + if(pqd) + pqd += 6; + + // Finally we can update m_jointPos for each of the m_links + for (int i = 0; i < num_links; ++i) { - base_quat = base_quat * btQuaternion(base_omega / omega_norm,-omega_times_dt); - } + btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]); + btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); - // Make sure the quaternion represents a valid rotation. - // (Not strictly necessary, but helps prevent any round-off errors from building up.) - base_quat.normalize(); + switch(m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + btScalar jointVel = pJointVel[0]; + pJointPos[0] += dt * jointVel; + break; + } + case btMultibodyLink::eSpherical: + { + static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); + static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + pQuatUpdateFun(jointVel, jointOri, false, dt); + pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w(); + break; + } + case btMultibodyLink::ePlanar: + { + pJointPos[0] += dt * getJointVelMultiDof(i)[0]; + + btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); + btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); + pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; + pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; + + break; + } + default: + { + } - // Finally we can update joint_pos for each of the links - for (int i = 0; i < num_links; ++i) - { - float jointVel = getJointVel(i); - links[i].joint_pos += dt * jointVel; - links[i].updateCache(); + } + + m_links[i].updateCacheMultiDof(pq); + + if(pq) + pq += m_links[i].m_posVarCount; + if(pqd) + pqd += m_links[i].m_dofCount; } } -void btMultiBody::fillContactJacobian(int link, +void btMultiBody::fillConstraintJacobianMultiDof(int link, const btVector3 &contact_point, - const btVector3 &normal, + const btVector3 &normal_ang, + const btVector3 &normal_lin, btScalar *jac, btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v, @@ -899,46 +1599,53 @@ void btMultiBody::fillContactJacobian(int link, { // temporary space int num_links = getNumLinks(); - scratch_v.resize(2*num_links + 2); + int m_dofCount = getNumDofs(); + scratch_v.resize(3*num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang scratch_m.resize(num_links + 1); btVector3 * v_ptr = &scratch_v[0]; - btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1; - btVector3 * n_local = v_ptr; v_ptr += num_links + 1; + btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1; + btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1; + btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1; btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); - scratch_r.resize(num_links); - btScalar * results = num_links > 0 ? &scratch_r[0] : 0; + scratch_r.resize(m_dofCount); + btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0; btMatrix3x3 * rot_from_world = &scratch_m[0]; - const btVector3 p_minus_com_world = contact_point - base_pos; + const btVector3 p_minus_com_world = contact_point - m_basePos; + const btVector3 &normal_lin_world = normal_lin; //convenience + const btVector3 &normal_ang_world = normal_ang; - rot_from_world[0] = btMatrix3x3(base_quat); - - p_minus_com[0] = rot_from_world[0] * p_minus_com_world; - n_local[0] = rot_from_world[0] * normal; + rot_from_world[0] = btMatrix3x3(m_baseQuat); // omega coeffients first. - btVector3 omega_coeffs; - omega_coeffs = p_minus_com_world.cross(normal); - jac[0] = omega_coeffs[0]; - jac[1] = omega_coeffs[1]; - jac[2] = omega_coeffs[2]; + btVector3 omega_coeffs_world; + omega_coeffs_world = p_minus_com_world.cross(normal_lin_world); + jac[0] = omega_coeffs_world[0] + normal_ang_world[0]; + jac[1] = omega_coeffs_world[1] + normal_ang_world[1]; + jac[2] = omega_coeffs_world[2] + normal_ang_world[2]; // then v coefficients - jac[3] = normal[0]; - jac[4] = normal[1]; - jac[5] = normal[2]; + jac[3] = normal_lin_world[0]; + jac[4] = normal_lin_world[1]; + jac[5] = normal_lin_world[2]; + + //create link-local versions of p_minus_com and normal + p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world; + n_local_lin[0] = rot_from_world[0] * normal_lin_world; + n_local_ang[0] = rot_from_world[0] * normal_ang_world; // Set remaining jac values to zero for now. - for (int i = 6; i < 6 + num_links; ++i) { + for (int i = 6; i < 6 + m_dofCount; ++i) + { jac[i] = 0; } // Qdot coefficients, if necessary. if (num_links > 0 && link > -1) { - // TODO: speed this up -- don't calculate for links we don't need. + // TODO: speed this up -- don't calculate for m_links we don't need. // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions, // which is resulting in repeated work being done...) @@ -946,64 +1653,292 @@ void btMultiBody::fillContactJacobian(int link, for (int i = 0; i < num_links; ++i) { // transform to local frame - const int parent = links[i].parent; - const btMatrix3x3 mtx(links[i].cached_rot_parent_to_this); + const int parent = m_links[i].m_parent; + const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis); rot_from_world[i+1] = mtx * rot_from_world[parent+1]; - n_local[i+1] = mtx * n_local[parent+1]; - p_minus_com[i+1] = mtx * p_minus_com[parent+1] - links[i].cached_r_vector; - - // calculate the jacobian entry - if (links[i].is_revolute) { - results[i] = n_local[i+1].dot( links[i].axis_top.cross(p_minus_com[i+1]) + links[i].axis_bottom ); - } else { - results[i] = n_local[i+1].dot( links[i].axis_bottom ); - } + n_local_lin[i+1] = mtx * n_local_lin[parent+1]; + n_local_ang[i+1] = mtx * n_local_ang[parent+1]; + p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector; + + // calculate the jacobian entry + switch(m_links[i].m_jointType) + { + case btMultibodyLink::eRevolute: + { + results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); + results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0)); + break; + } + case btMultibodyLink::ePrismatic: + { + results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0)); + break; + } + case btMultibodyLink::eSpherical: + { + results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); + results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1)); + results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2)); + + results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0)); + results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1)); + results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2)); + + break; + } + case btMultibodyLink::ePlanar: + { + results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0)); + results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1)); + results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2)); + + break; + } + default: + { + } + } + } // Now copy through to output. - while (link != -1) { - jac[6 + link] = results[link]; - link = links[link].parent; + //printf("jac[%d] = ", link); + while (link != -1) + { + for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) + { + jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof]; + //printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]); + } + + link = m_links[link].m_parent; } + //printf("]\n"); } } + void btMultiBody::wakeUp() { - awake = true; + m_awake = true; } void btMultiBody::goToSleep() { - awake = false; + m_awake = false; } void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) { - int num_links = getNumLinks(); extern bool gDisableDeactivation; - if (!can_sleep || gDisableDeactivation) + if (!m_canSleep || gDisableDeactivation) { - awake = true; - sleep_timer = 0; + m_awake = true; + m_sleepTimer = 0; return; } // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities) btScalar motion = 0; - for (int i = 0; i < 6 + num_links; ++i) { - motion += m_real_buf[i] * m_real_buf[i]; - } + { + for (int i = 0; i < 6 + m_dofCount; ++i) + motion += m_realBuf[i] * m_realBuf[i]; + } + if (motion < SLEEP_EPSILON) { - sleep_timer += timestep; - if (sleep_timer > SLEEP_TIMEOUT) { + m_sleepTimer += timestep; + if (m_sleepTimer > SLEEP_TIMEOUT) { goToSleep(); } } else { - sleep_timer = 0; - if (!awake) + m_sleepTimer = 0; + if (!m_awake) wakeUp(); } } + + +void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin) +{ + + int num_links = getNumLinks(); + + // Cached 3x3 rotation matrices from parent frame to this frame. + btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0]; + + rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? + + for (int i = 0; i < num_links; ++i) + { + rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); + } + + int nLinks = getNumLinks(); + ///base + num m_links + world_to_local.resize(nLinks+1); + local_origin.resize(nLinks+1); + + world_to_local[0] = getWorldToBaseRot(); + local_origin[0] = getBasePos(); + + for (int k=0;k<getNumLinks();k++) + { + const int parent = getParent(k); + world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1]; + local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k))); + } + + for (int link=0;link<getNumLinks();link++) + { + int index = link+1; + + btVector3 posr = local_origin[index]; + btScalar 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])); + getLink(link).m_cachedWorldTransform = tr; + + } + +} + +void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin) +{ + world_to_local.resize(getNumLinks()+1); + local_origin.resize(getNumLinks()+1); + + world_to_local[0] = getWorldToBaseRot(); + local_origin[0] = getBasePos(); + + if (getBaseCollider()) + { + btVector3 posr = local_origin[0]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar 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])); + + getBaseCollider()->setWorldTransform(tr); + + } + + for (int k=0;k<getNumLinks();k++) + { + const int parent = getParent(k); + world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1]; + local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k))); + } + + + for (int m=0;m<getNumLinks();m++) + { + btMultiBodyLinkCollider* col = 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}; + btScalar 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); + } + } +} + +int btMultiBody::calculateSerializeBufferSize() const +{ + int sz = sizeof(btMultiBodyData); + return sz; +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const +{ + btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer; + getBaseWorldTransform().serialize(mbd->m_baseWorldTransform); + mbd->m_baseMass = this->getBaseMass(); + getBaseInertia().serialize(mbd->m_baseInertia); + { + char* name = (char*) serializer->findNameForPointer(m_baseName); + mbd->m_baseName = (char*)serializer->getUniquePointer(name); + if (mbd->m_baseName) + { + serializer->serializeName(name); + } + } + mbd->m_numLinks = this->getNumLinks(); + if (mbd->m_numLinks) + { + int sz = sizeof(btMultiBodyLinkData); + int numElem = mbd->m_numLinks; + btChunk* chunk = serializer->allocate(sz,numElem); + btMultiBodyLinkData* memPtr = (btMultiBodyLinkData*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + + memPtr->m_jointType = getLink(i).m_jointType; + memPtr->m_dofCount = getLink(i).m_dofCount; + memPtr->m_posVarCount = getLink(i).m_posVarCount; + + getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia); + memPtr->m_linkMass = getLink(i).m_mass; + memPtr->m_parentIndex = getLink(i).m_parent; + getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset); + getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset); + getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis); + btAssert(memPtr->m_dofCount<=3); + for (int dof = 0;dof<getLink(i).m_dofCount;dof++) + { + getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]); + getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]); + + memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof]; + memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof]; + + } + int numPosVar = getLink(i).m_posVarCount; + for (int posvar = 0; posvar < numPosVar;posvar++) + { + memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar]; + } + + + { + char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName); + memPtr->m_linkName = (char*)serializer->getUniquePointer(name); + if (memPtr->m_linkName) + { + serializer->serializeName(name); + } + } + { + char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName); + memPtr->m_jointName = (char*)serializer->getUniquePointer(name); + if (memPtr->m_jointName) + { + serializer->serializeName(name); + } + } + memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider); + + } + serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]); + } + mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0; + + return btMultiBodyDataName; +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h index 7177bebbff5..8fbe6cda827 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h @@ -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 - ); - - void setupRevolute(int i, // 0 to num_links-1 + //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 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 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 + 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 &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 dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_deltaV[dof] = 0.f; } + } - for (int i = 0; i < 6 + getNumLinks(); ++i) + 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,12 +422,24 @@ 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 diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 44e04c3a132..12997d2e374 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -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); -} -btMultiBodyConstraint::~btMultiBodyConstraint() -{ } - - -btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow, - btMultiBodyJacobianData& data, - btScalar* jacOrgA,btScalar* jacOrgB, - const btContactSolverInfo& infoGlobal, - btScalar desiredVelocity, - btScalar lowerLimit, - btScalar upperLimit) +void btMultiBodyConstraint::updateJacobianSizes() { - - - - constraintRow.m_multiBodyA = m_bodyA; - constraintRow.m_multiBodyB = m_bodyB; - - btMultiBody* multiBodyA = constraintRow.m_multiBodyA; - btMultiBody* multiBodyB = constraintRow.m_multiBodyB; - - if (multiBodyA) + if(m_bodyA) { - - 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; - } - + m_jacSizeA = (6 + m_bodyA->getNumDofs()); } - - //compute rhs and remaining constraintRow fields - - - - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; + if(m_bodyB) { + m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs(); + } + else + m_jacSizeBoth = m_jacSizeA; +} - 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; +void btMultiBodyConstraint::allocateJacobiansMultiDof() +{ + updateJacobianSizes(); - } - return rel_vel; + m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); + m_data.resize((2 + m_jacSizeBoth) * m_numRows); } +btMultiBodyConstraint::~btMultiBodyConstraint() +{ +} 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 + } + else if(rb0) { - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); - } + 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; + //compute rhs and remaining solverConstraint fields + 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; + } diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 9fa317330b3..3b32b46e911 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -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; }; diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 577f846225b..4f66b20b2c1 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -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) { diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 0f4cd69c029..321ee4231a3 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -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); }; diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 0910f8f6abb..a9c0b33b3a3 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -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)); } + }//if (!isSleeping) + } + } +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + - bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); - } + { + 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,76 +754,245 @@ 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); + bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin); + + } else + { + bod->clearVelocities(); + } + } + } +} - - 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])); +void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint) +{ + m_multiBodyConstraints.push_back(constraint); +} - bod->getBaseCollider()->setWorldTransform(tr); +void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint) +{ + m_multiBodyConstraints.remove(constraint); +} - } - - 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))); - } +void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint) +{ + constraint->debugDraw(getDebugDrawer()); +} - 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); +void btMultiBodyDynamicsWorld::debugDrawWorld() +{ + BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld"); - int index = link+1; + bool drawConstraints = false; + if (getDebugDrawer()) + { + int mode = getDebugDrawer()->getDebugMode(); + if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) + { + drawConstraints = true; + } - 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])); + if (drawConstraints) + { + BT_PROFILE("btMultiBody debugDrawWorld"); + + btAlignedObjectArray<btQuaternion> world_to_local1; + btAlignedObjectArray<btVector3> local_origin1; - col->setWorldTransform(tr); + 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); } + } - } else - { - bod->clearVelocities(); } } } + + btDiscreteDynamicsWorld::debugDrawWorld(); } -void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint) +void btMultiBodyDynamicsWorld::applyGravity() { - m_multiBodyConstraints.push_back(constraint); + 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::removeMultiBodyConstraint( btMultiBodyConstraint* constraint) +void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces() +{ + for (int i=0;i<this->m_multiBodies.size();i++) + { + btMultiBody* bod = m_multiBodies[i]; + bod->clearConstraintForces(); + } +} +void btMultiBodyDynamicsWorld::clearMultiBodyForces() { - m_multiBodyConstraints.remove(constraint); + { + 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); + } + } + +}
\ No newline at end of file diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index ad57a346dcc..03ef3335c22 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -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 diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h new file mode 100644 index 00000000000..5c2fa8ed5b9 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h @@ -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 diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index ea309e8857d..3f05aa4d5fa 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -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 } } - - - + + + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h index 0c7fc170822..55b8d122b97 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -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 diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index ab5a430231b..062d19accaa 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -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); + } + }; + + } + } } - + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index 43869348141..011aadcfa4b 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -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 diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h index fbd54c45db3..668e4443904 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -24,84 +24,193 @@ 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 + // -// Link struct +// Various spatial helper functions // -struct btMultibodyLink -{ +//namespace { - BT_DECLARE_ALIGNED_ALLOCATOR(); - btScalar joint_pos; // qi +#include "LinearMath/btSpatialAlgebra.h" - btScalar mass; // mass of link - btVector3 inertia; // inertia of link (local frame; diagonal) +//} - int parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link. +// +// Link struct +// - btQuaternion zero_rot_parent_to_this; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant. +struct btMultibodyLink +{ - // "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; + BT_DECLARE_ALIGNED_ALLOCATOR(); - btVector3 d_vector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only. + btScalar m_mass; // mass of link + btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal) - // 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; + int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link. - bool is_revolute; // true = revolute, false = prismatic + btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant. - btQuaternion cached_rot_parent_to_this; // rotates vectors in parent frame to vectors in local frame - btVector3 cached_r_vector; // vector from COM of parent to COM of this link, in local frame. + btVector3 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; - btVector3 applied_force; // In WORLD frame - btVector3 applied_torque; // In WORLD frame - btScalar joint_torque; + btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity; + enum eFeatherstoneJointType + { + eRevolute = 0, + ePrismatic = 1, + eSpherical = 2, + ePlanar = 3, + eFixed = 4, + eInvalid + }; + + + + // "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; } + + int m_dofOffset, m_cfgOffset; + + 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) - { - 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 + btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); + + switch(m_jointType) { - // 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); + } } } }; diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index 27f12514a6d..5080ea87454 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -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; diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index f6690049156..12b21f74603 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -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); } } diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index 26ca12b406d..b2e219ac159 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -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 diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h index cf06dfb9ebf..6fa1550e9e6 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -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, diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp index 3bf7b5c1311..986f2148701 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp @@ -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); diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp new file mode 100644 index 00000000000..1f4015c7c72 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp @@ -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; + } + + diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h new file mode 100644 index 00000000000..7555cd9d207 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h @@ -0,0 +1,108 @@ +/* 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 (Kilian Grundl) +//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h +//STL/std::vector replaced by btAlignedObjectArray + + + +#ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_ +#define BT_NUMERICS_LEMKE_ALGORITHM_H_ + +#include "LinearMath/btMatrixX.h" + + +#include <vector> //todo: replace by btAlignedObjectArray + +class btLemkeAlgorithm +{ +public: + + + btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int & DEBUGLEVEL_ = 0) : + DEBUGLEVEL(DEBUGLEVEL_) + { + setSystem(M_, q_); + } + + /* GETTER / SETTER */ + /** + * \brief return info of solution process + */ + int getInfo() { + return info; + } + + /** + * \brief get the number of steps until the solution was found + */ + int getSteps(void) { + return steps; + } + + + + /** + * \brief set system with Matrix M and vector q + */ + void setSystem(const btMatrixXu & M_, const btVectorXu & q_) + { + m_M = M_; + m_q = q_; + } + /***************************************************/ + + /** + * \brief solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) + */ + btVectorXu solve(unsigned int maxloops = 0); + + virtual ~btLemkeAlgorithm() { + } + +protected: + int findLexicographicMinimum(const btMatrixXu &A, const int & pivotColIndex); + bool LexicographicPositive(const btVectorXu & v); + void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis); + bool greaterZero(const btVectorXu & vector); + bool validBasis(const btAlignedObjectArray<int>& basis); + + btMatrixXu m_M; + btVectorXu m_q; + + /** + * \brief number of steps until the Lemke algorithm found a solution + */ + unsigned int steps; + + /** + * \brief define level of debug output + */ + int DEBUGLEVEL; + + /** + * \brief did the algorithm find a solution + * + * -1 : not successful + * 0 : successful + */ + int info; +}; + + +#endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */ diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h new file mode 100644 index 00000000000..98484c37964 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h @@ -0,0 +1,350 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///original version written by Erwin Coumans, October 2013 + +#ifndef BT_LEMKE_SOLVER_H +#define BT_LEMKE_SOLVER_H + + +#include "btMLCPSolverInterface.h" +#include "btLemkeAlgorithm.h" + + + + +///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) " +///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time. +///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team +class btLemkeSolver : public btMLCPSolverInterface +{ +protected: + +public: + + btScalar m_maxValue; + int m_debugLevel; + int m_maxLoops; + bool m_useLoHighBounds; + + + + btLemkeSolver() + :m_maxValue(100000), + m_debugLevel(0), + m_maxLoops(1000), + m_useLoHighBounds(true) + { + } + virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) + { + + if (m_useLoHighBounds) + { + + BT_PROFILE("btLemkeSolver::solveMLCP"); + int n = A.rows(); + if (0==n) + return true; + + bool fail = false; + + btVectorXu solution(n); + btVectorXu q1; + q1.resize(n); + for (int row=0;row<n;row++) + { + q1[row] = -b[row]; + } + + // cout << "A" << endl; + // cout << A << endl; + + ///////////////////////////////////// + + //slow matrix inversion, replace with LU decomposition + btMatrixXu A1; + btMatrixXu B(n,n); + { + BT_PROFILE("inverse(slow)"); + A1.resize(A.rows(),A.cols()); + for (int row=0;row<A.rows();row++) + { + for (int col=0;col<A.cols();col++) + { + A1.setElem(row,col,A(row,col)); + } + } + + btMatrixXu matrix; + matrix.resize(n,2*n); + for (int row=0;row<n;row++) + { + for (int col=0;col<n;col++) + { + matrix.setElem(row,col,A1(row,col)); + } + } + + + btScalar ratio,a; + int i,j,k; + for(i = 0; i < n; i++){ + for(j = n; j < 2*n; j++){ + if(i==(j-n)) + matrix.setElem(i,j,1.0); + else + matrix.setElem(i,j,0.0); + } + } + for(i = 0; i < n; i++){ + for(j = 0; j < n; j++){ + if(i!=j) + { + btScalar v = matrix(i,i); + if (btFuzzyZero(v)) + { + a = 0.000001f; + } + ratio = matrix(j,i)/matrix(i,i); + for(k = 0; k < 2*n; k++){ + matrix.addElem(j,k,- ratio * matrix(i,k)); + } + } + } + } + for(i = 0; i < n; i++){ + a = matrix(i,i); + if (btFuzzyZero(a)) + { + a = 0.000001f; + } + btScalar invA = 1.f/a; + for(j = 0; j < 2*n; j++){ + matrix.mulElem(i,j,invA); + } + } + + + + + + for (int row=0;row<n;row++) + { + for (int col=0;col<n;col++) + { + B.setElem(row,col,matrix(row,n+col)); + } + } + } + + btMatrixXu b1(n,1); + + btMatrixXu M(n*2,n*2); + for (int row=0;row<n;row++) + { + b1.setElem(row,0,-b[row]); + for (int col=0;col<n;col++) + { + btScalar v =B(row,col); + M.setElem(row,col,v); + M.setElem(n+row,n+col,v); + M.setElem(n+row,col,-v); + M.setElem(row,n+col,-v); + + } + } + + btMatrixXu Bb1 = B*b1; +// q = [ (-B*b1 - lo)' (hi + B*b1)' ]' + + btVectorXu qq; + qq.resize(n*2); + for (int row=0;row<n;row++) + { + qq[row] = -Bb1(row,0)-lo[row]; + qq[n+row] = Bb1(row,0)+hi[row]; + } + + btVectorXu z1; + + btMatrixXu y1; + y1.resize(n,1); + btLemkeAlgorithm lemke(M,qq,m_debugLevel); + { + BT_PROFILE("lemke.solve"); + lemke.setSystem(M,qq); + z1 = lemke.solve(m_maxLoops); + } + for (int row=0;row<n;row++) + { + y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]); + } + btMatrixXu y1_b1(n,1); + for (int i=0;i<n;i++) + { + y1_b1.setElem(i,0,y1(i,0)-b1(i,0)); + } + + btMatrixXu x1; + + x1 = B*(y1_b1); + + for (int row=0;row<n;row++) + { + solution[row] = x1(row,0);//n]; + } + + int errorIndexMax = -1; + int errorIndexMin = -1; + float errorValueMax = -1e30; + float errorValueMin = 1e30; + + for (int i=0;i<n;i++) + { + x[i] = solution[i]; + volatile btScalar check = x[i]; + if (x[i] != check) + { + //printf("Lemke result is #NAN\n"); + x.setZero(); + return false; + } + + //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver + //we need to figure out why it happens, and fix it, or detect it properly) + if (x[i]>m_maxValue) + { + if (x[i]> errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i]<-m_maxValue) + { + if (x[i]<errorValueMin) + { + errorIndexMin = i; + errorValueMin = x[i]; + fail = true; + //printf("x[i] = %f,",x[i]); + } + } + } + if (fail) + { + int m_errorCountTimes = 0; + if (errorIndexMin<0) + errorValueMin = 0.f; + if (errorIndexMax<0) + errorValueMax = 0.f; + m_errorCountTimes++; + // printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); + for (int i=0;i<n;i++) + { + x[i]=0.f; + } + } + return !fail; + } else + + { + int dimension = A.rows(); + if (0==dimension) + return true; + +// printf("================ solving using Lemke/Newton/Fixpoint\n"); + + btVectorXu q; + q.resize(dimension); + for (int row=0;row<dimension;row++) + { + q[row] = -b[row]; + } + + btLemkeAlgorithm lemke(A,q,m_debugLevel); + + + lemke.setSystem(A,q); + + btVectorXu solution = lemke.solve(m_maxLoops); + + //check solution + + bool fail = false; + int errorIndexMax = -1; + int errorIndexMin = -1; + float errorValueMax = -1e30; + float errorValueMin = 1e30; + + for (int i=0;i<dimension;i++) + { + x[i] = solution[i+dimension]; + volatile btScalar check = x[i]; + if (x[i] != check) + { + x.setZero(); + return false; + } + + //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver + //we need to figure out why it happens, and fix it, or detect it properly) + if (x[i]>m_maxValue) + { + if (x[i]> errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i]<-m_maxValue) + { + if (x[i]<errorValueMin) + { + errorIndexMin = i; + errorValueMin = x[i]; + fail = true; + //printf("x[i] = %f,",x[i]); + } + } + } + if (fail) + { + static int errorCountTimes = 0; + if (errorIndexMin<0) + errorValueMin = 0.f; + if (errorIndexMax<0) + errorValueMax = 0.f; + printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); + for (int i=0;i<dimension;i++) + { + x[i]=0.f; + } + } + + + return !fail; + } + return true; + + } + +}; + +#endif //BT_LEMKE_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp index 0635b958bed..6688694a928 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -44,8 +44,8 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2; - int numBodies = m_tmpSolverBodyPool.size(); - m_allConstraintArray.resize(0); + // int numBodies = m_tmpSolverBodyPool.size(); + m_allConstraintPtrArray.resize(0); m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size()); btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size()); // printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size()); @@ -53,7 +53,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int dindex = 0; for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++) { - m_allConstraintArray.push_back(m_tmpSolverNonContactConstraintPool[i]); + m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]); m_limitDependencies[dindex++] = -1; } @@ -65,14 +65,14 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, { for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++) { - m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]); + m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); m_limitDependencies[dindex++] = -1; - m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]); + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]); int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact)); m_limitDependencies[dindex++] = findex +firstContactConstraintOffset; if (numFrictionPerContact==2) { - m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]); + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]); m_limitDependencies[dindex++] = findex+firstContactConstraintOffset; } } @@ -80,19 +80,19 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, { for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++) { - m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]); + m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); m_limitDependencies[dindex++] = -1; } for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++) { - m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i]); + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]); m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset; } } - if (!m_allConstraintArray.size()) + if (!m_allConstraintPtrArray.size()) { m_A.resize(0,0); m_b.resize(0); @@ -156,7 +156,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int numContactRows = interleaveContactAndFriction ? 3 : 1; - int numConstraintRows = m_allConstraintArray.size(); + int numConstraintRows = m_allConstraintPtrArray.size(); int n = numConstraintRows; { BT_PROFILE("init b (rhs)"); @@ -166,11 +166,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) m_bSplit.setZero(); for (int i=0;i<numConstraintRows ;i++) { - btScalar jacDiag = m_allConstraintArray[i].m_jacDiagABInv; + btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv; if (!btFuzzyZero(jacDiag)) { - btScalar rhs = m_allConstraintArray[i].m_rhs; - btScalar rhsPenetration = m_allConstraintArray[i].m_rhsPenetration; + btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; + btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; m_b[i]=rhs/jacDiag; m_bSplit[i] = rhsPenetration/jacDiag; } @@ -178,8 +178,8 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) } } - btScalar* w = 0; - int nub = 0; +// btScalar* w = 0; +// int nub = 0; m_lo.resize(numConstraintRows); m_hi.resize(numConstraintRows); @@ -195,14 +195,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) m_hi[i] = BT_INFINITY; } else { - m_lo[i] = m_allConstraintArray[i].m_lowerLimit; - m_hi[i] = m_allConstraintArray[i].m_upperLimit; + m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; } } } // - int m=m_allConstraintArray.size(); + int m=m_allConstraintPtrArray.size(); int numBodies = m_tmpSolverBodyPool.size(); btAlignedObjectArray<int> bodyJointNodeArray; @@ -213,7 +213,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) btAlignedObjectArray<btJointNode> jointNodeArray; { BT_PROFILE("jointNodeArray.reserve"); - jointNodeArray.reserve(2*m_allConstraintArray.size()); + jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); } static btMatrixXu J3; @@ -235,7 +235,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { BT_PROFILE("ofs resize"); ofs.resize(0); - ofs.resizeNoInitialize(m_allConstraintArray.size()); + ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); } { BT_PROFILE("Compute J and JinvM"); @@ -243,11 +243,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) int numRows = 0; - for (int i=0;i<m_allConstraintArray.size();i+=numRows,c++) + for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++) { ofs[c] = rowOffset; - int sbA = m_allConstraintArray[i].m_solverBodyIdA; - int sbB = m_allConstraintArray[i].m_solverBodyIdB; + int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; @@ -268,13 +268,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) } for (int row=0;row<numRows;row++,cur++) { - btVector3 normalInvMass = m_allConstraintArray[i+row].m_contactNormal1 * orgBodyA->getInvMass(); - btVector3 relPosCrossNormalInvInertia = m_allConstraintArray[i+row].m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); + btVector3 normalInvMass = m_allConstraintPtrArray[i+row]->m_contactNormal1 * orgBodyA->getInvMass(); + btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); for (int r=0;r<3;r++) { - J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal1[r]); - J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos1CrossNormal[r]); + J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]); + J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]); JinvM3.setElem(cur,r,normalInvMass[r]); JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); } @@ -305,13 +305,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) for (int row=0;row<numRows;row++,cur++) { - btVector3 normalInvMassB = m_allConstraintArray[i+row].m_contactNormal2*orgBodyB->getInvMass(); - btVector3 relPosInvInertiaB = m_allConstraintArray[i+row].m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); + btVector3 normalInvMassB = m_allConstraintPtrArray[i+row]->m_contactNormal2*orgBodyB->getInvMass(); + btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); for (int r=0;r<3;r++) { - J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal2[r]); - J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos2CrossNormal[r]); + J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]); + J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]); JinvM3.setElem(cur,r,normalInvMassB[r]); JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); } @@ -349,13 +349,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int numRows = 0; BT_PROFILE("Compute A"); - for (int i=0;i<m_allConstraintArray.size();i+= numRows,c++) + for (int i=0;i<m_allConstraintPtrArray.size();i+= numRows,c++) { int row__ = ofs[c]; - int sbA = m_allConstraintArray[i].m_solverBodyIdA; - int sbB = m_allConstraintArray[i].m_solverBodyIdB; - btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ; @@ -371,7 +371,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows; - size_t ofsother = (m_allConstraintArray[cr0].m_solverBodyIdB == sbA) ? 8*numRowsOther : 0; + size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8*numRowsOther : 0; //printf("%d joint i %d and j0: %d: ",count++,i,j0); m_A.multiplyAdd2_p8r ( JinvMrow, Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]); @@ -390,7 +390,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) if (j1<c) { int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows; - size_t ofsother = (m_allConstraintArray[cj1].m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; + size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]); } @@ -404,15 +404,15 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) // compute diagonal blocks of m_A int row__ = 0; - int numJointRows = m_allConstraintArray.size(); + int numJointRows = m_allConstraintPtrArray.size(); int jj=0; for (;row__<numJointRows;) { - int sbA = m_allConstraintArray[row__].m_solverBodyIdA; - int sbB = m_allConstraintArray[row__].m_solverBodyIdB; - btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + //int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; @@ -453,9 +453,9 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING) { - for (int i=0;i<m_allConstraintArray.size();i++) + for (int i=0;i<m_allConstraintPtrArray.size();i++) { - const btSolverConstraint& c = m_allConstraintArray[i]; + const btSolverConstraint& c = *m_allConstraintPtrArray[i]; m_x[i]=c.m_appliedImpulse; m_xSplit[i] = c.m_appliedPushImpulse; } @@ -471,7 +471,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) { int numBodies = this->m_tmpSolverBodyPool.size(); - int numConstraintRows = m_allConstraintArray.size(); + int numConstraintRows = m_allConstraintPtrArray.size(); m_b.resize(numConstraintRows); if (infoGlobal.m_splitImpulse) @@ -482,11 +482,11 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) for (int i=0;i<numConstraintRows ;i++) { - if (m_allConstraintArray[i].m_jacDiagABInv) + if (m_allConstraintPtrArray[i]->m_jacDiagABInv) { - m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv; + m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv; if (infoGlobal.m_splitImpulse) - m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv; + m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv; } } @@ -517,28 +517,28 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) for (int i=0;i<numConstraintRows;i++) { - m_lo[i] = m_allConstraintArray[i].m_lowerLimit; - m_hi[i] = m_allConstraintArray[i].m_upperLimit; + m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; - int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA; - int bodyIndex1 = m_allConstraintArray[i].m_solverBodyIdB; + int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB; if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) { - setElem(J,i,6*bodyIndex0+0,m_allConstraintArray[i].m_contactNormal1[0]); - setElem(J,i,6*bodyIndex0+1,m_allConstraintArray[i].m_contactNormal1[1]); - setElem(J,i,6*bodyIndex0+2,m_allConstraintArray[i].m_contactNormal1[2]); - setElem(J,i,6*bodyIndex0+3,m_allConstraintArray[i].m_relpos1CrossNormal[0]); - setElem(J,i,6*bodyIndex0+4,m_allConstraintArray[i].m_relpos1CrossNormal[1]); - setElem(J,i,6*bodyIndex0+5,m_allConstraintArray[i].m_relpos1CrossNormal[2]); + setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]); + setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]); + setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]); + setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); + setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); + setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); } if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) { - setElem(J,i,6*bodyIndex1+0,m_allConstraintArray[i].m_contactNormal2[0]); - setElem(J,i,6*bodyIndex1+1,m_allConstraintArray[i].m_contactNormal2[1]); - setElem(J,i,6*bodyIndex1+2,m_allConstraintArray[i].m_contactNormal2[2]); - setElem(J,i,6*bodyIndex1+3,m_allConstraintArray[i].m_relpos2CrossNormal[0]); - setElem(J,i,6*bodyIndex1+4,m_allConstraintArray[i].m_relpos2CrossNormal[1]); - setElem(J,i,6*bodyIndex1+5,m_allConstraintArray[i].m_relpos2CrossNormal[2]); + setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]); + setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]); + setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]); + setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); + setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); + setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); } } @@ -573,9 +573,9 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) m_xSplit.resize(numConstraintRows); // m_x.setZero(); - for (int i=0;i<m_allConstraintArray.size();i++) + for (int i=0;i<m_allConstraintPtrArray.size();i++) { - const btSolverConstraint& c = m_allConstraintArray[i]; + const btSolverConstraint& c = *m_allConstraintPtrArray[i]; m_x[i]=c.m_appliedImpulse; if (infoGlobal.m_splitImpulse) m_xSplit[i] = c.m_appliedPushImpulse; @@ -597,27 +597,33 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod if (result) { BT_PROFILE("process MLCP results"); - for (int i=0;i<m_allConstraintArray.size();i++) + for (int i=0;i<m_allConstraintPtrArray.size();i++) { { - btSolverConstraint& c = m_allConstraintArray[i]; + btSolverConstraint& c = *m_allConstraintPtrArray[i]; int sbA = c.m_solverBodyIdA; int sbB = c.m_solverBodyIdB; - btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + //btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA]; btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB]; - solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_x[i]); - solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_x[i]); + { + btScalar deltaImpulse = m_x[i]-c.m_appliedImpulse; + c.m_appliedImpulse = m_x[i]; + solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + } + if (infoGlobal.m_splitImpulse) { - solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_xSplit[i]); - solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_xSplit[i]); + btScalar deltaImpulse = m_xSplit[i] - c.m_appliedPushImpulse; + solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); c.m_appliedPushImpulse = m_xSplit[i]; } - c.m_appliedImpulse = m_x[i]; + } } } @@ -629,4 +635,6 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod } return 0.f; -}
\ No newline at end of file +} + + diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h index 31e8eb88ba5..43e85445bad 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h @@ -39,13 +39,15 @@ protected: btVectorXu m_xSplit2; btAlignedObjectArray<int> m_limitDependencies; - btConstraintArray m_allConstraintArray; + btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray; btMLCPSolverInterface* m_solver; int m_fallback; btScalar m_cfm; virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + virtual void createMLCP(const btContactSolverInfo& infoGlobal); virtual void createMLCPFast(const btContactSolverInfo& infoGlobal); diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h index d2ad54d21a8..77cc57c6e0e 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h @@ -62,7 +62,7 @@ public: } float aDiag = A(i,i); - x [i] = (b [i] - delta) / A(i,i); + x [i] = (b [i] - delta) / aDiag; float s = 1.f; if (limitDependency[i]>=0) diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index 77b475b9682..a7b1688469f 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -296,8 +296,9 @@ void btRaycastVehicle::updateVehicle( btScalar step ) int i=0; for (i=0;i<m_wheelInfo.size();i++) { - btScalar depth; - depth = rayCast( m_wheelInfo[i]); + //btScalar depth; + //depth = + rayCast( m_wheelInfo[i]); } updateSuspension(step); diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h index f59555f94d2..82d44c73e05 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -58,8 +58,6 @@ public: }; private: - btScalar m_tau; - btScalar m_damping; btVehicleRaycaster* m_vehicleRaycaster; btScalar m_pitchControl; btScalar m_steeringValue; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp index 2bfd62f2a2f..51f4b33d034 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp @@ -3605,8 +3605,8 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]); m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]); memPtr->m_cfm = m_joints[i]->m_cfm; - memPtr->m_erp = m_joints[i]->m_erp; - memPtr->m_split = m_joints[i]->m_split; + memPtr->m_erp = float(m_joints[i]->m_erp); + memPtr->m_split = float(m_joints[i]->m_split); memPtr->m_delete = m_joints[i]->m_delete; for (int j=0;j<4;j++) diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.h b/extern/bullet2/src/BulletSoftBody/btSoftBody.h index ee1a3d95228..bd5846bfb67 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.h @@ -171,6 +171,7 @@ public: /* ImplicitFn */ struct ImplicitFn { + virtual ~ImplicitFn() {} virtual btScalar Eval(const btVector3& x)=0; }; @@ -528,6 +529,7 @@ public: { struct IControl { + virtual ~IControl() {} virtual void Prepare(AJoint*) {} virtual btScalar Speed(AJoint*,btScalar current) { return(current); } static IControl* Default() { static IControl def;return(&def); } diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp index 36f675a6c0c..293a393e55e 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -480,6 +480,168 @@ void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); } + +//The btSoftBody object from the BulletSDK includes an array of Nodes and Links. These links appear +// to be first set up to connect a node to between 5 and 6 of its neighbors [480 links], +//and then to the rest of the nodes after the execution of the Floyd-Warshall graph algorithm +//[another 930 links]. +//The way the links are stored by default, we have a number of cases where adjacent links share a node in common +// - this leads to the creation of a data dependency through memory. +//The PSolve_Links() function reads and writes nodes as it iterates over each link. +//So, we now have the possibility of a data dependency between iteration X +//that processes link L with iteration X+1 that processes link L+1 +//because L and L+1 have one node in common, and iteration X updates the positions of that node, +//and iteration X+1 reads in the position of that shared node. +// +//Such a memory dependency limits the ability of a modern CPU to speculate beyond +//a certain point because it has to respect a possible dependency +//- this prevents the CPU from making full use of its out-of-order resources. +//If we re-order the links such that we minimize the cases where a link L and L+1 share a common node, +//we create a temporal gap between when the node position is written, +//and when it is subsequently read. This in turn allows the CPU to continue execution without +//risking a dependency violation. Such a reordering would result in significant speedups on +//modern CPUs with lots of execution resources. +//In our testing, we see it have a tremendous impact not only on the A7, +//but also on all x86 cores that ship with modern Macs. +//The attached source file includes a single function (ReoptimizeLinkOrder) which can be called on a +//btSoftBody object in the solveConstraints() function before the actual solver is invoked, +//or right after generateBendingConstraints() once we have all 1410 links. + + +//=================================================================== +// +// +// This function takes in a list of interdependent Links and tries +// to maximize the distance between calculation +// of dependent links. This increases the amount of parallelism that can +// be exploited by out-of-order instruction processors with large but +// (inevitably) finite instruction windows. +// +//=================================================================== + +// A small structure to track lists of dependent link calculations +class LinkDeps_t { + public: + int value; // A link calculation that is dependent on this one + // Positive values = "input A" while negative values = "input B" + LinkDeps_t *next; // Next dependence in the list +}; +typedef LinkDeps_t *LinkDepsPtr_t; + +// Dependency list constants +#define REOP_NOT_DEPENDENT -1 +#define REOP_NODE_COMPLETE -2 // Must be less than REOP_NOT_DEPENDENT + + +void btSoftBodyHelpers::ReoptimizeLinkOrder(btSoftBody *psb /* This can be replaced by a btSoftBody pointer */) +{ + int i, nLinks=psb->m_links.size(), nNodes=psb->m_nodes.size(); + btSoftBody::Link *lr; + int ar, br; + btSoftBody::Node *node0 = &(psb->m_nodes[0]); + btSoftBody::Node *node1 = &(psb->m_nodes[1]); + LinkDepsPtr_t linkDep; + int readyListHead, readyListTail, linkNum, linkDepFrees, depLink; + + // Allocate temporary buffers + int *nodeWrittenAt = new int[nNodes+1]; // What link calculation produced this node's current values? + int *linkDepA = new int[nLinks]; // Link calculation input is dependent upon prior calculation #N + int *linkDepB = new int[nLinks]; + int *readyList = new int[nLinks]; // List of ready-to-process link calculations (# of links, maximum) + LinkDeps_t *linkDepFreeList = new LinkDeps_t[2*nLinks]; // Dependent-on-me list elements (2x# of links, maximum) + LinkDepsPtr_t *linkDepListStarts = new LinkDepsPtr_t[nLinks]; // Start nodes of dependent-on-me lists, one for each link + + // Copy the original, unsorted links to a side buffer + btSoftBody::Link *linkBuffer = new btSoftBody::Link[nLinks]; + memcpy(linkBuffer, &(psb->m_links[0]), sizeof(btSoftBody::Link)*nLinks); + + // Clear out the node setup and ready list + for (i=0; i < nNodes+1; i++) { + nodeWrittenAt[i] = REOP_NOT_DEPENDENT; + } + for (i=0; i < nLinks; i++) { + linkDepListStarts[i] = NULL; + } + readyListHead = readyListTail = linkDepFrees = 0; + + // Initial link analysis to set up data structures + for (i=0; i < nLinks; i++) { + + // Note which prior link calculations we are dependent upon & build up dependence lists + lr = &(psb->m_links[i]); + ar = (lr->m_n[0] - node0)/(node1 - node0); + br = (lr->m_n[1] - node0)/(node1 - node0); + if (nodeWrittenAt[ar] > REOP_NOT_DEPENDENT) { + linkDepA[i] = nodeWrittenAt[ar]; + linkDep = &linkDepFreeList[linkDepFrees++]; + linkDep->value = i; + linkDep->next = linkDepListStarts[nodeWrittenAt[ar]]; + linkDepListStarts[nodeWrittenAt[ar]] = linkDep; + } else { + linkDepA[i] = REOP_NOT_DEPENDENT; + } + if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT) { + linkDepB[i] = nodeWrittenAt[br]; + linkDep = &linkDepFreeList[linkDepFrees++]; + linkDep->value = -(i+1); + linkDep->next = linkDepListStarts[nodeWrittenAt[br]]; + linkDepListStarts[nodeWrittenAt[br]] = linkDep; + } else { + linkDepB[i] = REOP_NOT_DEPENDENT; + } + + // Add this link to the initial ready list, if it is not dependent on any other links + if ((linkDepA[i] == REOP_NOT_DEPENDENT) && (linkDepB[i] == REOP_NOT_DEPENDENT)) { + readyList[readyListTail++] = i; + linkDepA[i] = linkDepB[i] = REOP_NODE_COMPLETE; // Probably not needed now + } + + // Update the nodes to mark which ones are calculated by this link + nodeWrittenAt[ar] = nodeWrittenAt[br] = i; + } + + // Process the ready list and create the sorted list of links + // -- By treating the ready list as a queue, we maximize the distance between any + // inter-dependent node calculations + // -- All other (non-related) nodes in the ready list will automatically be inserted + // in between each set of inter-dependent link calculations by this loop + i = 0; + while (readyListHead != readyListTail) { + // Use ready list to select the next link to process + linkNum = readyList[readyListHead++]; + // Copy the next-to-calculate link back into the original link array + psb->m_links[i++] = linkBuffer[linkNum]; + + // Free up any link inputs that are dependent on this one + linkDep = linkDepListStarts[linkNum]; + while (linkDep) { + depLink = linkDep->value; + if (depLink >= 0) { + linkDepA[depLink] = REOP_NOT_DEPENDENT; + } else { + depLink = -depLink - 1; + linkDepB[depLink] = REOP_NOT_DEPENDENT; + } + // Add this dependent link calculation to the ready list if *both* inputs are clear + if ((linkDepA[depLink] == REOP_NOT_DEPENDENT) && (linkDepB[depLink] == REOP_NOT_DEPENDENT)) { + readyList[readyListTail++] = depLink; + linkDepA[depLink] = linkDepB[depLink] = REOP_NODE_COMPLETE; // Probably not needed now + } + linkDep = linkDep->next; + } + } + + // Delete the temporary buffers + delete [] nodeWrittenAt; + delete [] linkDepA; + delete [] linkDepB; + delete [] readyList; + delete [] linkDepFreeList; + delete [] linkDepListStarts; + delete [] linkBuffer; +} + + // void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, btIDebugDraw* idraw) diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h index 620a52fe394..7271530109a 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h @@ -137,7 +137,12 @@ struct btSoftBodyHelpers bool bfacelinks, bool btetralinks, bool bfacesfromtetras); - + + /// Sort the list of links to move link calculations that are dependent upon earlier + /// ones as far as possible away from the calculation of those values + /// This tends to make adjacent loop iterations not dependent upon one another, + /// so out-of-order processors can execute instructions from multiple iterations at once + static void ReoptimizeLinkOrder(btSoftBody *psb ); }; #endif //BT_SOFT_BODY_HELPERS_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h index 19d0543ef9e..759509a1d86 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h @@ -161,7 +161,7 @@ public: } virtual btScalar getMargin() const { - return getMargin(); + return btConvexInternalShape::getMargin(); } }; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index 5f35935450c..653d5a06b49 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -76,7 +76,7 @@ void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep ); { BT_PROFILE("predictUnconstraintMotionSoftBody"); - m_softBodySolver->predictMotion( timeStep ); + m_softBodySolver->predictMotion( float(timeStep) ); } } diff --git a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h index bcf0c798253..8992ddbb68d 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/btAlignedObjectArray.h b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h index 24e59ab65d7..6193ef7f427 100644 --- a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h +++ b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h @@ -39,6 +39,12 @@ subject to the following restrictions: #include <new> //for placement new #endif //BT_USE_PLACEMENT_NEW +// The register keyword is deprecated in C++11 so don't use it. +#if __cplusplus > 199711L +#define BT_REGISTER +#else +#define BT_REGISTER register +#endif ///The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods ///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data @@ -202,24 +208,16 @@ protected: ///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations. SIMD_FORCE_INLINE void resizeNoInitialize(int newsize) { - int curSize = size(); - - if (newsize < curSize) + if (newsize > size()) { - } else - { - if (newsize > size()) - { - reserve(newsize); - } - //leave this uninitialized + reserve(newsize); } m_size = newsize; } SIMD_FORCE_INLINE void resize(int newsize, const T& fillData=T()) { - int curSize = size(); + const BT_REGISTER int curSize = size(); if (newsize < curSize) { @@ -229,7 +227,7 @@ protected: } } else { - if (newsize > size()) + if (newsize > curSize) { reserve(newsize); } @@ -246,7 +244,7 @@ protected: } SIMD_FORCE_INLINE T& expandNonInitializing( ) { - int sz = size(); + const BT_REGISTER int sz = size(); if( sz == capacity() ) { reserve( allocSize(size()) ); @@ -259,7 +257,7 @@ protected: SIMD_FORCE_INLINE T& expand( const T& fillValue=T()) { - int sz = size(); + const BT_REGISTER int sz = size(); if( sz == capacity() ) { reserve( allocSize(size()) ); @@ -275,7 +273,7 @@ protected: SIMD_FORCE_INLINE void push_back(const T& _Val) { - int sz = size(); + const BT_REGISTER int sz = size(); if( sz == capacity() ) { reserve( allocSize(size()) ); diff --git a/extern/bullet2/src/LinearMath/btCpuFeatureUtility.h b/extern/bullet2/src/LinearMath/btCpuFeatureUtility.h new file mode 100644 index 00000000000..d2cab52d488 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btCpuFeatureUtility.h @@ -0,0 +1,92 @@ + +#ifndef BT_CPU_UTILITY_H +#define BT_CPU_UTILITY_H + +#include "LinearMath/btScalar.h" + +#include <string.h>//memset +#ifdef USE_SIMD +#include <emmintrin.h> +#ifdef BT_ALLOW_SSE4 +#include <intrin.h> +#endif //BT_ALLOW_SSE4 +#endif //USE_SIMD + +#if defined BT_USE_NEON +#define ARM_NEON_GCC_COMPATIBILITY 1 +#include <arm_neon.h> +#include <sys/types.h> +#include <sys/sysctl.h> //for sysctlbyname +#endif //BT_USE_NEON + +///Rudimentary btCpuFeatureUtility for CPU features: only report the features that Bullet actually uses (SSE4/FMA3, NEON_HPFP) +///We assume SSE2 in case BT_USE_SSE2 is defined in LinearMath/btScalar.h +class btCpuFeatureUtility +{ +public: + enum btCpuFeature + { + CPU_FEATURE_FMA3=1, + CPU_FEATURE_SSE4_1=2, + CPU_FEATURE_NEON_HPFP=4 + }; + + static int getCpuFeatures() + { + + static int capabilities = 0; + static bool testedCapabilities = false; + if (0 != testedCapabilities) + { + return capabilities; + } + +#ifdef BT_USE_NEON + { + uint32_t hasFeature = 0; + size_t featureSize = sizeof(hasFeature); + int err = sysctlbyname("hw.optional.neon_hpfp", &hasFeature, &featureSize, NULL, 0); + if (0 == err && hasFeature) + capabilities |= CPU_FEATURE_NEON_HPFP; + } +#endif //BT_USE_NEON + +#ifdef BT_ALLOW_SSE4 + { + int cpuInfo[4]; + memset(cpuInfo, 0, sizeof(cpuInfo)); + unsigned long long sseExt = 0; + __cpuid(cpuInfo, 1); + + bool osUsesXSAVE_XRSTORE = cpuInfo[2] & (1 << 27) || false; + bool cpuAVXSuport = cpuInfo[2] & (1 << 28) || false; + + if (osUsesXSAVE_XRSTORE && cpuAVXSuport) + { + sseExt = _xgetbv(0); + } + const int OSXSAVEFlag = (1UL << 27); + const int AVXFlag = ((1UL << 28) | OSXSAVEFlag); + const int FMAFlag = ((1UL << 12) | AVXFlag | OSXSAVEFlag); + if ((cpuInfo[2] & FMAFlag) == FMAFlag && (sseExt & 6) == 6) + { + capabilities |= btCpuFeatureUtility::CPU_FEATURE_FMA3; + } + + const int SSE41Flag = (1 << 19); + if (cpuInfo[2] & SSE41Flag) + { + capabilities |= btCpuFeatureUtility::CPU_FEATURE_SSE4_1; + } + } +#endif//BT_ALLOW_SSE4 + + testedCapabilities = true; + return capabilities; + } + + +}; + + +#endif //BT_CPU_UTILITY_H diff --git a/extern/bullet2/src/LinearMath/btDefaultMotionState.h b/extern/bullet2/src/LinearMath/btDefaultMotionState.h index c90b749230c..01c5f8d932d 100644 --- a/extern/bullet2/src/LinearMath/btDefaultMotionState.h +++ b/extern/bullet2/src/LinearMath/btDefaultMotionState.h @@ -25,14 +25,14 @@ ATTRIBUTE_ALIGNED16(struct) btDefaultMotionState : public btMotionState ///synchronizes world transform from user to physics virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) const { - centerOfMassWorldTrans = m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ; + centerOfMassWorldTrans = m_graphicsWorldTrans * m_centerOfMassOffset.inverse() ; } ///synchronizes world transform from physics to user ///Bullet only calls the update of worldtransform for active objects virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans) { - m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ; + m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset; } diff --git a/extern/bullet2/src/LinearMath/btGrahamScan2dConvexHull.h b/extern/bullet2/src/LinearMath/btGrahamScan2dConvexHull.h index e658c5cf062..13a79aa5856 100644 --- a/extern/bullet2/src/LinearMath/btGrahamScan2dConvexHull.h +++ b/extern/bullet2/src/LinearMath/btGrahamScan2dConvexHull.h @@ -85,9 +85,17 @@ inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector3>& original originalPoints[0].m_angle = -1e30f; for (int i=1;i<originalPoints.size();i++) { - btVector3 xvec = axis0; - btVector3 ar = originalPoints[i]-originalPoints[0]; - originalPoints[i].m_angle = btCross(xvec, ar).dot(normalAxis) / ar.length(); + btVector3 ar = originalPoints[i]-originalPoints[0]; + btScalar ar1 = axis1.dot(ar); + btScalar ar0 = axis0.dot(ar); + if( ar1*ar1+ar0*ar0 < FLT_EPSILON ) + { + originalPoints[i].m_angle = 0.0f; + } + else + { + originalPoints[i].m_angle = btAtan2Fast(ar1, ar0); + } } //step 2: sort all points, based on 'angle' with this anchor @@ -111,6 +119,11 @@ inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector3>& original else hull.push_back(originalPoints[i]); } + + if( hull.size() == 1 ) + { + hull.push_back( originalPoints[i] ); + } } } diff --git a/extern/bullet2/src/LinearMath/btHashMap.h b/extern/bullet2/src/LinearMath/btHashMap.h index ce07db3ac6b..af9727b7ada 100644 --- a/extern/bullet2/src/LinearMath/btHashMap.h +++ b/extern/bullet2/src/LinearMath/btHashMap.h @@ -399,6 +399,10 @@ protected: return find(key); } + const Value* operator[](const Key& key) const { + return find(key); + } + const Value* find(const Key& key) const { int index = findIndex(key); diff --git a/extern/bullet2/src/LinearMath/btIDebugDraw.h b/extern/bullet2/src/LinearMath/btIDebugDraw.h index de97c3f87fd..58c9838c490 100644 --- a/extern/bullet2/src/LinearMath/btIDebugDraw.h +++ b/extern/bullet2/src/LinearMath/btIDebugDraw.h @@ -21,6 +21,7 @@ subject to the following restrictions: #include "btTransform.h" + ///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations. ///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld. ///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum. @@ -29,6 +30,29 @@ class btIDebugDraw { public: + ATTRIBUTE_ALIGNED16(struct) DefaultColors + { + btVector3 m_activeObject; + btVector3 m_deactivatedObject; + btVector3 m_wantsDeactivationObject; + btVector3 m_disabledDeactivationObject; + btVector3 m_disabledSimulationObject; + btVector3 m_aabb; + btVector3 m_contactPoint; + + DefaultColors() + : m_activeObject(1,1,1), + m_deactivatedObject(0,1,0), + m_wantsDeactivationObject(0,1,1), + m_disabledDeactivationObject(1,0,0), + m_disabledSimulationObject(1,1,0), + m_aabb(1,0,0), + m_contactPoint(1,1,0) + { + } + }; + + enum DebugDrawModes { DBG_NoDebug=0, @@ -46,12 +70,18 @@ class btIDebugDraw DBG_DrawConstraints = (1 << 11), DBG_DrawConstraintLimits = (1 << 12), DBG_FastWireframe = (1<<13), - DBG_DrawNormals = (1<<14), + DBG_DrawNormals = (1<<14), + DBG_DrawFrames = (1<<15), DBG_MAX_DEBUG_DRAW_MODE }; virtual ~btIDebugDraw() {}; + + virtual DefaultColors getDefaultColors() const { DefaultColors colors; return colors; } + ///the default implementation for setDefaultColors has no effect. A derived class can implement it and store the colors. + virtual void setDefaultColors(const DefaultColors& /*colors*/) {} + virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0; virtual void drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor) @@ -147,7 +177,7 @@ class btIDebugDraw const btVector3& vx = axis; btVector3 vy = normal.cross(axis); btScalar step = stepDegrees * SIMD_RADS_PER_DEG; - int nSteps = (int)((maxAngle - minAngle) / step); + int nSteps = (int)btFabs((maxAngle - minAngle) / step); if(!nSteps) nSteps = 1; btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle); if(drawSect) @@ -438,6 +468,10 @@ class btIDebugDraw drawLine(transform*pt0,transform*pt1,color); drawLine(transform*pt2,transform*pt3,color); } + + virtual void flushLines() + { + } }; diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet2/src/LinearMath/btMatrix3x3.h index 14fe704f81a..41dea694835 100644 --- a/extern/bullet2/src/LinearMath/btMatrix3x3.h +++ b/extern/bullet2/src/LinearMath/btMatrix3x3.h @@ -610,6 +610,27 @@ public: /**@brief Return the inverse of the matrix */ btMatrix3x3 inverse() const; + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + ///Solve33 is from Box2d, thanks to Erin Catto, + btVector3 solve33(const btVector3& b) const + { + btVector3 col1 = getColumn(0); + btVector3 col2 = getColumn(1); + btVector3 col3 = getColumn(2); + + btScalar det = btDot(col1, btCross(col2, col3)); + if (btFabs(det)>SIMD_EPSILON) + { + det = 1.0f / det; + } + btVector3 x; + x[0] = det * btDot(b, btCross(col2, col3)); + x[1] = det * btDot(col1, btCross(b, col3)); + x[2] = det * btDot(col1, btCross(col2, b)); + return x; + } + btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; btMatrix3x3 timesTranspose(const btMatrix3x3& m) const; diff --git a/extern/bullet2/src/LinearMath/btMatrixX.h b/extern/bullet2/src/LinearMath/btMatrixX.h index 865d77967a9..42caed42eff 100644 --- a/extern/bullet2/src/LinearMath/btMatrixX.h +++ b/extern/bullet2/src/LinearMath/btMatrixX.h @@ -19,6 +19,7 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "LinearMath/btAlignedObjectArray.h" +#include <stdio.h> //#define BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM @@ -94,7 +95,7 @@ struct btVectorX { T temp; temp = scale / absxi; - ssq = ssq * (temp * temp) + 1.0; + ssq = ssq * (temp * temp) + BT_ONE; scale = absxi; } else @@ -354,11 +355,11 @@ struct btMatrixX for (int i=0; i < res.rows(); ++i) { T dotProd=0; - T dotProd2=0; - int waste=0,waste2=0; +// T dotProd2=0; + //int waste=0,waste2=0; { - bool useOtherCol = true; +// bool useOtherCol = true; { for (int v=0;v<rows();v++) { diff --git a/extern/bullet2/src/LinearMath/btQuadWord.h b/extern/bullet2/src/LinearMath/btQuadWord.h index 11067ef47d9..fcfb3be4447 100644 --- a/extern/bullet2/src/LinearMath/btQuadWord.h +++ b/extern/bullet2/src/LinearMath/btQuadWord.h @@ -73,7 +73,7 @@ public: public: -#if defined(BT_USE_SSE) || defined(BT_USE_NEON) +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) // Set Vector SIMD_FORCE_INLINE btQuadWord(const btSimdFloat4 vec) diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet2/src/LinearMath/btQuaternion.h index 665421de1e4..ede76938404 100644 --- a/extern/bullet2/src/LinearMath/btQuaternion.h +++ b/extern/bullet2/src/LinearMath/btQuaternion.h @@ -22,6 +22,13 @@ subject to the following restrictions: #include "btQuadWord.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define btQuaternionData btQuaternionDoubleData +#define btQuaternionDataName "btQuaternionDoubleData" +#else +#define btQuaternionData btQuaternionFloatData +#define btQuaternionDataName "btQuaternionFloatData" +#endif //BT_USE_DOUBLE_PRECISION @@ -560,7 +567,18 @@ public: SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; } - + SIMD_FORCE_INLINE void serialize(struct btQuaternionData& dataOut) const; + + SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionData& dataIn); + + SIMD_FORCE_INLINE void serializeFloat(struct btQuaternionFloatData& dataOut) const; + + SIMD_FORCE_INLINE void deSerializeFloat(const struct btQuaternionFloatData& dataIn); + + SIMD_FORCE_INLINE void serializeDouble(struct btQuaternionDoubleData& dataOut) const; + + SIMD_FORCE_INLINE void deSerializeDouble(const struct btQuaternionDoubleData& dataIn); + }; @@ -903,6 +921,62 @@ shortestArcQuatNormalize2(btVector3& v0,btVector3& v1) return shortestArcQuat(v0,v1); } + + + +struct btQuaternionFloatData +{ + float m_floats[4]; +}; + +struct btQuaternionDoubleData +{ + double m_floats[4]; + +}; + +SIMD_FORCE_INLINE void btQuaternion::serializeFloat(struct btQuaternionFloatData& dataOut) const +{ + ///could also do a memcpy, check if it is worth it + for (int i=0;i<4;i++) + dataOut.m_floats[i] = float(m_floats[i]); +} + +SIMD_FORCE_INLINE void btQuaternion::deSerializeFloat(const struct btQuaternionFloatData& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = btScalar(dataIn.m_floats[i]); +} + + +SIMD_FORCE_INLINE void btQuaternion::serializeDouble(struct btQuaternionDoubleData& dataOut) const +{ + ///could also do a memcpy, check if it is worth it + for (int i=0;i<4;i++) + dataOut.m_floats[i] = double(m_floats[i]); +} + +SIMD_FORCE_INLINE void btQuaternion::deSerializeDouble(const struct btQuaternionDoubleData& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = btScalar(dataIn.m_floats[i]); +} + + +SIMD_FORCE_INLINE void btQuaternion::serialize(struct btQuaternionData& dataOut) const +{ + ///could also do a memcpy, check if it is worth it + for (int i=0;i<4;i++) + dataOut.m_floats[i] = m_floats[i]; +} + +SIMD_FORCE_INLINE void btQuaternion::deSerialize(const struct btQuaternionData& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = dataIn.m_floats[i]; +} + + #endif //BT_SIMD__QUATERNION_H_ diff --git a/extern/bullet2/src/LinearMath/btQuickprof.cpp b/extern/bullet2/src/LinearMath/btQuickprof.cpp index 544aee89d02..d88d965a4cc 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.cpp +++ b/extern/bullet2/src/LinearMath/btQuickprof.cpp @@ -10,7 +10,7 @@ ** ***************************************************************************************************/ -// Credits: The Clock class was inspired by the Timer classes in +// Credits: The Clock class was inspired by the Timer classes in // Ogre (www.ogre3d.org). #include "btQuickprof.h" @@ -27,8 +27,8 @@ static btClock gProfileClock; #include <stdio.h> #endif -#if defined (SUNOS) || defined (__SUNOS__) -#include <stdio.h> +#if defined (SUNOS) || defined (__SUNOS__) +#include <stdio.h> #endif #if defined(WIN32) || defined(_WIN32) @@ -37,12 +37,17 @@ static btClock gProfileClock; #define WIN32_LEAN_AND_MEAN #define NOWINRES #define NOMCX -#define NOIME +#define NOIME #ifdef _XBOX #include <Xtl.h> #else //_XBOX #include <windows.h> + +#if WINVER <0x0602 +#define GetTickCount64 GetTickCount +#endif + #endif //_XBOX #include <time.h> @@ -59,7 +64,7 @@ struct btClockData #ifdef BT_USE_WINDOWS_TIMERS LARGE_INTEGER mClockFrequency; - DWORD mStartTick; + LONGLONG mStartTick; LONGLONG mPrevElapsedTime; LARGE_INTEGER mStartTime; #else @@ -105,7 +110,7 @@ void btClock::reset() { #ifdef BT_USE_WINDOWS_TIMERS QueryPerformanceCounter(&m_data->mStartTime); - m_data->mStartTick = GetTickCount(); + m_data->mStartTick = GetTickCount64(); m_data->mPrevElapsedTime = 0; #else #ifdef __CELLOS_LV2__ @@ -121,34 +126,34 @@ void btClock::reset() #endif } -/// Returns the time in ms since the last call to reset or since +/// Returns the time in ms since the last call to reset or since /// the btClock was created. unsigned long int btClock::getTimeMilliseconds() { #ifdef BT_USE_WINDOWS_TIMERS LARGE_INTEGER currentTime; QueryPerformanceCounter(¤tTime); - LONGLONG elapsedTime = currentTime.QuadPart - + LONGLONG elapsedTime = currentTime.QuadPart - m_data->mStartTime.QuadPart; // Compute the number of millisecond ticks elapsed. - unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / + unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / m_data->mClockFrequency.QuadPart); - // Check for unexpected leaps in the Win32 performance counter. - // (This is caused by unexpected data across the PCI to ISA + // Check for unexpected leaps in the Win32 performance counter. + // (This is caused by unexpected data across the PCI to ISA // bridge, aka south bridge. See Microsoft KB274323.) - unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick; + unsigned long elapsedTicks = (unsigned long)(GetTickCount64() - m_data->mStartTick); signed long msecOff = (signed long)(msecTicks - elapsedTicks); if (msecOff < -100 || msecOff > 100) { // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - m_data->mClockFrequency.QuadPart / 1000, elapsedTime - + LONGLONG msecAdjustment = mymin(msecOff * + m_data->mClockFrequency.QuadPart / 1000, elapsedTime - m_data->mPrevElapsedTime); m_data->mStartTime.QuadPart += msecAdjustment; elapsedTime -= msecAdjustment; // Recompute the number of millisecond ticks elapsed. - msecTicks = (unsigned long)(1000 * elapsedTime / + msecTicks = (unsigned long)(1000 * elapsedTime / m_data->mClockFrequency.QuadPart); } @@ -171,36 +176,36 @@ unsigned long int btClock::getTimeMilliseconds() struct timeval currentTime; gettimeofday(¤tTime, 0); - return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 + + return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 + (currentTime.tv_usec - m_data->mStartTime.tv_usec) / 1000; #endif //__CELLOS_LV2__ #endif } - /// Returns the time in us since the last call to reset or since + /// Returns the time in us since the last call to reset or since /// the Clock was created. unsigned long int btClock::getTimeMicroseconds() { #ifdef BT_USE_WINDOWS_TIMERS LARGE_INTEGER currentTime; QueryPerformanceCounter(¤tTime); - LONGLONG elapsedTime = currentTime.QuadPart - + LONGLONG elapsedTime = currentTime.QuadPart - m_data->mStartTime.QuadPart; // Compute the number of millisecond ticks elapsed. - unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / + unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / m_data->mClockFrequency.QuadPart); - // Check for unexpected leaps in the Win32 performance counter. - // (This is caused by unexpected data across the PCI to ISA + // Check for unexpected leaps in the Win32 performance counter. + // (This is caused by unexpected data across the PCI to ISA // bridge, aka south bridge. See Microsoft KB274323.) - unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick; + unsigned long elapsedTicks = (unsigned long)(GetTickCount64() - m_data->mStartTick); signed long msecOff = (signed long)(msecTicks - elapsedTicks); if (msecOff < -100 || msecOff > 100) { // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - m_data->mClockFrequency.QuadPart / 1000, elapsedTime - + LONGLONG msecAdjustment = mymin(msecOff * + m_data->mClockFrequency.QuadPart / 1000, elapsedTime - m_data->mPrevElapsedTime); m_data->mStartTime.QuadPart += msecAdjustment; elapsedTime -= msecAdjustment; @@ -210,7 +215,7 @@ unsigned long int btClock::getTimeMicroseconds() m_data->mPrevElapsedTime = elapsedTime; // Convert to microseconds. - unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / + unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / m_data->mClockFrequency.QuadPart); return usecTicks; @@ -229,14 +234,22 @@ unsigned long int btClock::getTimeMicroseconds() struct timeval currentTime; gettimeofday(¤tTime, 0); - return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 + + return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 + (currentTime.tv_usec - m_data->mStartTime.tv_usec); #endif//__CELLOS_LV2__ -#endif +#endif } +/// Returns the time in s since the last call to reset or since +/// the Clock was created. +btScalar btClock::getTimeSeconds() +{ + static const btScalar microseconds_to_seconds = btScalar(0.000001); + return btScalar(getTimeMicroseconds()) * microseconds_to_seconds; +} + inline void Profile_Get_Ticks(unsigned long int * ticks) @@ -293,8 +306,7 @@ void CProfileNode::CleanupMemory() CProfileNode::~CProfileNode( void ) { - delete ( Child); - delete ( Sibling); + CleanupMemory(); } @@ -318,7 +330,7 @@ CProfileNode * CProfileNode::Get_Sub_Node( const char * name ) } // We didn't find it, so add it - + CProfileNode * node = new CProfileNode( name, this ); node->Sibling = Child; Child = node; @@ -330,7 +342,7 @@ void CProfileNode::Reset( void ) { TotalCalls = 0; TotalTime = 0.0f; - + if ( Child ) { Child->Reset(); @@ -352,7 +364,7 @@ void CProfileNode::Call( void ) bool CProfileNode::Return( void ) { - if ( --RecursionCounter == 0 && TotalCalls != 0 ) { + if ( --RecursionCounter == 0 && TotalCalls != 0 ) { unsigned long int time; Profile_Get_Ticks(&time); time-=StartTime; @@ -445,8 +457,8 @@ void CProfileManager::Start_Profile( const char * name ) { if (name != CurrentNode->Get_Name()) { CurrentNode = CurrentNode->Get_Sub_Node( name ); - } - + } + CurrentNode->Call(); } @@ -470,7 +482,7 @@ void CProfileManager::Stop_Profile( void ) * This resets everything except for the tree structure. All of the timing data is reset. * *=============================================================================================*/ void CProfileManager::Reset( void ) -{ +{ gProfileClock.reset(); Root.Reset(); Root.Call(); @@ -516,9 +528,9 @@ void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spaci printf("Profiling: %s (total running time: %.3f ms) ---\n", profileIterator->Get_Current_Parent_Name(), parent_time ); float totalTime = 0.f; - + int numChildren = 0; - + for (i = 0; !profileIterator->Is_Done(); i++,profileIterator->Next()) { numChildren++; @@ -535,11 +547,11 @@ void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spaci if (parent_time < accumulated_time) { - printf("what's wrong\n"); + //printf("what's wrong\n"); } for (i=0;i<spacing;i++) printf("."); printf("%s (%.3f %%) :: %.3f ms\n", "Unaccounted:",parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time); - + for (i=0;i<numChildren;i++) { profileIterator->Enter_Child(i); diff --git a/extern/bullet2/src/LinearMath/btQuickprof.h b/extern/bullet2/src/LinearMath/btQuickprof.h index 93f3f4a60fa..362f62d6d40 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.h +++ b/extern/bullet2/src/LinearMath/btQuickprof.h @@ -52,6 +52,11 @@ public: /// Returns the time in us since the last call to reset or since /// the Clock was created. unsigned long int getTimeMicroseconds(); + + /// Returns the time in s since the last call to reset or since + /// the Clock was created. + btScalar getTimeSeconds(); + private: struct btClockData* m_data; }; diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index 401e11eaaa8..0bfd255bdb4 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -28,7 +28,7 @@ subject to the following restrictions: #include <float.h> /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ -#define BT_BULLET_VERSION 282 +#define BT_BULLET_VERSION 284 inline int btGetVersion() { @@ -48,6 +48,11 @@ inline int btGetVersion() #define ATTRIBUTE_ALIGNED16(a) a #define ATTRIBUTE_ALIGNED64(a) a #define ATTRIBUTE_ALIGNED128(a) a + #elif (_M_ARM) + #define SIMD_FORCE_INLINE __forceinline + #define ATTRIBUTE_ALIGNED16(a) __declspec() a + #define ATTRIBUTE_ALIGNED64(a) __declspec() a + #define ATTRIBUTE_ALIGNED128(a) __declspec () a #else //#define BT_HAS_ALIGNED_ALLOCATOR #pragma warning(disable : 4324) // disable padding warning @@ -67,13 +72,20 @@ inline int btGetVersion() #define btFsel(a,b,c) __fsel((a),(b),(c)) #else -#if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION)) +#if defined (_M_ARM) + //Do not turn SSE on for ARM (may want to turn on BT_USE_NEON however) +#elif (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION)) #if _MSC_VER>1400 #define BT_USE_SIMD_VECTOR3 #endif #define BT_USE_SSE #ifdef BT_USE_SSE + +#if (_MSC_FULL_VER >= 170050727)//Visual Studio 2012 can compile SSE4/FMA3 (but SSE4/FMA3 is not enabled by default) + #define BT_ALLOW_SSE4 +#endif //(_MSC_FULL_VER >= 160040219) + //BT_USE_SSE_IN_API is disabled under Windows by default, because //it makes it harder to integrate Bullet into your application under Windows //(structured embedding Bullet structs/classes need to be 16-byte aligned) @@ -338,12 +350,23 @@ inline __m128 operator * (const __m128 A, const __m128 B) #else//BT_USE_NEON #ifndef BT_INFINITY - static int btInfinityMask = 0x7F800000; - #define BT_INFINITY (*(float*)&btInfinityMask) - inline int btGetInfinityMask()//suppress stupid compiler warning - { - return btInfinityMask; - } + struct btInfMaskConverter + { + union { + float mask; + int intmask; + }; + btInfMaskConverter(int mask=0x7F800000) + :intmask(mask) + { + } + }; + static btInfMaskConverter btInfinityMask = 0x7F800000; + #define BT_INFINITY (btInfinityMask.mask) + inline int btGetInfinityMask()//suppress stupid compiler warning + { + return btInfinityMask.intmask; + } #endif #endif//BT_USE_NEON @@ -395,19 +418,30 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); } SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) { #ifdef USE_APPROXIMATION +#ifdef __LP64__ + float xhalf = 0.5f*y; + int i = *(int*)&y; + i = 0x5f375a86 - (i>>1); + y = *(float*)&i; + y = y*(1.5f - xhalf*y*y); + y = y*(1.5f - xhalf*y*y); + y = y*(1.5f - xhalf*y*y); + y=1/y; + return y; +#else double x, z, tempf; unsigned long *tfptr = ((unsigned long *)&tempf) + 1; - - tempf = y; - *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ - x = tempf; - z = y*btScalar(0.5); - x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */ - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - return x*y; + tempf = y; + *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ + x = tempf; + z = y*btScalar(0.5); + x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */ + x = (btScalar(1.5)*x)-(x*x)*(x*z); + x = (btScalar(1.5)*x)-(x*x)*(x*z); + x = (btScalar(1.5)*x)-(x*x)*(x*z); + x = (btScalar(1.5)*x)-(x*x)*(x*z); + return x*y; +#endif #else return sqrtf(y); #endif @@ -452,9 +486,17 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); } #ifdef BT_USE_DOUBLE_PRECISION #define SIMD_EPSILON DBL_EPSILON #define SIMD_INFINITY DBL_MAX +#define BT_ONE 1.0 +#define BT_ZERO 0.0 +#define BT_TWO 2.0 +#define BT_HALF 0.5 #else #define SIMD_EPSILON FLT_EPSILON #define SIMD_INFINITY FLT_MAX +#define BT_ONE 1.0f +#define BT_ZERO 0.0f +#define BT_TWO 2.0f +#define BT_HALF 0.5f #endif SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) diff --git a/extern/bullet2/src/LinearMath/btSerializer.cpp b/extern/bullet2/src/LinearMath/btSerializer.cpp index ba344939530..8fdcfb14211 100644 --- a/extern/bullet2/src/LinearMath/btSerializer.cpp +++ b/extern/bullet2/src/LinearMath/btSerializer.cpp @@ -1,5 +1,5 @@ char sBulletDNAstr[]= { -char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(69),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), +char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(123),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95), char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111), char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), @@ -162,340 +162,433 @@ char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105), char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(83), char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103), -char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65), -char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109), -char(95),char(116),char(97),char(117),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97), -char(120),char(69),char(114),char(114),char(111),char(114),char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111), -char(114),char(0),char(109),char(95),char(101),char(114),char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111), -char(98),char(97),char(108),char(67),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115), -char(101),char(80),char(101),char(110),char(101),char(116),char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), -char(100),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110), -char(69),char(114),char(112),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119), -char(97),char(114),char(109),char(115),char(116),char(97),char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95), -char(109),char(97),char(120),char(71),char(121),char(114),char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109), -char(95),char(115),char(105),char(110),char(103),char(108),char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114), -char(105),char(99),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117), -char(109),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114), -char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99), -char(116),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), -char(100),char(0),char(109),char(95),char(109),char(105),char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116), -char(99),char(104),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115), -char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0), -char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109), -char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95), -char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0), -char(109),char(95),char(112),char(114),char(101),char(118),char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109), -char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97), -char(116),char(101),char(100),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95), -char(97),char(114),char(101),char(97),char(0),char(109),char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101), -char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110), -char(103),char(116),char(104),char(0),char(109),char(95),char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100), -char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114), -char(101),char(97),char(0),char(109),char(95),char(99),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100), -char(105),char(99),char(101),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101), -char(0),char(109),char(95),char(99),char(49),char(0),char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111), -char(99),char(97),char(108),char(70),char(114),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100), -char(121),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111), -char(77),char(111),char(100),char(101),char(108),char(0),char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95), -char(100),char(114),char(97),char(103),char(0),char(109),char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117), -char(114),char(101),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105), -char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99), -char(104),char(0),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100), -char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99), -char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116), -char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114), -char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100), -char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111), -char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100), -char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116), -char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103), -char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105), -char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116), -char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102), -char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83), -char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116), -char(105),char(109),char(101),char(83),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73), -char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110), -char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116), -char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116), -char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97), -char(108),char(101),char(0),char(109),char(95),char(97),char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111), -char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109), -char(95),char(110),char(117),char(109),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87), -char(101),char(105),char(103),char(116),char(115),char(0),char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102), -char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95), -char(108),char(111),char(99),char(105),char(105),char(0),char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112), -char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115), -char(91),char(50),char(93),char(0),char(109),char(95),char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97), -char(109),char(101),char(114),char(101),char(102),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101), -char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97), -char(109),char(101),char(82),char(101),char(102),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95), -char(110),char(117),char(109),char(77),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109), -char(95),char(105),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0), -char(109),char(95),char(110),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112), -char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97), -char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109), -char(97),char(120),char(83),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108), -char(115),char(101),char(0),char(109),char(95),char(115),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109), -char(112),char(117),char(108),char(115),char(101),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105), -char(110),char(115),char(65),char(110),char(99),char(104),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109), -char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100), -char(121),char(65),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50), -char(93),char(0),char(109),char(95),char(99),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101), -char(108),char(101),char(116),char(101),char(0),char(109),char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50), -char(93),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121), -char(66),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109), -char(95),char(112),char(111),char(115),char(101),char(0),char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0), -char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109), -char(95),char(102),char(97),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97), -char(0),char(42),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116), -char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77), -char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0), -char(109),char(95),char(110),char(117),char(109),char(70),char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114), -char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0), -char(109),char(95),char(110),char(117),char(109),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74), -char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(0),char(84),char(89),char(80),char(69), -char(87),char(0),char(0),char(0),char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116), -char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111), -char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100), -char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115), -char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98), -char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78), -char(111),char(100),char(101),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109), -char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70), -char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100), -char(66),char(118),char(104),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108), -char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97), -char(116),char(105),char(99),char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(67),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100), -char(105),char(117),char(115),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97), -char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105), -char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120), -char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97), -char(114),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115), -char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105), -char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115), -char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110), -char(100),char(83),char(104),char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), -char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121), -char(108),char(105),char(110),char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), -char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108), -char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108), -char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77), -char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101), -char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108), -char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116), -char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115), -char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), -char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108), -char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108), -char(100),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116), -char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49), -char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108), -char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100), -char(121),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97), -char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110), -char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111), -char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116), +char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(66), +char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82), +char(80),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95), +char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(108),char(105),char(110), +char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114), +char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(108),char(105),char(110), +char(101),char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(108), +char(105),char(110),char(101),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(108), +char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115), +char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105), +char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105), +char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(110),char(97),char(98), +char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83), +char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97), +char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(108), +char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115), +char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83), +char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91), +char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(66),char(111),char(117),char(110),char(99),char(101),char(0),char(109), +char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110), +char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), +char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114), +char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(84),char(97), +char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), +char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(97),char(110), +char(103),char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(97), +char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115), +char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109), +char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105), +char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114), +char(69),char(110),char(97),char(98),char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103), +char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95), +char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91), +char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116), +char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95), +char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103), +char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(111),char(116),char(97),char(116),char(101),char(79), +char(114),char(100),char(101),char(114),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),char(0),char(109),char(95),char(97),char(120),char(105), +char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),char(95),char(116),char(97),char(117),char(0),char(109), +char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),char(120),char(69),char(114),char(114),char(111),char(114), +char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),char(114),char(0),char(109),char(95),char(101),char(114), +char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),char(98),char(97),char(108),char(67),char(102),char(109), +char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(80),char(101),char(110),char(101),char(116), +char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(115),char(112), +char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),char(69),char(114),char(112),char(0),char(109),char(95), +char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),char(97),char(114),char(109),char(115),char(116),char(97), +char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(97),char(120),char(71),char(121),char(114), +char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(115),char(105),char(110),char(103),char(108), +char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110), +char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),char(109),char(73),char(116),char(101),char(114),char(97), +char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(77),char(111),char(100),char(101),char(0),char(109), +char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(82),char(101),char(115),char(116),char(105), +char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(109),char(105), +char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),char(99),char(104),char(83),char(105),char(122),char(101), +char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(105), +char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117), +char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109), +char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105), +char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(114),char(101),char(118), +char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99), +char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97),char(116),char(101),char(100),char(70),char(111),char(114), +char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95),char(97),char(114),char(101),char(97),char(0),char(109), +char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101), +char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110),char(103),char(116),char(104),char(0),char(109),char(95), +char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99), +char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114),char(101),char(97),char(0),char(109),char(95),char(99), +char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(52), +char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(99),char(49),char(0), +char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(70),char(114),char(97), +char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(0),char(109),char(95),char(110),char(111), +char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111),char(77),char(111),char(100),char(101),char(108),char(0), +char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95),char(100),char(114),char(97),char(103),char(0),char(109), +char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117),char(114),char(101),char(0),char(109),char(95),char(118), +char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105),char(99),char(70),char(114),char(105),char(99),char(116), +char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99),char(104),char(0),char(109),char(95),char(114),char(105), +char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109), +char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110), +char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114), +char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(72),char(97),char(114),char(100),char(110),char(101), +char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101), +char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101), +char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109), +char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100), +char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115), +char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111), +char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117), +char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67), +char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109), +char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(99),char(97), +char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73),char(116),char(101),char(114),char(97),char(116),char(105), +char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(73),char(116),char(101),char(114),char(97),char(116), +char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111), +char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111), +char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(97), +char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110), +char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(80),char(111), +char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87),char(101),char(105),char(103),char(116),char(115),char(0), +char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102),char(114),char(97),char(109),char(101),char(0),char(109), +char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(108),char(111),char(99),char(105),char(105),char(0), +char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91), +char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95), +char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(114),char(101),char(102),char(115), +char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109), +char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97),char(109),char(101),char(82),char(101),char(102),char(115), +char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(115), +char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(105),char(109),char(97),char(115),char(115), +char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(105),char(109), +char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95), +char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0), +char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(120),char(83),char(101),char(108),char(102), +char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(115), +char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(70), +char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105),char(110),char(115),char(65),char(110),char(99),char(104), +char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116), +char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(0),char(42),char(109),char(95), +char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(99),char(102), +char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101),char(108),char(101),char(116),char(101),char(0),char(109), +char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50),char(93),char(0),char(109),char(95),char(98),char(111), +char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(116),char(121),char(112),char(101),char(0), +char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(101),char(0), +char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100), +char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109),char(95),char(102),char(97),char(99),char(101),char(115), +char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(42),char(109),char(95),char(97),char(110), +char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(42),char(109), +char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(116),char(101),char(114),char(105),char(97), +char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70), +char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97), +char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(67), +char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74),char(111),char(105),char(110),char(116),char(115),char(0), +char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(109),char(95),char(122),char(101),char(114),char(111),char(82),char(111),char(116),char(80),char(97), +char(114),char(101),char(110),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(67), +char(111),char(109),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),char(102),char(115),char(101),char(116),char(0),char(109),char(95), +char(116),char(104),char(105),char(115),char(80),char(105),char(118),char(111),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102), +char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(84),char(111),char(112),char(91), +char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(66),char(111),char(116),char(116),char(111),char(109), +char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106), +char(111),char(105),char(110),char(116),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(67),char(111),char(108),char(108), +char(105),char(100),char(101),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109), +char(95),char(108),char(105),char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(73),char(110), +char(100),char(101),char(120),char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112),char(111),char(115), +char(86),char(97),char(114),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111),char(115),char(91), +char(55),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109),char(95),char(106), +char(111),char(105),char(110),char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(98),char(97),char(115), +char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101), +char(114),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111), +char(114),char(109),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98), +char(97),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(84),char(89),char(80),char(69),char(95),char(0),char(0),char(0),char(99),char(104),char(97),char(114), +char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0), +char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116), +char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114), +char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101), +char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51), +char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105), +char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114), +char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116), +char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97), +char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101), +char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101), +char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97), +char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104), +char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105), +char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99), +char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110), +char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115), +char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101), +char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105), +char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110), +char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103), +char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114), +char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104), +char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104), +char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111), +char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110), +char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83), +char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104), +char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110), +char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104), +char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117), +char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115), +char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111), +char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114), +char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97), +char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108), +char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108), +char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82), +char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116), +char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116), +char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116), +char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80), +char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98), +char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101), char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(50),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105), -char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111), -char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0), -char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119), -char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67), -char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50), -char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103), +char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108), +char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116), +char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67), +char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111), +char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116), char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110), -char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114), -char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108), -char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117), -char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114), -char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116), -char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83), -char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116), -char(66),char(111),char(100),char(121),char(70),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100), -char(121),char(84),char(101),char(116),char(114),char(97),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100), -char(65),char(110),char(99),char(104),char(111),char(114),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67), -char(111),char(110),char(102),char(105),char(103),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111), -char(115),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116), -char(101),char(114),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105), -char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111), -char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0), -char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),char(12),char(0),char(36),char(0),char(8),char(0),char(16),char(0), -char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(84),char(0), -char(-124),char(0),char(12),char(0),char(52),char(0),char(52),char(0),char(20),char(0),char(64),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0), -char(32),char(0),char(28),char(0),char(60),char(0),char(56),char(0),char(76),char(0),char(76),char(0),char(24),char(0),char(60),char(0),char(60),char(0),char(60),char(0), -char(16),char(0),char(64),char(0),char(68),char(0),char(-48),char(1),char(0),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-24),char(1), -char(-96),char(3),char(8),char(0),char(52),char(0),char(52),char(0),char(0),char(0),char(68),char(0),char(84),char(0),char(-124),char(0),char(116),char(0),char(92),char(1), -char(-36),char(0),char(-116),char(1),char(124),char(1),char(-44),char(0),char(-4),char(0),char(-52),char(1),char(92),char(1),char(116),char(2),char(-52),char(0),char(108),char(1), -char(92),char(0),char(-116),char(0),char(16),char(0),char(100),char(0),char(20),char(0),char(36),char(0),char(100),char(0),char(92),char(0),char(104),char(0),char(-64),char(0), -char(92),char(1),char(104),char(0),char(-84),char(1),char(0),char(0),char(83),char(84),char(82),char(67),char(76),char(0),char(0),char(0),char(10),char(0),char(3),char(0), -char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0), -char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0), -char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0), -char(13),char(0),char(9),char(0),char(16),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(17),char(0),char(2),char(0),char(15),char(0),char(10),char(0), -char(13),char(0),char(11),char(0),char(18),char(0),char(2),char(0),char(16),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(19),char(0),char(4),char(0), -char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(20),char(0),char(6),char(0), +char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116), +char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105), +char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), +char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114), +char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97), +char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111), +char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111), +char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111), +char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101), +char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108), +char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114), +char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66), +char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116), +char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100), +char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97), +char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97), +char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114), +char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68), +char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97), +char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(111),char(117),char(98), +char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105), +char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66), +char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116), +char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78), +char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0), +char(12),char(0),char(36),char(0),char(8),char(0),char(16),char(0),char(32),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0), +char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(84),char(0),char(-124),char(0),char(12),char(0),char(52),char(0),char(52),char(0), +char(20),char(0),char(64),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),char(32),char(0),char(28),char(0),char(60),char(0),char(56),char(0), +char(76),char(0),char(76),char(0),char(24),char(0),char(60),char(0),char(60),char(0),char(60),char(0),char(16),char(0),char(64),char(0),char(68),char(0),char(-48),char(1), +char(0),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-24),char(1),char(-96),char(3),char(8),char(0),char(52),char(0),char(52),char(0), +char(0),char(0),char(68),char(0),char(84),char(0),char(-124),char(0),char(116),char(0),char(92),char(1),char(-36),char(0),char(-116),char(1),char(124),char(1),char(-44),char(0), +char(-4),char(0),char(-52),char(1),char(92),char(1),char(116),char(2),char(-124),char(2),char(-76),char(4),char(-52),char(0),char(108),char(1),char(92),char(0),char(-116),char(0), +char(16),char(0),char(100),char(0),char(20),char(0),char(36),char(0),char(100),char(0),char(92),char(0),char(104),char(0),char(-64),char(0),char(92),char(1),char(104),char(0), +char(-84),char(1),char(-68),char(2),char(108),char(1),char(-68),char(0),char(100),char(0),char(0),char(0),char(83),char(84),char(82),char(67),char(84),char(0),char(0),char(0), +char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0), +char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0), +char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0), +char(15),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0), +char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0), +char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0), +char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0), char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0), -char(0),char(0),char(21),char(0),char(21),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0), -char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(22),char(0),char(3),char(0),char(2),char(0),char(14),char(0), -char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(23),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0), +char(0),char(0),char(21),char(0),char(23),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0), +char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0), +char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(25),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0), char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0), -char(20),char(0),char(30),char(0),char(22),char(0),char(31),char(0),char(19),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0), -char(24),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0), -char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(21),char(0),char(30),char(0),char(22),char(0),char(31),char(0), -char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(19),char(0),char(32),char(0),char(25),char(0),char(3),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(26),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(13),char(0),char(39),char(0), -char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(27),char(0),char(5),char(0),char(25),char(0),char(38),char(0), -char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(28),char(0),char(2),char(0), -char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(29),char(0),char(4),char(0),char(27),char(0),char(47),char(0),char(28),char(0),char(48),char(0), -char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(30),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(31),char(0),char(2),char(0), -char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(32),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0), -char(33),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(34),char(0),char(8),char(0),char(13),char(0),char(54),char(0), -char(14),char(0),char(55),char(0),char(30),char(0),char(56),char(0),char(32),char(0),char(57),char(0),char(33),char(0),char(58),char(0),char(31),char(0),char(59),char(0), -char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(35),char(0),char(4),char(0),char(34),char(0),char(62),char(0),char(13),char(0),char(63),char(0), -char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(36),char(0),char(7),char(0),char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0), -char(23),char(0),char(66),char(0),char(24),char(0),char(67),char(0),char(37),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0), -char(38),char(0),char(2),char(0),char(36),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(39),char(0),char(4),char(0),char(17),char(0),char(71),char(0), -char(25),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(40),char(0),char(4),char(0),char(25),char(0),char(38),char(0), -char(39),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(41),char(0),char(3),char(0),char(27),char(0),char(47),char(0), -char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(42),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(78),char(0), -char(0),char(0),char(37),char(0),char(43),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0), -char(44),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0), -char(37),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(44),char(0),char(85),char(0),char(4),char(0),char(86),char(0), +char(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0), +char(26),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0), +char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0), +char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0), +char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(28),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0), +char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0), +char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0), +char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0), +char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0), +char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0), +char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0), +char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0), +char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0), +char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0), +char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0), +char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0), +char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0), +char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0), +char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0), +char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0), +char(46),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0), +char(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0), char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0), char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0), -char(45),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0), -char(4),char(0),char(96),char(0),char(46),char(0),char(5),char(0),char(27),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0), -char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(47),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0), -char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(18),char(0),char(104),char(0),char(18),char(0),char(105),char(0),char(14),char(0),char(106),char(0), +char(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0), +char(4),char(0),char(96),char(0),char(48),char(0),char(5),char(0),char(29),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0), +char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(49),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0), +char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(104),char(0),char(20),char(0),char(105),char(0),char(14),char(0),char(106),char(0), char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0), char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0), char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0), -char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(48),char(0),char(25),char(0),char(9),char(0),char(101),char(0), -char(9),char(0),char(102),char(0),char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(17),char(0),char(104),char(0),char(17),char(0),char(105),char(0), +char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(50),char(0),char(25),char(0),char(9),char(0),char(101),char(0), +char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(104),char(0),char(19),char(0),char(105),char(0), char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0), char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0), char(7),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0), -char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(49),char(0),char(2),char(0), -char(50),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(51),char(0),char(2),char(0),char(52),char(0),char(124),char(0),char(13),char(0),char(125),char(0), -char(53),char(0),char(21),char(0),char(48),char(0),char(126),char(0),char(15),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0), +char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(51),char(0),char(2),char(0), +char(52),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(53),char(0),char(2),char(0),char(54),char(0),char(124),char(0),char(13),char(0),char(125),char(0), +char(55),char(0),char(21),char(0),char(50),char(0),char(126),char(0),char(17),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0), char(13),char(0),char(-126),char(0),char(13),char(0),char(-125),char(0),char(13),char(0),char(125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0), char(13),char(0),char(-122),char(0),char(13),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0), char(7),char(0),char(-117),char(0),char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0), -char(7),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(54),char(0),char(22),char(0),char(47),char(0),char(126),char(0),char(16),char(0),char(127),char(0), +char(7),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(56),char(0),char(22),char(0),char(49),char(0),char(126),char(0),char(18),char(0),char(127),char(0), char(14),char(0),char(-128),char(0),char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(125),char(0), char(14),char(0),char(-124),char(0),char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0), char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0), char(8),char(0),char(-114),char(0),char(8),char(0),char(-113),char(0),char(8),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(0),char(0),char(37),char(0), -char(55),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(56),char(0),char(13),char(0),char(53),char(0),char(-108),char(0), -char(53),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0), +char(57),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-108),char(0), +char(55),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0), char(4),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0), -char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(57),char(0),char(13),char(0),char(58),char(0),char(-108),char(0),char(58),char(0),char(-107),char(0), +char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(59),char(0),char(13),char(0),char(60),char(0),char(-108),char(0),char(60),char(0),char(-107),char(0), char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0), char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0), -char(4),char(0),char(-97),char(0),char(59),char(0),char(14),char(0),char(54),char(0),char(-108),char(0),char(54),char(0),char(-107),char(0),char(0),char(0),char(35),char(0), +char(4),char(0),char(-97),char(0),char(61),char(0),char(14),char(0),char(56),char(0),char(-108),char(0),char(56),char(0),char(-107),char(0),char(0),char(0),char(35),char(0), char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0), char(8),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0), -char(0),char(0),char(-96),char(0),char(60),char(0),char(3),char(0),char(57),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0), -char(61),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(62),char(0),char(3),char(0), -char(57),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(63),char(0),char(13),char(0),char(57),char(0),char(-95),char(0), -char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0), +char(0),char(0),char(-96),char(0),char(62),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0), +char(63),char(0),char(3),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(64),char(0),char(3),char(0), +char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-95),char(0), +char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0), char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(64),char(0),char(13),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0), -char(17),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0), +char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(66),char(0),char(13),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0), +char(19),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0), char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0), -char(7),char(0),char(-81),char(0),char(65),char(0),char(14),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0), +char(7),char(0),char(-81),char(0),char(67),char(0),char(14),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0), char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0), char(8),char(0),char(-85),char(0),char(8),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0), -char(0),char(0),char(-80),char(0),char(66),char(0),char(10),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0), +char(0),char(0),char(-80),char(0),char(68),char(0),char(10),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0), char(8),char(0),char(-79),char(0),char(8),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0), -char(8),char(0),char(-81),char(0),char(8),char(0),char(-76),char(0),char(67),char(0),char(11),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0), -char(17),char(0),char(-91),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(68),char(0),char(9),char(0), -char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0), -char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(69),char(0),char(9),char(0), -char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0), -char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(70),char(0),char(5),char(0), -char(68),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0), -char(71),char(0),char(5),char(0),char(69),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0), -char(8),char(0),char(-65),char(0),char(72),char(0),char(9),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0), -char(7),char(0),char(-75),char(0),char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0), -char(4),char(0),char(-70),char(0),char(73),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0), -char(8),char(0),char(-75),char(0),char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0), -char(4),char(0),char(-70),char(0),char(74),char(0),char(5),char(0),char(56),char(0),char(-95),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0), -char(7),char(0),char(-62),char(0),char(0),char(0),char(37),char(0),char(75),char(0),char(4),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-64),char(0), -char(14),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(50),char(0),char(22),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-76),char(0), -char(8),char(0),char(111),char(0),char(8),char(0),char(-60),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(-59),char(0),char(8),char(0),char(-58),char(0), -char(8),char(0),char(-57),char(0),char(8),char(0),char(-56),char(0),char(8),char(0),char(-55),char(0),char(8),char(0),char(-54),char(0),char(8),char(0),char(-53),char(0), -char(8),char(0),char(-52),char(0),char(8),char(0),char(-51),char(0),char(8),char(0),char(-50),char(0),char(8),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0), -char(4),char(0),char(-47),char(0),char(4),char(0),char(-46),char(0),char(4),char(0),char(-45),char(0),char(4),char(0),char(-44),char(0),char(0),char(0),char(37),char(0), -char(52),char(0),char(22),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-60),char(0), -char(7),char(0),char(113),char(0),char(7),char(0),char(-59),char(0),char(7),char(0),char(-58),char(0),char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0), -char(7),char(0),char(-55),char(0),char(7),char(0),char(-54),char(0),char(7),char(0),char(-53),char(0),char(7),char(0),char(-52),char(0),char(7),char(0),char(-51),char(0), -char(7),char(0),char(-50),char(0),char(7),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0),char(4),char(0),char(-47),char(0),char(4),char(0),char(-46),char(0), -char(4),char(0),char(-45),char(0),char(4),char(0),char(-44),char(0),char(0),char(0),char(37),char(0),char(76),char(0),char(4),char(0),char(7),char(0),char(-43),char(0), -char(7),char(0),char(-42),char(0),char(7),char(0),char(-41),char(0),char(4),char(0),char(79),char(0),char(77),char(0),char(10),char(0),char(76),char(0),char(-40),char(0), -char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(13),char(0),char(-37),char(0),char(13),char(0),char(-36),char(0),char(13),char(0),char(-35),char(0), -char(7),char(0),char(-120),char(0),char(7),char(0),char(-34),char(0),char(4),char(0),char(-33),char(0),char(4),char(0),char(53),char(0),char(78),char(0),char(4),char(0), -char(76),char(0),char(-40),char(0),char(4),char(0),char(-32),char(0),char(7),char(0),char(-31),char(0),char(4),char(0),char(-30),char(0),char(79),char(0),char(4),char(0), -char(13),char(0),char(-35),char(0),char(76),char(0),char(-40),char(0),char(4),char(0),char(-29),char(0),char(7),char(0),char(-28),char(0),char(80),char(0),char(7),char(0), -char(13),char(0),char(-27),char(0),char(76),char(0),char(-40),char(0),char(4),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),char(7),char(0),char(-24),char(0), -char(7),char(0),char(-23),char(0),char(4),char(0),char(53),char(0),char(81),char(0),char(6),char(0),char(15),char(0),char(-22),char(0),char(13),char(0),char(-24),char(0), -char(13),char(0),char(-21),char(0),char(58),char(0),char(-20),char(0),char(4),char(0),char(-19),char(0),char(7),char(0),char(-23),char(0),char(82),char(0),char(26),char(0), -char(4),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(-16),char(0),char(7),char(0),char(-15),char(0), -char(7),char(0),char(-14),char(0),char(7),char(0),char(-13),char(0),char(7),char(0),char(-12),char(0),char(7),char(0),char(-11),char(0),char(7),char(0),char(-10),char(0), -char(7),char(0),char(-9),char(0),char(7),char(0),char(-8),char(0),char(7),char(0),char(-7),char(0),char(7),char(0),char(-6),char(0),char(7),char(0),char(-5),char(0), -char(7),char(0),char(-4),char(0),char(7),char(0),char(-3),char(0),char(7),char(0),char(-2),char(0),char(7),char(0),char(-1),char(0),char(7),char(0),char(0),char(1), -char(7),char(0),char(1),char(1),char(4),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(4),char(0),char(4),char(1),char(4),char(0),char(5),char(1), -char(4),char(0),char(118),char(0),char(83),char(0),char(12),char(0),char(15),char(0),char(6),char(1),char(15),char(0),char(7),char(1),char(15),char(0),char(8),char(1), -char(13),char(0),char(9),char(1),char(13),char(0),char(10),char(1),char(7),char(0),char(11),char(1),char(4),char(0),char(12),char(1),char(4),char(0),char(13),char(1), -char(4),char(0),char(14),char(1),char(4),char(0),char(15),char(1),char(7),char(0),char(-25),char(0),char(4),char(0),char(53),char(0),char(84),char(0),char(27),char(0), -char(17),char(0),char(16),char(1),char(15),char(0),char(17),char(1),char(15),char(0),char(18),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(19),char(1), -char(13),char(0),char(20),char(1),char(13),char(0),char(21),char(1),char(13),char(0),char(22),char(1),char(13),char(0),char(23),char(1),char(4),char(0),char(24),char(1), -char(7),char(0),char(25),char(1),char(4),char(0),char(26),char(1),char(4),char(0),char(27),char(1),char(4),char(0),char(28),char(1),char(7),char(0),char(29),char(1), -char(7),char(0),char(30),char(1),char(4),char(0),char(31),char(1),char(4),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1), -char(7),char(0),char(35),char(1),char(7),char(0),char(36),char(1),char(7),char(0),char(37),char(1),char(7),char(0),char(38),char(1),char(4),char(0),char(39),char(1), -char(4),char(0),char(40),char(1),char(4),char(0),char(41),char(1),char(85),char(0),char(12),char(0),char(9),char(0),char(42),char(1),char(9),char(0),char(43),char(1), -char(13),char(0),char(44),char(1),char(7),char(0),char(45),char(1),char(7),char(0),char(-57),char(0),char(7),char(0),char(46),char(1),char(4),char(0),char(47),char(1), -char(13),char(0),char(48),char(1),char(4),char(0),char(49),char(1),char(4),char(0),char(50),char(1),char(4),char(0),char(51),char(1),char(4),char(0),char(53),char(0), -char(86),char(0),char(19),char(0),char(48),char(0),char(126),char(0),char(83),char(0),char(52),char(1),char(76),char(0),char(53),char(1),char(77),char(0),char(54),char(1), -char(78),char(0),char(55),char(1),char(79),char(0),char(56),char(1),char(80),char(0),char(57),char(1),char(81),char(0),char(58),char(1),char(84),char(0),char(59),char(1), -char(85),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(4),char(0),char(27),char(1),char(4),char(0),char(62),char(1),char(4),char(0),char(63),char(1), -char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(4),char(0),char(66),char(1),char(4),char(0),char(67),char(1),char(82),char(0),char(68),char(1), -}; +char(8),char(0),char(-81),char(0),char(8),char(0),char(-76),char(0),char(69),char(0),char(11),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0), +char(19),char(0),char(-91),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0), +char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0), +char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0), +char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(71),char(0),char(9),char(0), +char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0), +char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(72),char(0),char(5),char(0), +char(70),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0), +char(73),char(0),char(5),char(0),char(71),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0), +char(8),char(0),char(-65),char(0),char(74),char(0),char(41),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0), +char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0),char(13),char(0),char(-62),char(0), +char(13),char(0),char(-61),char(0),char(13),char(0),char(-60),char(0),char(13),char(0),char(-59),char(0),char(13),char(0),char(-58),char(0),char(13),char(0),char(-57),char(0), +char(13),char(0),char(-56),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0), +char(0),char(0),char(-51),char(0),char(0),char(0),char(-50),char(0),char(0),char(0),char(-49),char(0),char(0),char(0),char(-80),char(0),char(13),char(0),char(-73),char(0), +char(13),char(0),char(-72),char(0),char(13),char(0),char(-48),char(0),char(13),char(0),char(-47),char(0),char(13),char(0),char(-46),char(0),char(13),char(0),char(-45),char(0), +char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0),char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0), +char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0), +char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-95),char(0), +char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),char(14),char(0),char(-64),char(0), +char(14),char(0),char(-63),char(0),char(14),char(0),char(-62),char(0),char(14),char(0),char(-61),char(0),char(14),char(0),char(-60),char(0),char(14),char(0),char(-59),char(0), +char(14),char(0),char(-58),char(0),char(14),char(0),char(-57),char(0),char(14),char(0),char(-56),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0), +char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0),char(0),char(0),char(-51),char(0),char(0),char(0),char(-50),char(0),char(0),char(0),char(-49),char(0), +char(0),char(0),char(-80),char(0),char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(14),char(0),char(-48),char(0),char(14),char(0),char(-47),char(0), +char(14),char(0),char(-46),char(0),char(14),char(0),char(-45),char(0),char(14),char(0),char(-44),char(0),char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0), +char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(14),char(0),char(-39),char(0),char(14),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0), +char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0), +char(76),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),char(7),char(0),char(-75),char(0), +char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0), +char(77),char(0),char(9),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(8),char(0),char(-75),char(0), +char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0), +char(78),char(0),char(5),char(0),char(58),char(0),char(-95),char(0),char(13),char(0),char(-31),char(0),char(13),char(0),char(-30),char(0),char(7),char(0),char(-29),char(0), +char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-31),char(0),char(14),char(0),char(-30),char(0), +char(8),char(0),char(-29),char(0),char(52),char(0),char(22),char(0),char(8),char(0),char(-28),char(0),char(8),char(0),char(-76),char(0),char(8),char(0),char(111),char(0), +char(8),char(0),char(-27),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(-26),char(0),char(8),char(0),char(-25),char(0),char(8),char(0),char(-24),char(0), +char(8),char(0),char(-23),char(0),char(8),char(0),char(-22),char(0),char(8),char(0),char(-21),char(0),char(8),char(0),char(-20),char(0),char(8),char(0),char(-19),char(0), +char(8),char(0),char(-18),char(0),char(8),char(0),char(-17),char(0),char(8),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0), +char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(22),char(0), +char(7),char(0),char(-28),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(113),char(0), +char(7),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),char(7),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0), +char(7),char(0),char(-21),char(0),char(7),char(0),char(-20),char(0),char(7),char(0),char(-19),char(0),char(7),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0), +char(7),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0), +char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(80),char(0),char(4),char(0),char(7),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0), +char(7),char(0),char(-8),char(0),char(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0),char(80),char(0),char(-7),char(0),char(13),char(0),char(-6),char(0), +char(13),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(7),char(0),char(-120),char(0), +char(7),char(0),char(-1),char(0),char(4),char(0),char(0),char(1),char(4),char(0),char(53),char(0),char(82),char(0),char(4),char(0),char(80),char(0),char(-7),char(0), +char(4),char(0),char(1),char(1),char(7),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(83),char(0),char(4),char(0),char(13),char(0),char(-2),char(0), +char(80),char(0),char(-7),char(0),char(4),char(0),char(4),char(1),char(7),char(0),char(5),char(1),char(84),char(0),char(7),char(0),char(13),char(0),char(6),char(1), +char(80),char(0),char(-7),char(0),char(4),char(0),char(7),char(1),char(7),char(0),char(8),char(1),char(7),char(0),char(9),char(1),char(7),char(0),char(10),char(1), +char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(11),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(12),char(1), +char(60),char(0),char(13),char(1),char(4),char(0),char(14),char(1),char(7),char(0),char(10),char(1),char(86),char(0),char(26),char(0),char(4),char(0),char(15),char(1), +char(7),char(0),char(16),char(1),char(7),char(0),char(-76),char(0),char(7),char(0),char(17),char(1),char(7),char(0),char(18),char(1),char(7),char(0),char(19),char(1), +char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),char(7),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1), +char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),char(7),char(0),char(27),char(1),char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1), +char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(7),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1), +char(4),char(0),char(35),char(1),char(4),char(0),char(36),char(1),char(4),char(0),char(37),char(1),char(4),char(0),char(38),char(1),char(4),char(0),char(118),char(0), +char(87),char(0),char(12),char(0),char(17),char(0),char(39),char(1),char(17),char(0),char(40),char(1),char(17),char(0),char(41),char(1),char(13),char(0),char(42),char(1), +char(13),char(0),char(43),char(1),char(7),char(0),char(44),char(1),char(4),char(0),char(45),char(1),char(4),char(0),char(46),char(1),char(4),char(0),char(47),char(1), +char(4),char(0),char(48),char(1),char(7),char(0),char(8),char(1),char(4),char(0),char(53),char(0),char(88),char(0),char(27),char(0),char(19),char(0),char(49),char(1), +char(17),char(0),char(50),char(1),char(17),char(0),char(51),char(1),char(13),char(0),char(42),char(1),char(13),char(0),char(52),char(1),char(13),char(0),char(53),char(1), +char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(7),char(0),char(58),char(1), +char(4),char(0),char(59),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(7),char(0),char(62),char(1),char(7),char(0),char(63),char(1), +char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(7),char(0),char(66),char(1),char(7),char(0),char(67),char(1),char(7),char(0),char(68),char(1), +char(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),char(4),char(0),char(72),char(1),char(4),char(0),char(73),char(1), +char(4),char(0),char(74),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(75),char(1),char(9),char(0),char(76),char(1),char(13),char(0),char(77),char(1), +char(7),char(0),char(78),char(1),char(7),char(0),char(-24),char(0),char(7),char(0),char(79),char(1),char(4),char(0),char(80),char(1),char(13),char(0),char(81),char(1), +char(4),char(0),char(82),char(1),char(4),char(0),char(83),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0), +char(50),char(0),char(126),char(0),char(87),char(0),char(85),char(1),char(80),char(0),char(86),char(1),char(81),char(0),char(87),char(1),char(82),char(0),char(88),char(1), +char(83),char(0),char(89),char(1),char(84),char(0),char(90),char(1),char(85),char(0),char(91),char(1),char(88),char(0),char(92),char(1),char(89),char(0),char(93),char(1), +char(4),char(0),char(94),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(95),char(1),char(4),char(0),char(96),char(1),char(4),char(0),char(97),char(1), +char(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),char(86),char(0),char(101),char(1),char(91),char(0),char(17),char(0), +char(16),char(0),char(102),char(1),char(14),char(0),char(103),char(1),char(14),char(0),char(104),char(1),char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1), +char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1),char(49),char(0),char(109),char(1),char(14),char(0),char(110),char(1),char(8),char(0),char(111),char(1), +char(4),char(0),char(112),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(113),char(1),char(4),char(0),char(114),char(1),char(8),char(0),char(115),char(1), +char(8),char(0),char(116),char(1),char(8),char(0),char(117),char(1),char(92),char(0),char(17),char(0),char(15),char(0),char(102),char(1),char(13),char(0),char(103),char(1), +char(13),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1),char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1), +char(50),char(0),char(109),char(1),char(13),char(0),char(110),char(1),char(4),char(0),char(113),char(1),char(7),char(0),char(111),char(1),char(4),char(0),char(112),char(1), +char(4),char(0),char(84),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1),char(7),char(0),char(117),char(1),char(4),char(0),char(114),char(1), +char(93),char(0),char(8),char(0),char(0),char(0),char(118),char(1),char(91),char(0),char(88),char(1),char(49),char(0),char(119),char(1),char(20),char(0),char(120),char(1), +char(14),char(0),char(121),char(1),char(4),char(0),char(95),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0), +char(0),char(0),char(118),char(1),char(92),char(0),char(88),char(1),char(50),char(0),char(119),char(1),char(19),char(0),char(120),char(1),char(13),char(0),char(121),char(1), +char(7),char(0),char(122),char(1),char(4),char(0),char(95),char(1),}; int sBulletDNAlen= sizeof(sBulletDNAstr); char sBulletDNAstr64[]= { -char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(69),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), +char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(123),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95), char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111), char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), @@ -658,334 +751,427 @@ char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105), char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(83), char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103), -char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65), -char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109), -char(95),char(116),char(97),char(117),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97), -char(120),char(69),char(114),char(114),char(111),char(114),char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111), -char(114),char(0),char(109),char(95),char(101),char(114),char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111), -char(98),char(97),char(108),char(67),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115), -char(101),char(80),char(101),char(110),char(101),char(116),char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), -char(100),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110), -char(69),char(114),char(112),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119), -char(97),char(114),char(109),char(115),char(116),char(97),char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95), -char(109),char(97),char(120),char(71),char(121),char(114),char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109), -char(95),char(115),char(105),char(110),char(103),char(108),char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114), -char(105),char(99),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117), -char(109),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114), -char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99), -char(116),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), -char(100),char(0),char(109),char(95),char(109),char(105),char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116), -char(99),char(104),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115), -char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0), -char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109), -char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95), -char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0), -char(109),char(95),char(112),char(114),char(101),char(118),char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109), -char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97), -char(116),char(101),char(100),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95), -char(97),char(114),char(101),char(97),char(0),char(109),char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101), -char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110), -char(103),char(116),char(104),char(0),char(109),char(95),char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100), -char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114), -char(101),char(97),char(0),char(109),char(95),char(99),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100), -char(105),char(99),char(101),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101), -char(0),char(109),char(95),char(99),char(49),char(0),char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111), -char(99),char(97),char(108),char(70),char(114),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100), -char(121),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111), -char(77),char(111),char(100),char(101),char(108),char(0),char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95), -char(100),char(114),char(97),char(103),char(0),char(109),char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117), -char(114),char(101),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105), -char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99), -char(104),char(0),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100), -char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99), -char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116), -char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114), -char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100), -char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111), -char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100), -char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116), -char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103), -char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105), -char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116), -char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102), -char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83), -char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116), -char(105),char(109),char(101),char(83),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73), -char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110), -char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116), -char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116), -char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97), -char(108),char(101),char(0),char(109),char(95),char(97),char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111), -char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109), -char(95),char(110),char(117),char(109),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87), -char(101),char(105),char(103),char(116),char(115),char(0),char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102), -char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95), -char(108),char(111),char(99),char(105),char(105),char(0),char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112), -char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115), -char(91),char(50),char(93),char(0),char(109),char(95),char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97), -char(109),char(101),char(114),char(101),char(102),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101), -char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97), -char(109),char(101),char(82),char(101),char(102),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95), -char(110),char(117),char(109),char(77),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109), -char(95),char(105),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0), -char(109),char(95),char(110),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112), -char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97), -char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109), -char(97),char(120),char(83),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108), -char(115),char(101),char(0),char(109),char(95),char(115),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109), -char(112),char(117),char(108),char(115),char(101),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105), -char(110),char(115),char(65),char(110),char(99),char(104),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109), -char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100), -char(121),char(65),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50), -char(93),char(0),char(109),char(95),char(99),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101), -char(108),char(101),char(116),char(101),char(0),char(109),char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50), -char(93),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121), -char(66),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109), -char(95),char(112),char(111),char(115),char(101),char(0),char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0), -char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109), -char(95),char(102),char(97),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97), -char(0),char(42),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116), -char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77), -char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0), -char(109),char(95),char(110),char(117),char(109),char(70),char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114), -char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0), -char(109),char(95),char(110),char(117),char(109),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74), -char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(0),char(84),char(89),char(80),char(69), -char(87),char(0),char(0),char(0),char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116), -char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111), -char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100), -char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115), -char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98), -char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78), -char(111),char(100),char(101),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109), -char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70), -char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100), -char(66),char(118),char(104),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108), -char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97), -char(116),char(105),char(99),char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(67),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100), -char(105),char(117),char(115),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97), -char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105), -char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120), -char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97), -char(114),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115), -char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105), -char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115), -char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110), -char(100),char(83),char(104),char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), -char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121), -char(108),char(105),char(110),char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), -char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108), -char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108), -char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77), -char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101), -char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108), -char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116), -char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115), -char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), -char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108), -char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108), -char(100),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116), -char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49), -char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108), -char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100), -char(121),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97), -char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110), -char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111), -char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116), +char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(66), +char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82), +char(80),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95), +char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(108),char(105),char(110), +char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114), +char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(108),char(105),char(110), +char(101),char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(108), +char(105),char(110),char(101),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(108), +char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115), +char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105), +char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105), +char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(110),char(97),char(98), +char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83), +char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97), +char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(108), +char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115), +char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83), +char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91), +char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(66),char(111),char(117),char(110),char(99),char(101),char(0),char(109), +char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110), +char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), +char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114), +char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(84),char(97), +char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), +char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(97),char(110), +char(103),char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(97), +char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115), +char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109), +char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105), +char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114), +char(69),char(110),char(97),char(98),char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103), +char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95), +char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91), +char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116), +char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95), +char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103), +char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(111),char(116),char(97),char(116),char(101),char(79), +char(114),char(100),char(101),char(114),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),char(0),char(109),char(95),char(97),char(120),char(105), +char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),char(95),char(116),char(97),char(117),char(0),char(109), +char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),char(120),char(69),char(114),char(114),char(111),char(114), +char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),char(114),char(0),char(109),char(95),char(101),char(114), +char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),char(98),char(97),char(108),char(67),char(102),char(109), +char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(80),char(101),char(110),char(101),char(116), +char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(115),char(112), +char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),char(69),char(114),char(112),char(0),char(109),char(95), +char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),char(97),char(114),char(109),char(115),char(116),char(97), +char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(97),char(120),char(71),char(121),char(114), +char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(115),char(105),char(110),char(103),char(108), +char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110), +char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),char(109),char(73),char(116),char(101),char(114),char(97), +char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(77),char(111),char(100),char(101),char(0),char(109), +char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(82),char(101),char(115),char(116),char(105), +char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(109),char(105), +char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),char(99),char(104),char(83),char(105),char(122),char(101), +char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(105), +char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117), +char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109), +char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105), +char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(114),char(101),char(118), +char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99), +char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97),char(116),char(101),char(100),char(70),char(111),char(114), +char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95),char(97),char(114),char(101),char(97),char(0),char(109), +char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101), +char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110),char(103),char(116),char(104),char(0),char(109),char(95), +char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99), +char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114),char(101),char(97),char(0),char(109),char(95),char(99), +char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(52), +char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(99),char(49),char(0), +char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(70),char(114),char(97), +char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(0),char(109),char(95),char(110),char(111), +char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111),char(77),char(111),char(100),char(101),char(108),char(0), +char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95),char(100),char(114),char(97),char(103),char(0),char(109), +char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117),char(114),char(101),char(0),char(109),char(95),char(118), +char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105),char(99),char(70),char(114),char(105),char(99),char(116), +char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99),char(104),char(0),char(109),char(95),char(114),char(105), +char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109), +char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110), +char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114), +char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(72),char(97),char(114),char(100),char(110),char(101), +char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101), +char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101), +char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109), +char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100), +char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115), +char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111), +char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117), +char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67), +char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109), +char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(99),char(97), +char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73),char(116),char(101),char(114),char(97),char(116),char(105), +char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(73),char(116),char(101),char(114),char(97),char(116), +char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111), +char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111), +char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(97), +char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110), +char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(80),char(111), +char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87),char(101),char(105),char(103),char(116),char(115),char(0), +char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102),char(114),char(97),char(109),char(101),char(0),char(109), +char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(108),char(111),char(99),char(105),char(105),char(0), +char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91), +char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95), +char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(114),char(101),char(102),char(115), +char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109), +char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97),char(109),char(101),char(82),char(101),char(102),char(115), +char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(115), +char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(105),char(109),char(97),char(115),char(115), +char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(105),char(109), +char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95), +char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0), +char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(120),char(83),char(101),char(108),char(102), +char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(115), +char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(70), +char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105),char(110),char(115),char(65),char(110),char(99),char(104), +char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116), +char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(0),char(42),char(109),char(95), +char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(99),char(102), +char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101),char(108),char(101),char(116),char(101),char(0),char(109), +char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50),char(93),char(0),char(109),char(95),char(98),char(111), +char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(116),char(121),char(112),char(101),char(0), +char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(101),char(0), +char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100), +char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109),char(95),char(102),char(97),char(99),char(101),char(115), +char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(42),char(109),char(95),char(97),char(110), +char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(42),char(109), +char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(116),char(101),char(114),char(105),char(97), +char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70), +char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97), +char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(67), +char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74),char(111),char(105),char(110),char(116),char(115),char(0), +char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(109),char(95),char(122),char(101),char(114),char(111),char(82),char(111),char(116),char(80),char(97), +char(114),char(101),char(110),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(67), +char(111),char(109),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),char(102),char(115),char(101),char(116),char(0),char(109),char(95), +char(116),char(104),char(105),char(115),char(80),char(105),char(118),char(111),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102), +char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(84),char(111),char(112),char(91), +char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(66),char(111),char(116),char(116),char(111),char(109), +char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106), +char(111),char(105),char(110),char(116),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(67),char(111),char(108),char(108), +char(105),char(100),char(101),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109), +char(95),char(108),char(105),char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(73),char(110), +char(100),char(101),char(120),char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112),char(111),char(115), +char(86),char(97),char(114),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111),char(115),char(91), +char(55),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109),char(95),char(106), +char(111),char(105),char(110),char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(98),char(97),char(115), +char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101), +char(114),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111), +char(114),char(109),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98), +char(97),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(84),char(89),char(80),char(69),char(95),char(0),char(0),char(0),char(99),char(104),char(97),char(114), +char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0), +char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116), +char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114), +char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101), +char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51), +char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105), +char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114), +char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116), +char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97), +char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101), +char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101), +char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97), +char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104), +char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105), +char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99), +char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110), +char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115), +char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101), +char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105), +char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110), +char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103), +char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114), +char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104), +char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104), +char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111), +char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110), +char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83), +char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104), +char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110), +char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104), +char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117), +char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115), +char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111), +char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114), +char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97), +char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108), +char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108), +char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82), +char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116), +char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116), +char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116), +char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80), +char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98), +char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101), char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(50),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105), -char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111), -char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0), -char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119), -char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67), -char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50), -char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103), +char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108), +char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116), +char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67), +char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111), +char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116), char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110), -char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114), -char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108), -char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117), -char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114), -char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116), -char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83), -char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116), -char(66),char(111),char(100),char(121),char(70),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100), -char(121),char(84),char(101),char(116),char(114),char(97),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100), -char(65),char(110),char(99),char(104),char(111),char(114),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67), -char(111),char(110),char(102),char(105),char(103),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111), -char(115),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116), -char(101),char(114),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105), -char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111), -char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0), -char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),char(16),char(0),char(48),char(0),char(16),char(0),char(16),char(0), -char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(96),char(0), -char(-112),char(0),char(16),char(0),char(56),char(0),char(56),char(0),char(20),char(0),char(72),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0), -char(56),char(0),char(32),char(0),char(80),char(0),char(72),char(0),char(96),char(0),char(80),char(0),char(32),char(0),char(64),char(0),char(64),char(0),char(64),char(0), -char(16),char(0),char(72),char(0),char(80),char(0),char(-32),char(1),char(16),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-8),char(1), -char(-80),char(3),char(8),char(0),char(64),char(0),char(64),char(0),char(0),char(0),char(80),char(0),char(96),char(0),char(-112),char(0),char(-128),char(0),char(104),char(1), -char(-24),char(0),char(-104),char(1),char(-120),char(1),char(-32),char(0),char(8),char(1),char(-40),char(1),char(104),char(1),char(-128),char(2),char(-40),char(0),char(120),char(1), -char(104),char(0),char(-104),char(0),char(16),char(0),char(104),char(0),char(24),char(0),char(40),char(0),char(104),char(0),char(96),char(0),char(104),char(0),char(-56),char(0), -char(104),char(1),char(112),char(0),char(-32),char(1),char(0),char(0),char(83),char(84),char(82),char(67),char(76),char(0),char(0),char(0),char(10),char(0),char(3),char(0), -char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0), -char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0), -char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0), -char(13),char(0),char(9),char(0),char(16),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(17),char(0),char(2),char(0),char(15),char(0),char(10),char(0), -char(13),char(0),char(11),char(0),char(18),char(0),char(2),char(0),char(16),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(19),char(0),char(4),char(0), -char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(20),char(0),char(6),char(0), +char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116), +char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105), +char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), +char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114), +char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97), +char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111), +char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111), +char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111), +char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101), +char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108), +char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114), +char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66), +char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116), +char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100), +char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97), +char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97), +char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114), +char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68), +char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97), +char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(111),char(117),char(98), +char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105), +char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66), +char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116), +char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78), +char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0), +char(16),char(0),char(48),char(0),char(16),char(0),char(16),char(0),char(32),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0), +char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(96),char(0),char(-112),char(0),char(16),char(0),char(56),char(0),char(56),char(0), +char(20),char(0),char(72),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),char(56),char(0),char(32),char(0),char(80),char(0),char(72),char(0), +char(96),char(0),char(80),char(0),char(32),char(0),char(64),char(0),char(64),char(0),char(64),char(0),char(16),char(0),char(72),char(0),char(80),char(0),char(-32),char(1), +char(16),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-8),char(1),char(-80),char(3),char(8),char(0),char(64),char(0),char(64),char(0), +char(0),char(0),char(80),char(0),char(96),char(0),char(-112),char(0),char(-128),char(0),char(104),char(1),char(-24),char(0),char(-104),char(1),char(-120),char(1),char(-32),char(0), +char(8),char(1),char(-40),char(1),char(104),char(1),char(-128),char(2),char(-112),char(2),char(-64),char(4),char(-40),char(0),char(120),char(1),char(104),char(0),char(-104),char(0), +char(16),char(0),char(104),char(0),char(24),char(0),char(40),char(0),char(104),char(0),char(96),char(0),char(104),char(0),char(-56),char(0),char(104),char(1),char(112),char(0), +char(-32),char(1),char(-56),char(2),char(120),char(1),char(-56),char(0),char(112),char(0),char(0),char(0),char(83),char(84),char(82),char(67),char(84),char(0),char(0),char(0), +char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0), +char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0), +char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0), +char(15),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0), +char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0), +char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0), +char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0), char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0), -char(0),char(0),char(21),char(0),char(21),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0), -char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(22),char(0),char(3),char(0),char(2),char(0),char(14),char(0), -char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(23),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0), +char(0),char(0),char(21),char(0),char(23),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0), +char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0), +char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(25),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0), char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0), -char(20),char(0),char(30),char(0),char(22),char(0),char(31),char(0),char(19),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0), -char(24),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0), -char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(21),char(0),char(30),char(0),char(22),char(0),char(31),char(0), -char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(19),char(0),char(32),char(0),char(25),char(0),char(3),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(26),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(13),char(0),char(39),char(0), -char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(27),char(0),char(5),char(0),char(25),char(0),char(38),char(0), -char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(28),char(0),char(2),char(0), -char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(29),char(0),char(4),char(0),char(27),char(0),char(47),char(0),char(28),char(0),char(48),char(0), -char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(30),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(31),char(0),char(2),char(0), -char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(32),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0), -char(33),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(34),char(0),char(8),char(0),char(13),char(0),char(54),char(0), -char(14),char(0),char(55),char(0),char(30),char(0),char(56),char(0),char(32),char(0),char(57),char(0),char(33),char(0),char(58),char(0),char(31),char(0),char(59),char(0), -char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(35),char(0),char(4),char(0),char(34),char(0),char(62),char(0),char(13),char(0),char(63),char(0), -char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(36),char(0),char(7),char(0),char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0), -char(23),char(0),char(66),char(0),char(24),char(0),char(67),char(0),char(37),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0), -char(38),char(0),char(2),char(0),char(36),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(39),char(0),char(4),char(0),char(17),char(0),char(71),char(0), -char(25),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(40),char(0),char(4),char(0),char(25),char(0),char(38),char(0), -char(39),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(41),char(0),char(3),char(0),char(27),char(0),char(47),char(0), -char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(42),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(78),char(0), -char(0),char(0),char(37),char(0),char(43),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0), -char(44),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0), -char(37),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(44),char(0),char(85),char(0),char(4),char(0),char(86),char(0), +char(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0), +char(26),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0), +char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0), +char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0), +char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(28),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0), +char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0), +char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0), +char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0), +char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0), +char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0), +char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0), +char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0), +char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0), +char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0), +char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0), +char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0), +char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0), +char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0), +char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0), +char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0), +char(46),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0), +char(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0), char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0), char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0), -char(45),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0), -char(4),char(0),char(96),char(0),char(46),char(0),char(5),char(0),char(27),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0), -char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(47),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0), -char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(18),char(0),char(104),char(0),char(18),char(0),char(105),char(0),char(14),char(0),char(106),char(0), +char(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0), +char(4),char(0),char(96),char(0),char(48),char(0),char(5),char(0),char(29),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0), +char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(49),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0), +char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(104),char(0),char(20),char(0),char(105),char(0),char(14),char(0),char(106),char(0), char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0), char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0), char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0), -char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(48),char(0),char(25),char(0),char(9),char(0),char(101),char(0), -char(9),char(0),char(102),char(0),char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(17),char(0),char(104),char(0),char(17),char(0),char(105),char(0), +char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(50),char(0),char(25),char(0),char(9),char(0),char(101),char(0), +char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(104),char(0),char(19),char(0),char(105),char(0), char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0), char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0), char(7),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0), -char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(49),char(0),char(2),char(0), -char(50),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(51),char(0),char(2),char(0),char(52),char(0),char(124),char(0),char(13),char(0),char(125),char(0), -char(53),char(0),char(21),char(0),char(48),char(0),char(126),char(0),char(15),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0), +char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(51),char(0),char(2),char(0), +char(52),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(53),char(0),char(2),char(0),char(54),char(0),char(124),char(0),char(13),char(0),char(125),char(0), +char(55),char(0),char(21),char(0),char(50),char(0),char(126),char(0),char(17),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0), char(13),char(0),char(-126),char(0),char(13),char(0),char(-125),char(0),char(13),char(0),char(125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0), char(13),char(0),char(-122),char(0),char(13),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0), char(7),char(0),char(-117),char(0),char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0), -char(7),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(54),char(0),char(22),char(0),char(47),char(0),char(126),char(0),char(16),char(0),char(127),char(0), +char(7),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(56),char(0),char(22),char(0),char(49),char(0),char(126),char(0),char(18),char(0),char(127),char(0), char(14),char(0),char(-128),char(0),char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(125),char(0), char(14),char(0),char(-124),char(0),char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0), char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0), char(8),char(0),char(-114),char(0),char(8),char(0),char(-113),char(0),char(8),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(0),char(0),char(37),char(0), -char(55),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(56),char(0),char(13),char(0),char(53),char(0),char(-108),char(0), -char(53),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0), +char(57),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-108),char(0), +char(55),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0), char(4),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0), -char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(57),char(0),char(13),char(0),char(58),char(0),char(-108),char(0),char(58),char(0),char(-107),char(0), +char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(59),char(0),char(13),char(0),char(60),char(0),char(-108),char(0),char(60),char(0),char(-107),char(0), char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0), char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0), -char(4),char(0),char(-97),char(0),char(59),char(0),char(14),char(0),char(54),char(0),char(-108),char(0),char(54),char(0),char(-107),char(0),char(0),char(0),char(35),char(0), +char(4),char(0),char(-97),char(0),char(61),char(0),char(14),char(0),char(56),char(0),char(-108),char(0),char(56),char(0),char(-107),char(0),char(0),char(0),char(35),char(0), char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0), char(8),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0), -char(0),char(0),char(-96),char(0),char(60),char(0),char(3),char(0),char(57),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0), -char(61),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(62),char(0),char(3),char(0), -char(57),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(63),char(0),char(13),char(0),char(57),char(0),char(-95),char(0), -char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0), +char(0),char(0),char(-96),char(0),char(62),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0), +char(63),char(0),char(3),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(64),char(0),char(3),char(0), +char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-95),char(0), +char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0), char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(64),char(0),char(13),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0), -char(17),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0), +char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(66),char(0),char(13),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0), +char(19),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0), char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0), -char(7),char(0),char(-81),char(0),char(65),char(0),char(14),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0), +char(7),char(0),char(-81),char(0),char(67),char(0),char(14),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0), char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0), char(8),char(0),char(-85),char(0),char(8),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0), -char(0),char(0),char(-80),char(0),char(66),char(0),char(10),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0), +char(0),char(0),char(-80),char(0),char(68),char(0),char(10),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0), char(8),char(0),char(-79),char(0),char(8),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0), -char(8),char(0),char(-81),char(0),char(8),char(0),char(-76),char(0),char(67),char(0),char(11),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0), -char(17),char(0),char(-91),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(68),char(0),char(9),char(0), -char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0), -char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(69),char(0),char(9),char(0), -char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0), -char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(70),char(0),char(5),char(0), -char(68),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0), -char(71),char(0),char(5),char(0),char(69),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0), -char(8),char(0),char(-65),char(0),char(72),char(0),char(9),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0), -char(7),char(0),char(-75),char(0),char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0), -char(4),char(0),char(-70),char(0),char(73),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0), -char(8),char(0),char(-75),char(0),char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0), -char(4),char(0),char(-70),char(0),char(74),char(0),char(5),char(0),char(56),char(0),char(-95),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0), -char(7),char(0),char(-62),char(0),char(0),char(0),char(37),char(0),char(75),char(0),char(4),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-64),char(0), -char(14),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(50),char(0),char(22),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-76),char(0), -char(8),char(0),char(111),char(0),char(8),char(0),char(-60),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(-59),char(0),char(8),char(0),char(-58),char(0), -char(8),char(0),char(-57),char(0),char(8),char(0),char(-56),char(0),char(8),char(0),char(-55),char(0),char(8),char(0),char(-54),char(0),char(8),char(0),char(-53),char(0), -char(8),char(0),char(-52),char(0),char(8),char(0),char(-51),char(0),char(8),char(0),char(-50),char(0),char(8),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0), -char(4),char(0),char(-47),char(0),char(4),char(0),char(-46),char(0),char(4),char(0),char(-45),char(0),char(4),char(0),char(-44),char(0),char(0),char(0),char(37),char(0), -char(52),char(0),char(22),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-60),char(0), -char(7),char(0),char(113),char(0),char(7),char(0),char(-59),char(0),char(7),char(0),char(-58),char(0),char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0), -char(7),char(0),char(-55),char(0),char(7),char(0),char(-54),char(0),char(7),char(0),char(-53),char(0),char(7),char(0),char(-52),char(0),char(7),char(0),char(-51),char(0), -char(7),char(0),char(-50),char(0),char(7),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0),char(4),char(0),char(-47),char(0),char(4),char(0),char(-46),char(0), -char(4),char(0),char(-45),char(0),char(4),char(0),char(-44),char(0),char(0),char(0),char(37),char(0),char(76),char(0),char(4),char(0),char(7),char(0),char(-43),char(0), -char(7),char(0),char(-42),char(0),char(7),char(0),char(-41),char(0),char(4),char(0),char(79),char(0),char(77),char(0),char(10),char(0),char(76),char(0),char(-40),char(0), -char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(13),char(0),char(-37),char(0),char(13),char(0),char(-36),char(0),char(13),char(0),char(-35),char(0), -char(7),char(0),char(-120),char(0),char(7),char(0),char(-34),char(0),char(4),char(0),char(-33),char(0),char(4),char(0),char(53),char(0),char(78),char(0),char(4),char(0), -char(76),char(0),char(-40),char(0),char(4),char(0),char(-32),char(0),char(7),char(0),char(-31),char(0),char(4),char(0),char(-30),char(0),char(79),char(0),char(4),char(0), -char(13),char(0),char(-35),char(0),char(76),char(0),char(-40),char(0),char(4),char(0),char(-29),char(0),char(7),char(0),char(-28),char(0),char(80),char(0),char(7),char(0), -char(13),char(0),char(-27),char(0),char(76),char(0),char(-40),char(0),char(4),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),char(7),char(0),char(-24),char(0), -char(7),char(0),char(-23),char(0),char(4),char(0),char(53),char(0),char(81),char(0),char(6),char(0),char(15),char(0),char(-22),char(0),char(13),char(0),char(-24),char(0), -char(13),char(0),char(-21),char(0),char(58),char(0),char(-20),char(0),char(4),char(0),char(-19),char(0),char(7),char(0),char(-23),char(0),char(82),char(0),char(26),char(0), -char(4),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(-16),char(0),char(7),char(0),char(-15),char(0), -char(7),char(0),char(-14),char(0),char(7),char(0),char(-13),char(0),char(7),char(0),char(-12),char(0),char(7),char(0),char(-11),char(0),char(7),char(0),char(-10),char(0), -char(7),char(0),char(-9),char(0),char(7),char(0),char(-8),char(0),char(7),char(0),char(-7),char(0),char(7),char(0),char(-6),char(0),char(7),char(0),char(-5),char(0), -char(7),char(0),char(-4),char(0),char(7),char(0),char(-3),char(0),char(7),char(0),char(-2),char(0),char(7),char(0),char(-1),char(0),char(7),char(0),char(0),char(1), -char(7),char(0),char(1),char(1),char(4),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(4),char(0),char(4),char(1),char(4),char(0),char(5),char(1), -char(4),char(0),char(118),char(0),char(83),char(0),char(12),char(0),char(15),char(0),char(6),char(1),char(15),char(0),char(7),char(1),char(15),char(0),char(8),char(1), -char(13),char(0),char(9),char(1),char(13),char(0),char(10),char(1),char(7),char(0),char(11),char(1),char(4),char(0),char(12),char(1),char(4),char(0),char(13),char(1), -char(4),char(0),char(14),char(1),char(4),char(0),char(15),char(1),char(7),char(0),char(-25),char(0),char(4),char(0),char(53),char(0),char(84),char(0),char(27),char(0), -char(17),char(0),char(16),char(1),char(15),char(0),char(17),char(1),char(15),char(0),char(18),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(19),char(1), -char(13),char(0),char(20),char(1),char(13),char(0),char(21),char(1),char(13),char(0),char(22),char(1),char(13),char(0),char(23),char(1),char(4),char(0),char(24),char(1), -char(7),char(0),char(25),char(1),char(4),char(0),char(26),char(1),char(4),char(0),char(27),char(1),char(4),char(0),char(28),char(1),char(7),char(0),char(29),char(1), -char(7),char(0),char(30),char(1),char(4),char(0),char(31),char(1),char(4),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1), -char(7),char(0),char(35),char(1),char(7),char(0),char(36),char(1),char(7),char(0),char(37),char(1),char(7),char(0),char(38),char(1),char(4),char(0),char(39),char(1), -char(4),char(0),char(40),char(1),char(4),char(0),char(41),char(1),char(85),char(0),char(12),char(0),char(9),char(0),char(42),char(1),char(9),char(0),char(43),char(1), -char(13),char(0),char(44),char(1),char(7),char(0),char(45),char(1),char(7),char(0),char(-57),char(0),char(7),char(0),char(46),char(1),char(4),char(0),char(47),char(1), -char(13),char(0),char(48),char(1),char(4),char(0),char(49),char(1),char(4),char(0),char(50),char(1),char(4),char(0),char(51),char(1),char(4),char(0),char(53),char(0), -char(86),char(0),char(19),char(0),char(48),char(0),char(126),char(0),char(83),char(0),char(52),char(1),char(76),char(0),char(53),char(1),char(77),char(0),char(54),char(1), -char(78),char(0),char(55),char(1),char(79),char(0),char(56),char(1),char(80),char(0),char(57),char(1),char(81),char(0),char(58),char(1),char(84),char(0),char(59),char(1), -char(85),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(4),char(0),char(27),char(1),char(4),char(0),char(62),char(1),char(4),char(0),char(63),char(1), -char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(4),char(0),char(66),char(1),char(4),char(0),char(67),char(1),char(82),char(0),char(68),char(1), -}; +char(8),char(0),char(-81),char(0),char(8),char(0),char(-76),char(0),char(69),char(0),char(11),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0), +char(19),char(0),char(-91),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0), +char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0), +char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0), +char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(71),char(0),char(9),char(0), +char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0), +char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(72),char(0),char(5),char(0), +char(70),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0), +char(73),char(0),char(5),char(0),char(71),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0), +char(8),char(0),char(-65),char(0),char(74),char(0),char(41),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0), +char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0),char(13),char(0),char(-62),char(0), +char(13),char(0),char(-61),char(0),char(13),char(0),char(-60),char(0),char(13),char(0),char(-59),char(0),char(13),char(0),char(-58),char(0),char(13),char(0),char(-57),char(0), +char(13),char(0),char(-56),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0), +char(0),char(0),char(-51),char(0),char(0),char(0),char(-50),char(0),char(0),char(0),char(-49),char(0),char(0),char(0),char(-80),char(0),char(13),char(0),char(-73),char(0), +char(13),char(0),char(-72),char(0),char(13),char(0),char(-48),char(0),char(13),char(0),char(-47),char(0),char(13),char(0),char(-46),char(0),char(13),char(0),char(-45),char(0), +char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0),char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0), +char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0), +char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-95),char(0), +char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),char(14),char(0),char(-64),char(0), +char(14),char(0),char(-63),char(0),char(14),char(0),char(-62),char(0),char(14),char(0),char(-61),char(0),char(14),char(0),char(-60),char(0),char(14),char(0),char(-59),char(0), +char(14),char(0),char(-58),char(0),char(14),char(0),char(-57),char(0),char(14),char(0),char(-56),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0), +char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0),char(0),char(0),char(-51),char(0),char(0),char(0),char(-50),char(0),char(0),char(0),char(-49),char(0), +char(0),char(0),char(-80),char(0),char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(14),char(0),char(-48),char(0),char(14),char(0),char(-47),char(0), +char(14),char(0),char(-46),char(0),char(14),char(0),char(-45),char(0),char(14),char(0),char(-44),char(0),char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0), +char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(14),char(0),char(-39),char(0),char(14),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0), +char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0), +char(76),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),char(7),char(0),char(-75),char(0), +char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0), +char(77),char(0),char(9),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(8),char(0),char(-75),char(0), +char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0), +char(78),char(0),char(5),char(0),char(58),char(0),char(-95),char(0),char(13),char(0),char(-31),char(0),char(13),char(0),char(-30),char(0),char(7),char(0),char(-29),char(0), +char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-31),char(0),char(14),char(0),char(-30),char(0), +char(8),char(0),char(-29),char(0),char(52),char(0),char(22),char(0),char(8),char(0),char(-28),char(0),char(8),char(0),char(-76),char(0),char(8),char(0),char(111),char(0), +char(8),char(0),char(-27),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(-26),char(0),char(8),char(0),char(-25),char(0),char(8),char(0),char(-24),char(0), +char(8),char(0),char(-23),char(0),char(8),char(0),char(-22),char(0),char(8),char(0),char(-21),char(0),char(8),char(0),char(-20),char(0),char(8),char(0),char(-19),char(0), +char(8),char(0),char(-18),char(0),char(8),char(0),char(-17),char(0),char(8),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0), +char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(22),char(0), +char(7),char(0),char(-28),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(113),char(0), +char(7),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),char(7),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0), +char(7),char(0),char(-21),char(0),char(7),char(0),char(-20),char(0),char(7),char(0),char(-19),char(0),char(7),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0), +char(7),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0), +char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(80),char(0),char(4),char(0),char(7),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0), +char(7),char(0),char(-8),char(0),char(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0),char(80),char(0),char(-7),char(0),char(13),char(0),char(-6),char(0), +char(13),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(7),char(0),char(-120),char(0), +char(7),char(0),char(-1),char(0),char(4),char(0),char(0),char(1),char(4),char(0),char(53),char(0),char(82),char(0),char(4),char(0),char(80),char(0),char(-7),char(0), +char(4),char(0),char(1),char(1),char(7),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(83),char(0),char(4),char(0),char(13),char(0),char(-2),char(0), +char(80),char(0),char(-7),char(0),char(4),char(0),char(4),char(1),char(7),char(0),char(5),char(1),char(84),char(0),char(7),char(0),char(13),char(0),char(6),char(1), +char(80),char(0),char(-7),char(0),char(4),char(0),char(7),char(1),char(7),char(0),char(8),char(1),char(7),char(0),char(9),char(1),char(7),char(0),char(10),char(1), +char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(11),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(12),char(1), +char(60),char(0),char(13),char(1),char(4),char(0),char(14),char(1),char(7),char(0),char(10),char(1),char(86),char(0),char(26),char(0),char(4),char(0),char(15),char(1), +char(7),char(0),char(16),char(1),char(7),char(0),char(-76),char(0),char(7),char(0),char(17),char(1),char(7),char(0),char(18),char(1),char(7),char(0),char(19),char(1), +char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),char(7),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1), +char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),char(7),char(0),char(27),char(1),char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1), +char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(7),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1), +char(4),char(0),char(35),char(1),char(4),char(0),char(36),char(1),char(4),char(0),char(37),char(1),char(4),char(0),char(38),char(1),char(4),char(0),char(118),char(0), +char(87),char(0),char(12),char(0),char(17),char(0),char(39),char(1),char(17),char(0),char(40),char(1),char(17),char(0),char(41),char(1),char(13),char(0),char(42),char(1), +char(13),char(0),char(43),char(1),char(7),char(0),char(44),char(1),char(4),char(0),char(45),char(1),char(4),char(0),char(46),char(1),char(4),char(0),char(47),char(1), +char(4),char(0),char(48),char(1),char(7),char(0),char(8),char(1),char(4),char(0),char(53),char(0),char(88),char(0),char(27),char(0),char(19),char(0),char(49),char(1), +char(17),char(0),char(50),char(1),char(17),char(0),char(51),char(1),char(13),char(0),char(42),char(1),char(13),char(0),char(52),char(1),char(13),char(0),char(53),char(1), +char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(7),char(0),char(58),char(1), +char(4),char(0),char(59),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(7),char(0),char(62),char(1),char(7),char(0),char(63),char(1), +char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(7),char(0),char(66),char(1),char(7),char(0),char(67),char(1),char(7),char(0),char(68),char(1), +char(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),char(4),char(0),char(72),char(1),char(4),char(0),char(73),char(1), +char(4),char(0),char(74),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(75),char(1),char(9),char(0),char(76),char(1),char(13),char(0),char(77),char(1), +char(7),char(0),char(78),char(1),char(7),char(0),char(-24),char(0),char(7),char(0),char(79),char(1),char(4),char(0),char(80),char(1),char(13),char(0),char(81),char(1), +char(4),char(0),char(82),char(1),char(4),char(0),char(83),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0), +char(50),char(0),char(126),char(0),char(87),char(0),char(85),char(1),char(80),char(0),char(86),char(1),char(81),char(0),char(87),char(1),char(82),char(0),char(88),char(1), +char(83),char(0),char(89),char(1),char(84),char(0),char(90),char(1),char(85),char(0),char(91),char(1),char(88),char(0),char(92),char(1),char(89),char(0),char(93),char(1), +char(4),char(0),char(94),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(95),char(1),char(4),char(0),char(96),char(1),char(4),char(0),char(97),char(1), +char(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),char(86),char(0),char(101),char(1),char(91),char(0),char(17),char(0), +char(16),char(0),char(102),char(1),char(14),char(0),char(103),char(1),char(14),char(0),char(104),char(1),char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1), +char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1),char(49),char(0),char(109),char(1),char(14),char(0),char(110),char(1),char(8),char(0),char(111),char(1), +char(4),char(0),char(112),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(113),char(1),char(4),char(0),char(114),char(1),char(8),char(0),char(115),char(1), +char(8),char(0),char(116),char(1),char(8),char(0),char(117),char(1),char(92),char(0),char(17),char(0),char(15),char(0),char(102),char(1),char(13),char(0),char(103),char(1), +char(13),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1),char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1), +char(50),char(0),char(109),char(1),char(13),char(0),char(110),char(1),char(4),char(0),char(113),char(1),char(7),char(0),char(111),char(1),char(4),char(0),char(112),char(1), +char(4),char(0),char(84),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1),char(7),char(0),char(117),char(1),char(4),char(0),char(114),char(1), +char(93),char(0),char(8),char(0),char(0),char(0),char(118),char(1),char(91),char(0),char(88),char(1),char(49),char(0),char(119),char(1),char(20),char(0),char(120),char(1), +char(14),char(0),char(121),char(1),char(4),char(0),char(95),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0), +char(0),char(0),char(118),char(1),char(92),char(0),char(88),char(1),char(50),char(0),char(119),char(1),char(19),char(0),char(120),char(1),char(13),char(0),char(121),char(1), +char(7),char(0),char(122),char(1),char(4),char(0),char(95),char(1),}; int sBulletDNAlen64= sizeof(sBulletDNAstr64); diff --git a/extern/bullet2/src/LinearMath/btSerializer.h b/extern/bullet2/src/LinearMath/btSerializer.h index ff1dc574c5d..033895b1e5e 100644 --- a/extern/bullet2/src/LinearMath/btSerializer.h +++ b/extern/bullet2/src/LinearMath/btSerializer.h @@ -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. @@ -32,12 +32,12 @@ extern int sBulletDNAlen; extern char sBulletDNAstr64[]; extern int sBulletDNAlen64; -SIMD_FORCE_INLINE int btStrLen(const char* str) +SIMD_FORCE_INLINE int btStrLen(const char* str) { - if (!str) + if (!str) return(0); int len = 0; - + while (*str != 0) { str++; @@ -85,7 +85,7 @@ public: virtual void* getUniquePointer(void*oldPtr) = 0; virtual void startSerialization() = 0; - + virtual void finishSerialization() = 0; virtual const char* findNameForPointer(const void* ptr) const = 0; @@ -98,6 +98,9 @@ public: virtual void setSerializationFlags(int flags) = 0; + virtual int getNumChunks() const = 0; + + virtual const btChunk* getChunk(int chunkIndex) const = 0; }; @@ -110,6 +113,8 @@ public: # define BT_MAKE_ID(a,b,c,d) ( (int)(d)<<24 | (int)(c)<<16 | (b)<<8 | (a) ) #endif + +#define BT_MULTIBODY_CODE BT_MAKE_ID('M','B','D','Y') #define BT_SOFTBODY_CODE BT_MAKE_ID('S','B','D','Y') #define BT_COLLISIONOBJECT_CODE BT_MAKE_ID('C','O','B','J') #define BT_RIGIDBODY_CODE BT_MAKE_ID('R','B','D','Y') @@ -134,21 +139,46 @@ struct btPointerUid }; }; +struct btBulletSerializedArrays +{ + btBulletSerializedArrays() + { + } + btAlignedObjectArray<struct btQuantizedBvhDoubleData*> m_bvhsDouble; + btAlignedObjectArray<struct btQuantizedBvhFloatData*> m_bvhsFloat; + btAlignedObjectArray<struct btCollisionShapeData*> m_colShapeData; + btAlignedObjectArray<struct btDynamicsWorldDoubleData*> m_dynamicWorldInfoDataDouble; + btAlignedObjectArray<struct btDynamicsWorldFloatData*> m_dynamicWorldInfoDataFloat; + btAlignedObjectArray<struct btRigidBodyDoubleData*> m_rigidBodyDataDouble; + btAlignedObjectArray<struct btRigidBodyFloatData*> m_rigidBodyDataFloat; + btAlignedObjectArray<struct btCollisionObjectDoubleData*> m_collisionObjectDataDouble; + btAlignedObjectArray<struct btCollisionObjectFloatData*> m_collisionObjectDataFloat; + btAlignedObjectArray<struct btTypedConstraintFloatData*> m_constraintDataFloat; + btAlignedObjectArray<struct btTypedConstraintDoubleData*> m_constraintDataDouble; + btAlignedObjectArray<struct btTypedConstraintData*> m_constraintData;//for backwards compatibility + btAlignedObjectArray<struct btSoftBodyFloatData*> m_softBodyFloatData; + btAlignedObjectArray<struct btSoftBodyDoubleData*> m_softBodyDoubleData; + +}; + + ///The btDefaultSerializer is the main Bullet serialization class. ///The constructor takes an optional argument for backwards compatibility, it is recommended to leave this empty/zero. class btDefaultSerializer : public btSerializer { +protected: btAlignedObjectArray<char*> mTypes; btAlignedObjectArray<short*> mStructs; btAlignedObjectArray<short> mTlens; btHashMap<btHashInt, int> mStructReverse; btHashMap<btHashString,int> mTypeLookup; - + + btHashMap<btHashPtr,void*> m_chunkP; - + btHashMap<btHashPtr,const char*> m_nameMap; btHashMap<btHashPtr,btPointerUid> m_uniquePointers; @@ -156,6 +186,7 @@ class btDefaultSerializer : public btSerializer int m_totalSize; unsigned char* m_buffer; + bool m_ownsBuffer; int m_currentSize; void* m_dna; int m_dnaLength; @@ -164,10 +195,11 @@ class btDefaultSerializer : public btSerializer btAlignedObjectArray<btChunk*> m_chunkPtrs; - + protected: - virtual void* findPointer(void* oldPtr) + + virtual void* findPointer(void* oldPtr) { void** ptr = m_chunkP.find(oldPtr); if (ptr && *ptr) @@ -175,11 +207,11 @@ protected: return 0; } - - void writeDNA() + + virtual void writeDNA() { btChunk* dnaChunk = allocate(m_dnaLength,1); memcpy(dnaChunk->m_oldPtr,m_dna,m_dnaLength); @@ -193,7 +225,7 @@ protected: const int* valuePtr = mTypeLookup.find(key); if (valuePtr) return *valuePtr; - + return -1; } @@ -205,7 +237,7 @@ protected: int littleEndian= 1; littleEndian= ((char*)&littleEndian)[0]; - + m_dna = btAlignedAlloc(dnalen,16); memcpy(m_dna,bdnaOrg,dnalen); @@ -233,16 +265,16 @@ protected: // Parse names if (!littleEndian) *intPtr = btSwapEndian(*intPtr); - + dataLen = *intPtr; - + intPtr++; cp = (char*)intPtr; int i; for ( i=0; i<dataLen; i++) { - + while (*cp)cp++; cp++; } @@ -260,11 +292,11 @@ protected: if (!littleEndian) *intPtr = btSwapEndian(*intPtr); - + dataLen = *intPtr; intPtr++; - + cp = (char*)intPtr; for (i=0; i<dataLen; i++) { @@ -315,7 +347,7 @@ protected: if (!littleEndian) *intPtr = btSwapEndian(*intPtr); - dataLen = *intPtr ; + dataLen = *intPtr ; intPtr++; @@ -323,7 +355,7 @@ protected: for (i=0; i<dataLen; i++) { mStructs.push_back (shtPtr); - + if (!littleEndian) { shtPtr[0]= btSwapEndian(shtPtr[0]); @@ -353,20 +385,28 @@ protected: } } -public: - +public: + + btHashMap<btHashPtr,void*> m_skipPointers; - - btDefaultSerializer(int totalSize=0) + btDefaultSerializer(int totalSize=0, unsigned char* buffer=0) :m_totalSize(totalSize), m_currentSize(0), m_dna(0), m_dnaLength(0), m_serializationFlags(0) { - m_buffer = m_totalSize?(unsigned char*)btAlignedAlloc(totalSize,16):0; - + if (buffer==0) + { + m_buffer = m_totalSize?(unsigned char*)btAlignedAlloc(totalSize,16):0; + m_ownsBuffer = true; + } else + { + m_buffer = buffer; + m_ownsBuffer = false; + } + const bool VOID_IS_8 = ((sizeof(void*)==8)); #ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES @@ -385,7 +425,7 @@ public: btAssert(0); #endif } - + #else //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES if (VOID_IS_8) { @@ -395,27 +435,33 @@ public: initDNA((const char*)sBulletDNAstr,sBulletDNAlen); } #endif //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES - + } - virtual ~btDefaultSerializer() + virtual ~btDefaultSerializer() { - if (m_buffer) + if (m_buffer && m_ownsBuffer) btAlignedFree(m_buffer); if (m_dna) btAlignedFree(m_dna); } + void insertHeader() + { + writeHeader(m_buffer); + m_currentSize += BT_HEADER_LENGTH; + } + void writeHeader(unsigned char* buffer) const { - + #ifdef BT_USE_DOUBLE_PRECISION memcpy(buffer, "BULLETd", 7); #else memcpy(buffer, "BULLETf", 7); #endif //BT_USE_DOUBLE_PRECISION - + int littleEndian= 1; littleEndian= ((char*)&littleEndian)[0]; @@ -429,7 +475,7 @@ public: if (littleEndian) { - buffer[8]='v'; + buffer[8]='v'; } else { buffer[8]='V'; @@ -438,7 +484,7 @@ public: buffer[9] = '2'; buffer[10] = '8'; - buffer[11] = '2'; + buffer[11] = '4'; } @@ -450,7 +496,7 @@ public: unsigned char* buffer = internalAlloc(BT_HEADER_LENGTH); writeHeader(buffer); } - + } virtual void finishSerialization() @@ -486,6 +532,7 @@ public: mTlens.clear(); mStructReverse.clear(); mTypeLookup.clear(); + m_skipPointers.clear(); m_chunkP.clear(); m_nameMap.clear(); m_uniquePointers.clear(); @@ -502,8 +549,15 @@ public: { return uptr->m_ptr; } + + void** ptr2 = m_skipPointers[oldPtr]; + if (ptr2) + { + return 0; + } + m_uniqueIdGenerator++; - + btPointerUid uid; uid.m_uniqueIds[0] = m_uniqueIdGenerator; uid.m_uniqueIds[1] = m_uniqueIdGenerator; @@ -530,17 +584,17 @@ public: } chunk->m_dna_nr = getReverseType(structType); - + chunk->m_chunkCode = chunkCode; - + void* uniquePtr = getUniquePointer(oldPtr); - + m_chunkP.insert(oldPtr,uniquePtr);//chunk->m_oldPtr); chunk->m_oldPtr = uniquePtr;//oldPtr; - + } - + virtual unsigned char* internalAlloc(size_t size) { unsigned char* ptr = 0; @@ -558,7 +612,7 @@ public: return ptr; } - + virtual btChunk* allocate(size_t size, int numElements) { @@ -566,15 +620,15 @@ public: unsigned char* ptr = internalAlloc(int(size)*numElements+sizeof(btChunk)); unsigned char* data = ptr + sizeof(btChunk); - + btChunk* chunk = (btChunk*)ptr; chunk->m_chunkCode = 0; chunk->m_oldPtr = data; chunk->m_length = int(size)*numElements; chunk->m_number = numElements; - + m_chunkPtrs.push_back(chunk); - + return chunk; } @@ -631,9 +685,202 @@ public: { m_serializationFlags = flags; } + int getNumChunks() const + { + return m_chunkPtrs.size(); + } + const btChunk* getChunk(int chunkIndex) const + { + return m_chunkPtrs[chunkIndex]; + } }; +///In general it is best to use btDefaultSerializer, +///in particular when writing the data to disk or sending it over the network. +///The btInMemorySerializer is experimental and only suitable in a few cases. +///The btInMemorySerializer takes a shortcut and can be useful to create a deep-copy +///of objects. There will be a demo on how to use the btInMemorySerializer. +#ifdef ENABLE_INMEMORY_SERIALIZER + +struct btInMemorySerializer : public btDefaultSerializer +{ + btHashMap<btHashPtr,btChunk*> m_uid2ChunkPtr; + btHashMap<btHashPtr,void*> m_orgPtr2UniqueDataPtr; + btHashMap<btHashString,const void*> m_names2Ptr; + + + btBulletSerializedArrays m_arrays; + + btInMemorySerializer(int totalSize=0, unsigned char* buffer=0) + :btDefaultSerializer(totalSize,buffer) + { + + } + + virtual void startSerialization() + { + m_uid2ChunkPtr.clear(); + //todo: m_arrays.clear(); + btDefaultSerializer::startSerialization(); + } + + + + btChunk* findChunkFromUniquePointer(void* uniquePointer) + { + btChunk** chkPtr = m_uid2ChunkPtr[uniquePointer]; + if (chkPtr) + { + return *chkPtr; + } + return 0; + } + + virtual void registerNameForPointer(const void* ptr, const char* name) + { + btDefaultSerializer::registerNameForPointer(ptr,name); + m_names2Ptr.insert(name,ptr); + } + + virtual void finishSerialization() + { + } + + virtual void* getUniquePointer(void*oldPtr) + { + if (oldPtr==0) + return 0; + + // void* uniquePtr = getUniquePointer(oldPtr); + btChunk* chunk = findChunkFromUniquePointer(oldPtr); + if (chunk) + { + return chunk->m_oldPtr; + } else + { + const char* n = (const char*) oldPtr; + const void** ptr = m_names2Ptr[n]; + if (ptr) + { + return oldPtr; + } else + { + void** ptr2 = m_skipPointers[oldPtr]; + if (ptr2) + { + return 0; + } else + { + //If this assert hit, serialization happened in the wrong order + // 'getUniquePointer' + btAssert(0); + } + + } + return 0; + } + return oldPtr; + } + + virtual void finalizeChunk(btChunk* chunk, const char* structType, int chunkCode,void* oldPtr) + { + if (!(m_serializationFlags&BT_SERIALIZE_NO_DUPLICATE_ASSERT)) + { + btAssert(!findPointer(oldPtr)); + } + + chunk->m_dna_nr = getReverseType(structType); + chunk->m_chunkCode = chunkCode; + //void* uniquePtr = getUniquePointer(oldPtr); + m_chunkP.insert(oldPtr,oldPtr);//chunk->m_oldPtr); + // chunk->m_oldPtr = uniquePtr;//oldPtr; + + void* uid = findPointer(oldPtr); + m_uid2ChunkPtr.insert(uid,chunk); + + switch (chunk->m_chunkCode) + { + case BT_SOFTBODY_CODE: + { + #ifdef BT_USE_DOUBLE_PRECISION + m_arrays.m_softBodyDoubleData.push_back((btSoftBodyDoubleData*) chunk->m_oldPtr); + #else + m_arrays.m_softBodyFloatData.push_back((btSoftBodyFloatData*) chunk->m_oldPtr); + #endif + break; + } + case BT_COLLISIONOBJECT_CODE: + { + #ifdef BT_USE_DOUBLE_PRECISION + m_arrays.m_collisionObjectDataDouble.push_back((btCollisionObjectDoubleData*)chunk->m_oldPtr); + #else//BT_USE_DOUBLE_PRECISION + m_arrays.m_collisionObjectDataFloat.push_back((btCollisionObjectFloatData*)chunk->m_oldPtr); + #endif //BT_USE_DOUBLE_PRECISION + break; + } + case BT_RIGIDBODY_CODE: + { + #ifdef BT_USE_DOUBLE_PRECISION + m_arrays.m_rigidBodyDataDouble.push_back((btRigidBodyDoubleData*)chunk->m_oldPtr); + #else + m_arrays.m_rigidBodyDataFloat.push_back((btRigidBodyFloatData*)chunk->m_oldPtr); + #endif//BT_USE_DOUBLE_PRECISION + break; + }; + case BT_CONSTRAINT_CODE: + { + #ifdef BT_USE_DOUBLE_PRECISION + m_arrays.m_constraintDataDouble.push_back((btTypedConstraintDoubleData*)chunk->m_oldPtr); + #else + m_arrays.m_constraintDataFloat.push_back((btTypedConstraintFloatData*)chunk->m_oldPtr); + #endif + break; + } + case BT_QUANTIZED_BVH_CODE: + { + #ifdef BT_USE_DOUBLE_PRECISION + m_arrays.m_bvhsDouble.push_back((btQuantizedBvhDoubleData*) chunk->m_oldPtr); + #else + m_arrays.m_bvhsFloat.push_back((btQuantizedBvhFloatData*) chunk->m_oldPtr); + #endif + break; + } + + case BT_SHAPE_CODE: + { + btCollisionShapeData* shapeData = (btCollisionShapeData*) chunk->m_oldPtr; + m_arrays.m_colShapeData.push_back(shapeData); + break; + } + case BT_TRIANLGE_INFO_MAP: + case BT_ARRAY_CODE: + case BT_SBMATERIAL_CODE: + case BT_SBNODE_CODE: + case BT_DYNAMICSWORLD_CODE: + case BT_DNA_CODE: + { + break; + } + default: + { + } + }; + } + + int getNumChunks() const + { + return m_uid2ChunkPtr.size(); + } + + const btChunk* getChunk(int chunkIndex) const + { + return *m_uid2ChunkPtr.getAtIndex(chunkIndex); + } + +}; +#endif //ENABLE_INMEMORY_SERIALIZER + #endif //BT_SERIALIZER_H diff --git a/extern/bullet2/src/LinearMath/btSpatialAlgebra.h b/extern/bullet2/src/LinearMath/btSpatialAlgebra.h new file mode 100644 index 00000000000..8e59658bca7 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btSpatialAlgebra.h @@ -0,0 +1,331 @@ +/* +Copyright (c) 2003-2015 Erwin Coumans, Jakub Stepien + +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. +*/ + +///These spatial algebra classes are used for btMultiBody, +///see BulletDynamics/Featherstone + +#ifndef BT_SPATIAL_ALGEBRA_H +#define BT_SPATIAL_ALGEBRA_H + + +#include "btMatrix3x3.h" + +struct btSpatialForceVector +{ + btVector3 m_topVec, m_bottomVec; + // + btSpatialForceVector() { setZero(); } + btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {} + btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + setValue(ax, ay, az, lx, ly, lz); + } + // + void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; } + void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz); + } + // + void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } + void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az; + m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz; + } + // + const btVector3 & getLinear() const { return m_topVec; } + const btVector3 & getAngular() const { return m_bottomVec; } + // + void setLinear(const btVector3 &linear) { m_topVec = linear; } + void setAngular(const btVector3 &angular) { m_bottomVec = angular; } + // + void addAngular(const btVector3 &angular) { m_bottomVec += angular; } + void addLinear(const btVector3 &linear) { m_topVec += linear; } + // + void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } + // + btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } + btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } + btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); } + btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); } + btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); } + btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); } + //btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; } +}; + +struct btSpatialMotionVector +{ + btVector3 m_topVec, m_bottomVec; + // + btSpatialMotionVector() { setZero(); } + btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {} + // + void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; } + void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz); + } + // + void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } + void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az; + m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz; + } + // + const btVector3 & getAngular() const { return m_topVec; } + const btVector3 & getLinear() const { return m_bottomVec; } + // + void setAngular(const btVector3 &angular) { m_topVec = angular; } + void setLinear(const btVector3 &linear) { m_bottomVec = linear; } + // + void addAngular(const btVector3 &angular) { m_topVec += angular; } + void addLinear(const btVector3 &linear) { m_bottomVec += linear; } + // + void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } + // + btScalar dot(const btSpatialForceVector &b) const + { + return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec); + } + // + template<typename SpatialVectorType> + void cross(const SpatialVectorType &b, SpatialVectorType &out) const + { + out.m_topVec = m_topVec.cross(b.m_topVec); + out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); + } + template<typename SpatialVectorType> + SpatialVectorType cross(const SpatialVectorType &b) const + { + SpatialVectorType out; + out.m_topVec = m_topVec.cross(b.m_topVec); + out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); + return out; + } + // + btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } + btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } + btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; } + btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); } + btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); } + btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); } + btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); } +}; + +struct btSymmetricSpatialDyad +{ + btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat; + // + btSymmetricSpatialDyad() { setIdentity(); } + btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); } + // + void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) + { + m_topLeftMat = topLeftMat; + m_topRightMat = topRightMat; + m_bottomLeftMat = bottomLeftMat; + } + // + void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) + { + m_topLeftMat += topLeftMat; + m_topRightMat += topRightMat; + m_bottomLeftMat += bottomLeftMat; + } + // + void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity(); } + // + btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat) + { + m_topLeftMat -= mat.m_topLeftMat; + m_topRightMat -= mat.m_topRightMat; + m_bottomLeftMat -= mat.m_bottomLeftMat; + return *this; + } + // + btSpatialForceVector operator * (const btSpatialMotionVector &vec) + { + return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec); + } +}; + +struct btSpatialTransformationMatrix +{ + btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat; + btVector3 m_trnVec; + // + enum eOutputOperation + { + None = 0, + Add = 1, + Subtract = 2 + }; + // + template<typename SpatialVectorType> + void transform( const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) + { + if(outOp == None) + { + outVec.m_topVec = m_rotMat * inVec.m_topVec; + outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; + } + else if(outOp == Add) + { + outVec.m_topVec += m_rotMat * inVec.m_topVec; + outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; + } + else if(outOp == Subtract) + { + outVec.m_topVec -= m_rotMat * inVec.m_topVec; + outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; + } + + } + + template<typename SpatialVectorType> + void transformRotationOnly( const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) + { + if(outOp == None) + { + outVec.m_topVec = m_rotMat * inVec.m_topVec; + outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec; + } + else if(outOp == Add) + { + outVec.m_topVec += m_rotMat * inVec.m_topVec; + outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec; + } + else if(outOp == Subtract) + { + outVec.m_topVec -= m_rotMat * inVec.m_topVec; + outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec; + } + + } + + template<typename SpatialVectorType> + void transformInverse( const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) + { + if(outOp == None) + { + outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); + } + else if(outOp == Add) + { + outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); + } + else if(outOp == Subtract) + { + outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); + } + } + + template<typename SpatialVectorType> + void transformInverseRotationOnly( const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) + { + if(outOp == None) + { + outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec; + } + else if(outOp == Add) + { + outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec; + } + else if(outOp == Subtract) + { + outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec; + } + + } + + void transformInverse( const btSymmetricSpatialDyad &inMat, + btSymmetricSpatialDyad &outMat, + eOutputOperation outOp = None) + { + const btMatrix3x3 r_cross( 0, -m_trnVec[2], m_trnVec[1], + m_trnVec[2], 0, -m_trnVec[0], + -m_trnVec[1], m_trnVec[0], 0); + + + if(outOp == None) + { + outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; + outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; + } + else if(outOp == Add) + { + outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; + outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; + } + else if(outOp == Subtract) + { + outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; + outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; + } + } + + template<typename SpatialVectorType> + SpatialVectorType operator * (const SpatialVectorType &vec) + { + SpatialVectorType out; + transform(vec, out); + return out; + } +}; + +template<typename SpatialVectorType> +void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out) +{ + //output op maybe? + + out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); + out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); + out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); + //maybe simple a*spatTranspose(a) would be nicer? +} + +template<typename SpatialVectorType> +btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b) +{ + btSymmetricSpatialDyad out; + + out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); + out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); + out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); + + return out; + //maybe simple a*spatTranspose(a) would be nicer? +} + +#endif //BT_SPATIAL_ALGEBRA_H + diff --git a/extern/bullet2/src/LinearMath/btTransform.h b/extern/bullet2/src/LinearMath/btTransform.h index 907627379bf..d4f939a5d99 100644 --- a/extern/bullet2/src/LinearMath/btTransform.h +++ b/extern/bullet2/src/LinearMath/btTransform.h @@ -127,7 +127,7 @@ public: /**@brief Set from an array - * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ + * @param m A pointer to a 16 element array (12 rotation(row major padded on the right by 1), and 3 translation */ void setFromOpenGLMatrix(const btScalar *m) { m_basis.setFromOpenGLSubMatrix(m); @@ -135,7 +135,7 @@ public: } /**@brief Fill an array representation - * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ + * @param m A pointer to a 16 element array (12 rotation(row major padded on the right by 1), and 3 translation */ void getOpenGLMatrix(btScalar *m) const { m_basis.getOpenGLSubMatrix(m); diff --git a/extern/bullet2/src/LinearMath/btVector3.cpp b/extern/bullet2/src/LinearMath/btVector3.cpp index 2ba7029c9be..e05bdccd67e 100644 --- a/extern/bullet2/src/LinearMath/btVector3.cpp +++ b/extern/bullet2/src/LinearMath/btVector3.cpp @@ -15,9 +15,9 @@ This source version has been altered. */ -//#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 "btVector3.h" @@ -63,7 +63,7 @@ long _maxdot_large( const float *vv, const float *vec, unsigned long count, floa float4 stack_array[ STACK_ARRAY_COUNT ]; #if DEBUG - memset( stack_array, -1, STACK_ARRAY_COUNT * sizeof(stack_array[0]) ); + //memset( stack_array, -1, STACK_ARRAY_COUNT * sizeof(stack_array[0]) ); #endif size_t index; @@ -448,7 +448,7 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa float4 stack_array[ STACK_ARRAY_COUNT ]; #if DEBUG - memset( stack_array, -1, STACK_ARRAY_COUNT * sizeof(stack_array[0]) ); + //memset( stack_array, -1, STACK_ARRAY_COUNT * sizeof(stack_array[0]) ); #endif size_t index; diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h index 112b70dd66b..839b19c1449 100644 --- a/extern/bullet2/src/LinearMath/btVector3.h +++ b/extern/bullet2/src/LinearMath/btVector3.h @@ -501,10 +501,10 @@ public: __m128 tmp3 = _mm_add_ps(r0,r1); mVec128 = tmp3; #elif defined(BT_USE_NEON) - mVec128 = vsubq_f32(v1.mVec128, v0.mVec128); - mVec128 = vmulq_n_f32(mVec128, rt); - mVec128 = vaddq_f32(mVec128, v0.mVec128); -#else + float32x4_t vl = vsubq_f32(v1.mVec128, v0.mVec128); + vl = vmulq_n_f32(vl, rt); + mVec128 = vaddq_f32(vl, v0.mVec128); +#else btScalar s = btScalar(1.0) - rt; m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0]; m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1]; |