diff options
Diffstat (limited to 'extern/bullet2')
103 files changed, 11574 insertions, 1294 deletions
diff --git a/extern/bullet2/CMakeLists.txt b/extern/bullet2/CMakeLists.txt index 02ca2cd3755..2b2c18c0685 100644 --- a/extern/bullet2/CMakeLists.txt +++ b/extern/bullet2/CMakeLists.txt @@ -67,6 +67,8 @@ set(SRC 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/btBvhTriangleMeshShape.cpp @@ -140,6 +142,7 @@ set(SRC src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp + src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp src/BulletDynamics/Dynamics/btRigidBody.cpp src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -147,6 +150,15 @@ set(SRC 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 + src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp + src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp + src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp + src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp + src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp + src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp src/BulletSoftBody/btSoftBody.cpp src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @@ -204,6 +216,8 @@ set(SRC 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/btBvhTriangleMeshShape.h @@ -308,6 +322,7 @@ set(SRC 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 @@ -318,6 +333,22 @@ set(SRC src/BulletDynamics/Vehicle/btWheelInfo.h src/BulletDynamics/Character/btCharacterControllerInterface.h src/BulletDynamics/Character/btKinematicCharacterController.h + src/BulletDynamics/Featherstone/btMultiBody.h + src/BulletDynamics/Featherstone/btMultiBodyConstraint.h + src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h + src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h + src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h + src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h + src/BulletDynamics/Featherstone/btMultiBodyLink.h + src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h + src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h + src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h + src/BulletDynamics/MLCPSolvers/btDantzigLCP.h + src/BulletDynamics/MLCPSolvers/btDantzigSolver.h + src/BulletDynamics/MLCPSolvers/btMLCPSolver.h + src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h + src/BulletDynamics/MLCPSolvers/btPATHSolver.h + src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h src/BulletSoftBody/btSoftBody.h src/BulletSoftBody/btSoftBodyInternals.h @@ -359,6 +390,7 @@ set(SRC src/LinearMath/btTransformUtil.h src/LinearMath/btVector3.h src/LinearMath/btPolarDecomposition.h + src/LinearMath/btMatrixX.h src/btBulletCollisionCommon.h diff --git a/extern/bullet2/patches/ghost_character.patch b/extern/bullet2/patches/ghost_character.patch deleted file mode 100644 index d4098cb8bc2..00000000000 --- a/extern/bullet2/patches/ghost_character.patch +++ /dev/null @@ -1,28 +0,0 @@ -Index: extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp -=================================================================== ---- extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp (revision 49183) -+++ extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp (working copy) -@@ -77,6 +77,9 @@ - if (convexResult.m_hitCollisionObject == m_me) - return btScalar(1.0); - -+ if (!convexResult.m_hitCollisionObject->hasContactResponse()) -+ return btScalar(1.0); -+ - btVector3 hitNormalWorld; - if (normalInWorldSpace) - { -@@ -173,7 +176,12 @@ - m_manifoldArray.resize(0); - - btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i]; -- -+ btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject); -+ btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject); -+ -+ if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse())) -+ continue; -+ - if (collisionPair->m_algorithm) - collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray); - diff --git a/extern/bullet2/patches/ghost_softbody.patch b/extern/bullet2/patches/ghost_softbody.patch deleted file mode 100644 index b150d57040d..00000000000 --- a/extern/bullet2/patches/ghost_softbody.patch +++ /dev/null @@ -1,42 +0,0 @@ -Index: extern/bullet2/src/BulletSoftBody/btSoftBody.cpp -=================================================================== ---- extern/bullet2/src/BulletSoftBody/btSoftBody.cpp (Revision 43904) -+++ extern/bullet2/src/BulletSoftBody/btSoftBody.cpp (Revision 43905) -@@ -2780,21 +2780,23 @@ - { - const RContact& c = psb->m_rcontacts[i]; - const sCti& cti = c.m_cti; -- btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj); - -- const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); -- const btVector3 vb = c.m_node->m_x-c.m_node->m_q; -- const btVector3 vr = vb-va; -- const btScalar dn = btDot(vr, cti.m_normal); -- if(dn<=SIMD_EPSILON) -- { -- const btScalar dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg ); -- const btVector3 fv = vr - (cti.m_normal * dn); -- // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient -- const btVector3 impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst ); -- c.m_node->m_x -= impulse * c.m_c2; -- if (tmpRigid) -- tmpRigid->applyImpulse(impulse,c.m_c1); -+ if (cti.m_colObj->hasContactResponse()) { -+ btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj); -+ const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); -+ const btVector3 vb = c.m_node->m_x-c.m_node->m_q; -+ const btVector3 vr = vb-va; -+ const btScalar dn = btDot(vr, cti.m_normal); -+ if(dn<=SIMD_EPSILON) -+ { -+ const btScalar dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg ); -+ const btVector3 fv = vr - (cti.m_normal * dn); -+ // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient -+ const btVector3 impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst ); -+ c.m_node->m_x -= impulse * c.m_c2; -+ if (tmpRigid) -+ tmpRigid->applyImpulse(impulse,c.m_c1); -+ } - } - } - } diff --git a/extern/bullet2/readme.txt b/extern/bullet2/readme.txt index dd806d40fac..3c09dad1fbf 100644 --- a/extern/bullet2/readme.txt +++ b/extern/bullet2/readme.txt @@ -4,11 +4,5 @@ Questions? mail blender at erwincoumans.com, or check the bf-blender mailing lis Thanks, Erwin -Apply patches/ghost_softbody.patch to prevent softbodies being hit by ghost objects. -Originally committed in blender svn revision: 43905. - -Apply patches/ghost_character.patch to prevent characters from colliding with ghost objects. -Mitchell - Apply patches/convex_hull.patch to add access to the convex hull operation, used in the BMesh convex hull operator. diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 1ebb37797d9..89c307d14ca 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -25,7 +25,6 @@ class btOverlappingPairCache; struct btCollisionObjectWrapper; class btPersistentManifold; -class btStackAlloc; class btPoolAllocator; struct btDispatcherInfo @@ -47,8 +46,7 @@ struct btDispatcherInfo m_useEpa(true), m_allowedCcdPenetration(btScalar(0.04)), m_useConvexConservativeDistanceUtil(false), - m_convexConservativeDistanceThreshold(0.0f), - m_stackAllocator(0) + m_convexConservativeDistanceThreshold(0.0f) { } @@ -64,7 +62,6 @@ struct btDispatcherInfo btScalar m_allowedCcdPenetration; bool m_useConvexConservativeDistanceUtil; btScalar m_convexConservativeDistanceThreshold; - btStackAlloc* m_stackAllocator; }; ///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs. diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index 041bbe05ae2..ae22dadc73a 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -53,7 +53,7 @@ btHashedOverlappingPairCache::~btHashedOverlappingPairCache() void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) { - if (pair.m_algorithm) + if (pair.m_algorithm && dispatcher) { { pair.m_algorithm->~btCollisionAlgorithm(); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index 7a3806c1d28..eee90e473a9 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -96,6 +96,12 @@ class btHashedOverlappingPairCache : public btOverlappingPairCache btOverlapFilterCallback* m_overlapFilterCallback; bool m_blockedForChanges; +protected: + + btAlignedObjectArray<int> m_hashTable; + btAlignedObjectArray<int> m_next; + btOverlappingPairCallback* m_ghostPairCallback; + public: btHashedOverlappingPairCache(); @@ -265,11 +271,6 @@ private: virtual void sortOverlappingPairs(btDispatcher* dispatcher); -protected: - - btAlignedObjectArray<int> m_hashTable; - btAlignedObjectArray<int> m_next; - btOverlappingPairCallback* m_ghostPairCallback; }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h index f63e0923b78..66949849448 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -18,7 +18,6 @@ subject to the following restrictions: struct btCollisionAlgorithmCreateFunc; -class btStackAlloc; class btPoolAllocator; ///btCollisionConfiguration allows to configure Bullet collision detection @@ -38,7 +37,6 @@ public: virtual btPoolAllocator* getCollisionAlgorithmPool() = 0; - virtual btStackAlloc* getStackAllocator() = 0; virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index cf8ed59a541..d0924100058 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -38,7 +38,8 @@ btCollisionObject::btCollisionObject() m_hitFraction(btScalar(1.)), m_ccdSweptSphereRadius(btScalar(0.)), m_ccdMotionThreshold(btScalar(0.)), - m_checkCollideWith(false) + m_checkCollideWith(false), + m_updateRevision(0) { m_worldTransform.setIdentity(); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 2f17967fe0f..89cad168210 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -92,7 +92,11 @@ protected: int m_internalType; ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer - void* m_userObjectPointer; + union + { + void* m_userObjectPointer; + int m_userIndex; + }; ///time of impact calculation btScalar m_hitFraction; @@ -106,6 +110,9 @@ protected: /// If some object should have elaborate collision filtering by sub-classes int m_checkCollideWith; + ///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; @@ -135,7 +142,8 @@ public: CO_GHOST_OBJECT=4, CO_SOFT_BODY=8, CO_HF_FLUID=16, - CO_USER_TYPE=32 + CO_USER_TYPE=32, + CO_FEATHERSTONE_LINK=64 }; enum AnisotropicFrictionFlags @@ -202,6 +210,7 @@ public: virtual void setCollisionShape(btCollisionShape* collisionShape) { + m_updateRevision++; m_collisionShape = collisionShape; m_rootCollisionShape = collisionShape; } @@ -257,6 +266,7 @@ public: void setRestitution(btScalar rest) { + m_updateRevision++; m_restitution = rest; } btScalar getRestitution() const @@ -265,6 +275,7 @@ public: } void setFriction(btScalar frict) { + m_updateRevision++; m_friction = frict; } btScalar getFriction() const @@ -274,6 +285,7 @@ public: void setRollingFriction(btScalar frict) { + m_updateRevision++; m_rollingFriction = frict; } btScalar getRollingFriction() const @@ -300,6 +312,7 @@ public: void setWorldTransform(const btTransform& worldTrans) { + m_updateRevision++; m_worldTransform = worldTrans; } @@ -332,16 +345,19 @@ public: void setInterpolationWorldTransform(const btTransform& trans) { + m_updateRevision++; m_interpolationWorldTransform = trans; } void setInterpolationLinearVelocity(const btVector3& linvel) { + m_updateRevision++; m_interpolationLinearVelocity = linvel; } void setInterpolationAngularVelocity(const btVector3& angvel) { + m_updateRevision++; m_interpolationAngularVelocity = angvel; } @@ -431,13 +447,28 @@ public: { return m_userObjectPointer; } - + + int getUserIndex() const + { + return m_userIndex; + } ///users can point to their objects, userPointer is not used by Bullet void setUserPointer(void* userPointer) { m_userObjectPointer = userPointer; } + ///users can point to their objects, userPointer is not used by Bullet + void setUserIndex(int index) + { + m_userIndex = index; + } + + int getUpdateRevisionInternal() const + { + return m_updateRevision; + } + inline bool checkCollideWith(const btCollisionObject* co) const { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 4c09291692d..093c6f9b200 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -31,11 +31,10 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btDbvt.h" #include "LinearMath/btAabbUtil2.h" #include "LinearMath/btQuickprof.h" -#include "LinearMath/btStackAlloc.h" #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 @@ -73,8 +72,6 @@ m_broadphasePairCache(pairCache), m_debugDrawer(0), m_forceUpdateAllAabbs(true) { - m_stackAlloc = collisionConfiguration->getStackAllocator(); - m_dispatchInfo.m_stackAllocator = m_stackAlloc; } @@ -290,13 +287,19 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con btConvexShape* convexShape = (btConvexShape*) collisionShape; btVoronoiSimplexSolver simplexSolver; -#define USE_SUBSIMPLEX_CONVEX_CAST 1 -#ifdef USE_SUBSIMPLEX_CONVEX_CAST - btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver); -#else - //btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver); + btSubsimplexConvexCast subSimplexConvexCaster(castShape,convexShape,&simplexSolver); + + btGjkConvexCast gjkConvexCaster(castShape,convexShape,&simplexSolver); + //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); -#endif //#USE_SUBSIMPLEX_CONVEX_CAST + bool condition = true; + btConvexCast* convexCasterPtr = 0; + if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest) + convexCasterPtr = &subSimplexConvexCaster; + else + convexCasterPtr = &gjkConvexCaster; + + btConvexCast& convexCaster = *convexCasterPtr; if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) { @@ -328,34 +331,26 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con } else { if (collisionShape->isConcave()) { - // BT_PROFILE("rayTestConcave"); - if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) - { - ///optimized version for btBvhTriangleMeshShape - btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; - btTransform worldTocollisionObject = colObjWorldTransform.inverse(); - btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); - btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); - //ConvexCast::CastResult + //ConvexCast::CastResult struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback { btCollisionWorld::RayResultCallback* m_resultCallback; const btCollisionObject* m_collisionObject; - btTriangleMeshShape* m_triangleMesh; + const btConcaveShape* m_triangleMesh; btTransform m_colObjWorldTransform; BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh,const btTransform& colObjWorldTransform): - //@BP Mod - btTriangleRaycastCallback(from,to, resultCallback->m_flags), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh), - m_colObjWorldTransform(colObjWorldTransform) - { - } + btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,const btConcaveShape* triangleMesh,const btTransform& colObjWorldTransform): + //@BP Mod + btTriangleRaycastCallback(from,to, resultCallback->m_flags), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh), + m_colObjWorldTransform(colObjWorldTransform) + { + } virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) @@ -378,10 +373,28 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con }; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + // BT_PROFILE("rayTestConcave"); + if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + ///optimized version for btBvhTriangleMeshShape + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),triangleMesh,colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); - } else + } + 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 { //generic (slower) case btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index 9412242e8a3..b3fffdecd98 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://bulletphysics.com/Bullet/ +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. @@ -18,13 +18,11 @@ subject to the following restrictions: * @mainpage Bullet Documentation * * @section intro_sec Introduction - * Bullet Collision Detection & Physics SDK - * * 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 ). * * The main documentation is Bullet_User_Manual.pdf, included in the source code distribution. * There is the Physics Forum for feedback and general Collision Detection and Physics discussions. - * Please visit http://www.bulletphysics.com + * Please visit http://www.bulletphysics.org * * @section install_sec Installation * @@ -32,7 +30,16 @@ subject to the following restrictions: * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list * * @subsection step2 Step 2: Building - * Bullet main build system for all platforms is cmake, you can download http://www.cmake.org + * Bullet has multiple build systems, including premake, cmake and autotools. Premake and cmake support all platforms. + * Premake is included in the Bullet/build folder for Windows, Mac OSX and Linux. + * Under Windows you can click on Bullet/build/vs2010.bat to create Microsoft Visual Studio projects. + * On Mac OSX and Linux you can open a terminal and generate Makefile, codeblocks or Xcode4 projects: + * cd Bullet/build + * ./premake4_osx gmake or ./premake4_linux gmake or ./premake4_linux64 gmake or (for Mac) ./premake4_osx xcode4 + * cd Bullet/build/gmake + * make + * + * An alternative to premake is cmake. You can download cmake from http://www.cmake.org * cmake can autogenerate projectfiles for Microsoft Visual Studio, Apple Xcode, KDevelop and Unix Makefiles. * The easiest is to run the CMake cmake-gui graphical user interface and choose the options and generate projectfiles. * You can also use cmake in the command-line. Here are some examples for various platforms: @@ -65,7 +72,6 @@ subject to the following restrictions: #ifndef BT_COLLISION_WORLD_H #define BT_COLLISION_WORLD_H -class btStackAlloc; class btCollisionShape; class btConvexShape; class btBroadphaseInterface; @@ -91,8 +97,6 @@ protected: btDispatcherInfo m_dispatchInfo; - btStackAlloc* m_stackAlloc; - btBroadphaseInterface* m_broadphasePairCache; btIDebugDraw* m_debugDrawer; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 39b86a28918..991841ee20b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -11,6 +11,7 @@ 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 "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" @@ -22,6 +23,8 @@ subject to the following restrictions: #include "btManifoldResult.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" +btShapePairCallback gCompoundChildShapePairCallback = 0; + btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) :btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), m_isSwapped(isSwapped), @@ -129,6 +132,12 @@ public: childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); + if (gCompoundChildShapePairCallback) + { + if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape)) + return; + } + if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index b16fc524672..53675145637 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -11,6 +11,7 @@ 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_COMPOUND_COLLISION_ALGORITHM_H @@ -28,6 +29,10 @@ class btDispatcher; class btDispatcher; class btCollisionObject; +class btCollisionShape; +typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1); +extern btShapePairCallback gCompoundChildShapePairCallback; + /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm { @@ -37,6 +42,7 @@ class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm class btPersistentManifold* m_sharedManifold; bool m_ownsManifold; + int m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated void removeChildAlgorithms(); @@ -49,6 +55,12 @@ public: virtual ~btCompoundCollisionAlgorithm(); + btCollisionAlgorithm* getChildAlgorithm (int n) const + { + return m_childCollisionAlgorithms[n]; + } + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); @@ -63,6 +75,7 @@ public: } } + struct CreateFunc :public btCollisionAlgorithmCreateFunc { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp new file mode 100644 index 00000000000..a52dd34fe91 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -0,0 +1,421 @@ +/* +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. + +*/ + +#include "btCompoundCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/BroadphaseCollision/btDbvt.h" +#include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btAabbUtil2.h" +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + + +btShapePairCallback gCompoundCompoundChildShapePairCallback = 0; + +btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) +:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), +m_sharedManifold(ci.m_manifold) +{ + m_ownsManifold = false; + + void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16); + m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache(); + + const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; + btAssert (col0ObjWrap->getCollisionShape()->isCompound()); + + const btCollisionObjectWrapper* col1ObjWrap = body1Wrap; + btAssert (col1ObjWrap->getCollisionShape()->isCompound()); + + const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape()); + m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); + + const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape()); + m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); + + +} + + +btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm() +{ + removeChildAlgorithms(); + m_childCollisionAlgorithmCache->~btHashedSimplePairCache(); + btAlignedFree(m_childCollisionAlgorithmCache); +} + +void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +{ + int i; + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + for (i=0;i<pairs.size();i++) + { + if (pairs[i].m_userPointer) + { + + ((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray); + } + } +} + + +void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() +{ + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + + int numChildren = pairs.size(); + int i; + for (i=0;i<numChildren;i++) + { + if (pairs[i].m_userPointer) + { + btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer; + algo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algo); + } + } + m_childCollisionAlgorithmCache->removeAllPairs(); +} + +struct btCompoundCompoundLeafCallback : btDbvt::ICollide +{ + int m_numOverlapPairs; + + + const btCollisionObjectWrapper* m_compound0ColObjWrap; + const btCollisionObjectWrapper* m_compound1ColObjWrap; + btDispatcher* m_dispatcher; + const btDispatcherInfo& m_dispatchInfo; + btManifoldResult* m_resultOut; + + + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; + + btPersistentManifold* m_sharedManifold; + + btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap, + const btCollisionObjectWrapper* compound0ObjWrap, + btDispatcher* dispatcher, + const btDispatcherInfo& dispatchInfo, + btManifoldResult* resultOut, + btHashedSimplePairCache* childAlgorithmsCache, + btPersistentManifold* sharedManifold) + :m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), + m_childCollisionAlgorithmCache(childAlgorithmsCache), + m_sharedManifold(sharedManifold), + m_numOverlapPairs(0) + { + + } + + + + + void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1) + { + m_numOverlapPairs++; + + + int childIndex0 = leaf0->dataAsInt; + int childIndex1 = leaf1->dataAsInt; + + + btAssert(childIndex0>=0); + btAssert(childIndex1>=0); + + + const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape()); + btAssert(childIndex0<compoundShape0->getNumChildShapes()); + + const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape()); + btAssert(childIndex1<compoundShape1->getNumChildShapes()); + + const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0); + const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1); + + //backup + btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform(); + const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0); + btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ; + + btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform(); + const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1); + btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ; + + + //perform an AABB check first + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); + childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); + + if (gCompoundCompoundChildShapePairCallback) + { + if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1)) + return; + } + + if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0); + btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1); + + + btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1); + + btCollisionAlgorithm* colAlgo = 0; + + if (pair) + { + colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; + + } else + { + colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold); + pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1); + btAssert(pair); + pair->m_userPointer = colAlgo; + } + + btAssert(colAlgo); + + const btCollisionObjectWrapper* tmpWrap0 = 0; + const btCollisionObjectWrapper* tmpWrap1 = 0; + + tmpWrap0 = m_resultOut->getBody0Wrap(); + tmpWrap1 = m_resultOut->getBody1Wrap(); + + m_resultOut->setBody0Wrap(&compoundWrap0); + m_resultOut->setBody1Wrap(&compoundWrap1); + + m_resultOut->setShapeIdentifiersA(-1,childIndex0); + m_resultOut->setShapeIdentifiersB(-1,childIndex1); + + + colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut); + + m_resultOut->setBody0Wrap(tmpWrap0); + m_resultOut->setBody1Wrap(tmpWrap1); + + + + } + } +}; + + +static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, const btTransform& xform) +{ + btVector3 newmin,newmax; + btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax); + btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax); + return Intersect(a,newb); +} + + +static inline void MycollideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + btCompoundCompoundLeafCallback* callback) +{ + + if(root0&&root1) + { + int depth=1; + int treshold=btDbvt::DOUBLE_STACKSIZE-4; + btAlignedObjectArray<btDbvt::sStkNN> stkStack; + stkStack.resize(btDbvt::DOUBLE_STACKSIZE); + stkStack[0]=btDbvt::sStkNN(root0,root1); + do { + btDbvt::sStkNN p=stkStack[--depth]; + if(MyIntersect(p.a->volume,p.b->volume,xform)) + { + if(depth>treshold) + { + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]); + } + else + { + callback->Process(p.a,p.b); + } + } + } + } while(depth); + } +} + +void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; + const btCollisionObjectWrapper* col1ObjWrap= body1Wrap; + + btAssert (col0ObjWrap->getCollisionShape()->isCompound()); + btAssert (col1ObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape()); + const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape()); + + ///btCompoundShape might have changed: + ////make sure the internal child collision algorithm caches are still valid + if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1)) + { + ///clear all + removeChildAlgorithms(); + } + + + ///we need to refresh all contact manifolds + ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep + ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm + { + int i; + btManifoldArray manifoldArray; + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + for (i=0;i<pairs.size();i++) + { + if (pairs[i].m_userPointer) + { + btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer; + algo->getAllContactManifolds(manifoldArray); + for (int m=0;m<manifoldArray.size();m++) + { + if (manifoldArray[m]->getNumContacts()) + { + resultOut->setPersistentManifold(manifoldArray[m]); + resultOut->refreshContactPoints(); + resultOut->setPersistentManifold(0); + } + } + manifoldArray.resize(0); + } + } + } + + + const btDbvt* tree0 = compoundShape0->getDynamicAabbTree(); + const btDbvt* tree1 = compoundShape1->getDynamicAabbTree(); + + btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold); + + + const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform(); + MycollideTT(tree0->m_root,tree1->m_root,xform,&callback); + + //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs); + + //remove non-overlapping child pairs + + { + btAssert(m_removePairs.size()==0); + + //iterate over all children, perform an AABB check inside ProcessChildShape + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + + int i; + btManifoldArray manifoldArray; + + + + + + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + + for (i=0;i<pairs.size();i++) + { + if (pairs[i].m_userPointer) + { + btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer; + + { + btTransform orgTrans0; + const btCollisionShape* childShape0 = 0; + + btTransform newChildWorldTrans0; + btTransform orgInterpolationTrans0; + childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA); + orgTrans0 = col0ObjWrap->getWorldTransform(); + orgInterpolationTrans0 = col0ObjWrap->getWorldTransform(); + const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA); + newChildWorldTrans0 = orgTrans0*childTrans0 ; + childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); + } + + { + btTransform orgInterpolationTrans1; + const btCollisionShape* childShape1 = 0; + btTransform orgTrans1; + btTransform newChildWorldTrans1; + + childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB); + orgTrans1 = col1ObjWrap->getWorldTransform(); + orgInterpolationTrans1 = col1ObjWrap->getWorldTransform(); + const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB); + newChildWorldTrans1 = orgTrans1*childTrans1 ; + childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); + } + + + + if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + algo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algo); + m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB)); + } + } + } + for (int i=0;i<m_removePairs.size();i++) + { + m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB); + } + m_removePairs.clear(); + } + +} + +btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + btAssert(0); + return 0.f; + +} + + + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h new file mode 100644 index 00000000000..7e2d7ad7085 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h @@ -0,0 +1,90 @@ +/* +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. + +*/ + +#ifndef BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H +#define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H + +#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" + +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +class btDispatcher; +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/CollisionDispatch/btHashedSimplePairCache.h" +class btDispatcher; +class btCollisionObject; + +class btCollisionShape; +typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1); +extern btShapePairCallback gCompoundCompoundChildShapePairCallback; + +/// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes +class btCompoundCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; + btSimplePairArray m_removePairs; + + class btPersistentManifold* m_sharedManifold; + bool m_ownsManifold; + + + int m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated + int m_compoundShapeRevision1; + + void removeChildAlgorithms(); + +// void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + +public: + + btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + + virtual ~btCompoundCompoundCollisionAlgorithm(); + + + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray); + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); + return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + } + }; + + struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); + return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + } + }; + +}; + +#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp index 3e1afede1bf..4ec9ae71386 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp @@ -132,7 +132,6 @@ void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrap input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; } - input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0Wrap->getWorldTransform(); input.m_transformB = body1Wrap->getWorldTransform(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 18fde771b14..e23f5f7a88d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -76,21 +76,27 @@ void btConvexTriangleCallback::clearCache() } - -void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) +void btConvexTriangleCallback::processTriangle(btVector3* triangle,int +partId, int triangleIndex) { - - //just for debugging purposes - //printf("triangle %d",m_triangleCount++); + if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax)) + { + return; + } + + //just for debugging purposes + //printf("triangle %d",m_triangleCount++); - //aabb filter is already applied! + 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 diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 62f98a846f4..7f2722aa463 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -373,7 +373,6 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; } - input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0Wrap->getWorldTransform(); input.m_transformB = body1Wrap->getWorldTransform(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index 7faee6faf50..c3cacec4a4f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -19,6 +19,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h" + #include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" @@ -32,7 +34,6 @@ subject to the following restrictions: -#include "LinearMath/btStackAlloc.h" #include "LinearMath/btPoolAllocator.h" @@ -65,6 +66,10 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16); m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc),16); + m_compoundCompoundCreateFunc = new (mem)btCompoundCompoundCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16); m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc; mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16); @@ -106,16 +111,6 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); - if (constructionInfo.m_stackAlloc) - { - m_ownsStackAllocator = false; - this->m_stackAlloc = constructionInfo.m_stackAlloc; - } else - { - m_ownsStackAllocator = true; - void* mem = btAlignedAlloc(sizeof(btStackAlloc),16); - m_stackAlloc = new(mem)btStackAlloc(constructionInfo.m_defaultStackAllocatorSize); - } if (constructionInfo.m_persistentManifoldPool) { @@ -144,12 +139,6 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() { - if (m_ownsStackAllocator) - { - m_stackAlloc->destroy(); - m_stackAlloc->~btStackAlloc(); - btAlignedFree(m_stackAlloc); - } if (m_ownsCollisionAlgorithmPool) { m_collisionAlgorithmPool->~btPoolAllocator(); @@ -172,6 +161,9 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc(); btAlignedFree( m_compoundCreateFunc); + m_compoundCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree(m_compoundCompoundCreateFunc); + m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); btAlignedFree( m_swappedCompoundCreateFunc); @@ -275,6 +267,12 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg return m_swappedConvexConcaveCreateFunc; } + + if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) + { + return m_compoundCompoundCreateFunc; + } + if (btBroadphaseProxy::isCompound(proxyType0)) { return m_compoundCreateFunc; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 474785bfc7d..2078420e198 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -22,23 +22,19 @@ class btConvexPenetrationDepthSolver; struct btDefaultCollisionConstructionInfo { - btStackAlloc* m_stackAlloc; btPoolAllocator* m_persistentManifoldPool; btPoolAllocator* m_collisionAlgorithmPool; int m_defaultMaxPersistentManifoldPoolSize; int m_defaultMaxCollisionAlgorithmPoolSize; int m_customCollisionAlgorithmMaxElementSize; - int m_defaultStackAllocatorSize; int m_useEpaPenetrationAlgorithm; btDefaultCollisionConstructionInfo() - :m_stackAlloc(0), - m_persistentManifoldPool(0), + :m_persistentManifoldPool(0), m_collisionAlgorithmPool(0), m_defaultMaxPersistentManifoldPoolSize(4096), m_defaultMaxCollisionAlgorithmPoolSize(4096), m_customCollisionAlgorithmMaxElementSize(0), - m_defaultStackAllocatorSize(0), m_useEpaPenetrationAlgorithm(true) { } @@ -56,8 +52,6 @@ protected: int m_persistentManifoldPoolSize; - btStackAlloc* m_stackAlloc; - bool m_ownsStackAllocator; btPoolAllocator* m_persistentManifoldPool; bool m_ownsPersistentManifoldPool; @@ -75,6 +69,8 @@ protected: btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc; btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_compoundCompoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; btCollisionAlgorithmCreateFunc* m_sphereSphereCF; @@ -105,10 +101,6 @@ public: return m_collisionAlgorithmPool; } - virtual btStackAlloc* getStackAllocator() - { - return m_stackAlloc; - } virtual btVoronoiSimplexSolver* getSimplexSolver() { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp new file mode 100644 index 00000000000..cfcca5654de --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp @@ -0,0 +1,278 @@ +/* +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 "btHashedSimplePairCache.h" + + +#include <stdio.h> + +int gOverlappingSimplePairs = 0; +int gRemoveSimplePairs =0; +int gAddedSimplePairs =0; +int gFindSimplePairs =0; + + + + +btHashedSimplePairCache::btHashedSimplePairCache(): + m_blockedForChanges(false) +{ + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); + growTables(); +} + + + + +btHashedSimplePairCache::~btHashedSimplePairCache() +{ +} + + + + + + +void btHashedSimplePairCache::removeAllPairs() +{ + m_overlappingPairArray.clear(); + m_hashTable.clear(); + m_next.clear(); + + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); + growTables(); +} + + + +btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) +{ + gFindSimplePairs++; + + + /*if (indexA > indexB) + btSwap(indexA, indexB);*/ + + int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); + + if (hash >= m_hashTable.size()) + { + return NULL; + } + + int index = m_hashTable[hash]; + while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], indexA, indexB) == false) + { + index = m_next[index]; + } + + if (index == BT_SIMPLE_NULL_PAIR) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; +} + +//#include <stdio.h> + +void btHashedSimplePairCache::growTables() +{ + + int newCapacity = m_overlappingPairArray.capacity(); + + if (m_hashTable.size() < newCapacity) + { + //grow hashtable and next table + int curHashtableSize = m_hashTable.size(); + + m_hashTable.resize(newCapacity); + m_next.resize(newCapacity); + + + int i; + + for (i= 0; i < newCapacity; ++i) + { + m_hashTable[i] = BT_SIMPLE_NULL_PAIR; + } + for (i = 0; i < newCapacity; ++i) + { + m_next[i] = BT_SIMPLE_NULL_PAIR; + } + + for(i=0;i<curHashtableSize;i++) + { + + const btSimplePair& pair = m_overlappingPairArray[i]; + int indexA = pair.m_indexA; + int indexB = pair.m_indexB; + + int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + m_next[i] = m_hashTable[hashValue]; + m_hashTable[hashValue] = i; + } + + + } +} + +btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) +{ + + int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + + + btSimplePair* pair = internalFindPair(indexA, indexB, hash); + if (pair != NULL) + { + return pair; + } + + int count = m_overlappingPairArray.size(); + int oldCapacity = m_overlappingPairArray.capacity(); + void* mem = &m_overlappingPairArray.expandNonInitializing(); + + int newCapacity = m_overlappingPairArray.capacity(); + + if (oldCapacity < newCapacity) + { + growTables(); + //hash with new capacity + hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); + } + + pair = new (mem) btSimplePair(indexA,indexB); + + pair->m_userPointer = 0; + + m_next[count] = m_hashTable[hash]; + m_hashTable[hash] = count; + + return pair; +} + + + +void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) +{ + gRemoveSimplePairs++; + + + /*if (indexA > indexB) + btSwap(indexA, indexB);*/ + + int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); + + btSimplePair* pair = internalFindPair(indexA, indexB, hash); + if (pair == NULL) + { + return 0; + } + + + void* userData = pair->m_userPointer; + + + int pairIndex = int(pair - &m_overlappingPairArray[0]); + btAssert(pairIndex < m_overlappingPairArray.size()); + + // Remove the pair from the hash table. + int index = m_hashTable[hash]; + btAssert(index != BT_SIMPLE_NULL_PAIR); + + int previous = BT_SIMPLE_NULL_PAIR; + while (index != pairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_SIMPLE_NULL_PAIR) + { + btAssert(m_next[previous] == pairIndex); + m_next[previous] = m_next[pairIndex]; + } + else + { + m_hashTable[hash] = m_next[pairIndex]; + } + + // We now move the last pair into spot of the + // pair being removed. We need to fix the hash + // table indices to support the move. + + int lastPairIndex = m_overlappingPairArray.size() - 1; + + // If the removed pair is the last pair, we are done. + if (lastPairIndex == pairIndex) + { + m_overlappingPairArray.pop_back(); + return userData; + } + + // Remove the last pair from the hash table. + const btSimplePair* last = &m_overlappingPairArray[lastPairIndex]; + /* missing swap here too, Nat. */ + int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity()-1)); + + index = m_hashTable[lastHash]; + btAssert(index != BT_SIMPLE_NULL_PAIR); + + previous = BT_SIMPLE_NULL_PAIR; + while (index != lastPairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_SIMPLE_NULL_PAIR) + { + btAssert(m_next[previous] == lastPairIndex); + m_next[previous] = m_next[lastPairIndex]; + } + else + { + m_hashTable[lastHash] = m_next[lastPairIndex]; + } + + // Copy the last pair into the remove pair's spot. + m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex]; + + // Insert the last pair into the hash table + m_next[pairIndex] = m_hashTable[lastHash]; + m_hashTable[lastHash] = pairIndex; + + m_overlappingPairArray.pop_back(); + + return userData; +} +//#include <stdio.h> + + + + + + + + + + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h new file mode 100644 index 00000000000..e88ef97e968 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -0,0 +1,174 @@ +/* +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_HASHED_SIMPLE_PAIR_CACHE_H +#define BT_HASHED_SIMPLE_PAIR_CACHE_H + + + +#include "LinearMath/btAlignedObjectArray.h" + +const int BT_SIMPLE_NULL_PAIR=0xffffffff; + +struct btSimplePair +{ + btSimplePair(int indexA,int indexB) + :m_indexA(indexA), + m_indexB(indexB), + m_userPointer(0) + { + } + + int m_indexA; + int m_indexB; + union + { + void* m_userPointer; + int m_userValue; + }; +}; + +typedef btAlignedObjectArray<btSimplePair> btSimplePairArray; + + + +extern int gOverlappingSimplePairs; +extern int gRemoveSimplePairs; +extern int gAddedSimplePairs; +extern int gFindSimplePairs; + + + + +class btHashedSimplePairCache +{ + btSimplePairArray m_overlappingPairArray; + + bool m_blockedForChanges; + + +protected: + + btAlignedObjectArray<int> m_hashTable; + btAlignedObjectArray<int> m_next; + + +public: + btHashedSimplePairCache(); + virtual ~btHashedSimplePairCache(); + + void removeAllPairs(); + + virtual void* removeOverlappingPair(int indexA,int indexB); + + // Add a pair and return the new pair. If the pair already exists, + // no new pair is created and the old one is returned. + virtual btSimplePair* addOverlappingPair(int indexA,int indexB) + { + gAddedSimplePairs++; + + return internalAddPair(indexA,indexB); + } + + + virtual btSimplePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + + const btSimplePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + + btSimplePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const btSimplePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + + + btSimplePair* findPair(int indexA,int indexB); + + int GetCount() const { return m_overlappingPairArray.size(); } + + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } +private: + + btSimplePair* internalAddPair(int indexA, int indexB); + + void growTables(); + + SIMD_FORCE_INLINE bool equalsPair(const btSimplePair& pair, int indexA, int indexB) + { + return pair.m_indexA == indexA && pair.m_indexB == indexB; + } + + + + SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) + { + int key = static_cast<int>(((unsigned int)indexA) | (((unsigned int)indexB) <<16)); + // Thomas Wang's hash + + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return static_cast<unsigned int>(key); + } + + + + + + SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA , int proxyIdB, int hash) + { + + int index = m_hashTable[hash]; + + while( index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false) + { + index = m_next[index]; + } + + if ( index == BT_SIMPLE_NULL_PAIR ) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; + } + + +}; + + + + +#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index 12f422f193f..0aa75f2bff3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -273,6 +273,8 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf + + void btCompoundShape::setLocalScaling(const btVector3& scaling) { @@ -283,7 +285,7 @@ void btCompoundShape::setLocalScaling(const btVector3& scaling) // childScale = childScale * (childTrans.getBasis() * scaling); childScale = childScale * scaling / m_localScaling; m_children[i].m_childShape->setLocalScaling(childScale); - childTrans.setOrigin((childTrans.getOrigin())*scaling); + childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling); updateChildTransform(i, childTrans,false); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp index 5e83087b320..2d83c8bfbac 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp @@ -62,6 +62,10 @@ void btConeShape::setConeUpIndex(int upIndex) default: btAssert(0); }; + + m_implicitShapeDimensions[m_coneIndices[0]] = m_radius; + m_implicitShapeDimensions[m_coneIndices[1]] = m_height; + m_implicitShapeDimensions[m_coneIndices[2]] = m_radius; } btVector3 btConeShape::coneLocalSupport(const btVector3& v) const diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h index 5966ae48f11..4a0df0d558b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h @@ -90,6 +90,13 @@ public: } virtual void setLocalScaling(const btVector3& scaling); + + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + }; @@ -104,19 +111,61 @@ class btConeShapeX : public btConeShape return btVector3 (1,0,0); } + //debugging + virtual const char* getName()const + { + return "ConeX"; + } + + }; ///btConeShapeZ implements a Cone shape, around the Z axis class btConeShapeZ : public btConeShape { - public: - btConeShapeZ(btScalar radius,btScalar height); +public: + btConeShapeZ(btScalar radius,btScalar height); virtual btVector3 getAnisotropicRollingFrictionDirection() const { return btVector3 (0,0,1); } + //debugging + virtual const char* getName()const + { + return "ConeZ"; + } + + }; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btConeShapeData +{ + btConvexInternalShapeData m_convexInternalShapeData; + + int m_upIndex; + + char m_padding[4]; +}; + +SIMD_FORCE_INLINE int btConeShape::calculateSerializeBufferSize() const +{ + return sizeof(btConeShapeData); +} + +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btConeShapeData* shapeData = (btConeShapeData*) dataBuffer; + + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer); + + shapeData->m_upIndex = m_coneIndices[1]; + + return "btConeShapeData"; +} + #endif //BT_CONE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp index 9d40dfdd0dd..fb0d9fdb75a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -21,6 +21,7 @@ subject to the following restrictions: #include "btTriangleShape.h" #include "btSphereShape.h" #include "btCylinderShape.h" +#include "btConeShape.h" #include "btCapsuleShape.h" #include "btConvexHullShape.h" #include "btConvexPointCloudShape.h" @@ -336,6 +337,11 @@ btScalar btConvexShape::getMarginNonVirtual () const btCylinderShape* cylShape = (btCylinderShape*)this; return cylShape->getMarginNV(); } + case CONE_SHAPE_PROXYTYPE: + { + btConeShape* conShape = (btConeShape*)this; + return conShape->getMarginNV(); + } case CAPSULE_SHAPE_PROXYTYPE: { btCapsuleShape* capsuleShape = (btCapsuleShape*)this; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 8d4080a63a6..26322791d04 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -369,7 +369,7 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback getVertex(x+1,j+1,vertices[2]); callback->processTriangle(vertices,x,j); //second triangle - getVertex(x,j,vertices[0]); + // 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); @@ -382,7 +382,7 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback callback->processTriangle(vertices,x,j); //second triangle getVertex(x+1,j,vertices[0]); - getVertex(x,j+1,vertices[1]); + //getVertex(x,j+1,vertices[1]); getVertex(x+1,j+1,vertices[2]); callback->processTriangle(vertices,x,j); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp index 51a2f8a0739..5fbed334ec1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp @@ -111,10 +111,10 @@ int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicat return i/3; } } - } - m_3componentVertices.push_back((float)vertex.getX()); - m_3componentVertices.push_back((float)vertex.getY()); - m_3componentVertices.push_back((float)vertex.getZ()); + } + m_3componentVertices.push_back(vertex.getX()); + m_3componentVertices.push_back(vertex.getY()); + m_3componentVertices.push_back(vertex.getZ()); m_indexedMeshes[0].m_numVertices++; m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0]; return (m_3componentVertices.size()/3)-1; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h index 29d1b5cdaa4..0afc2321ff0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -27,7 +27,7 @@ subject to the following restrictions: class btTriangleMesh : public btTriangleIndexVertexArray { btAlignedObjectArray<btVector3> m_4componentVertices; - btAlignedObjectArray<float> m_3componentVertices; + btAlignedObjectArray<btScalar> m_3componentVertices; btAlignedObjectArray<unsigned int> m_32bitIndices; btAlignedObjectArray<unsigned short int> m_16bitIndices; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h index 72eb5aec461..29620abffb9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -17,7 +17,6 @@ subject to the following restrictions: #ifndef BT_CONVEX_PENETRATION_DEPTH_H #define BT_CONVEX_PENETRATION_DEPTH_H -class btStackAlloc; class btVector3; #include "btSimplexSolverInterface.h" class btConvexShape; @@ -33,8 +32,7 @@ public: const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc - ) = 0; + class btIDebugDraw* debugDraw) = 0; }; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index f958cc523ef..46ce1ab75e7 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -19,7 +19,6 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include "LinearMath/btVector3.h" -class btStackAlloc; /// This interface is made to be used by an iterative approach to do TimeOfImpact calculations /// This interface allows to query for closest points and penetration depth between two (convex) objects @@ -43,15 +42,13 @@ struct btDiscreteCollisionDetectorInterface struct ClosestPointInput { ClosestPointInput() - :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)), - m_stackAlloc(0) + :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)) { } btTransform m_transformA; btTransform m_transformB; btScalar m_maximumDistanceSquared; - btStackAlloc* m_stackAlloc; }; virtual ~btDiscreteCollisionDetectorInterface() {}; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index c6dc3f3a672..572ec36f563 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -25,7 +25,7 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btTransform& transformA, const btTransform& transformB, btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc ) + class btIDebugDraw* debugDraw) { (void)debugDraw; @@ -34,7 +34,7 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim // const btScalar radialmargin(btScalar(0.)); - btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin()); + btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin()); btGjkEpaSolver2::sResults results; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h index a49689a1501..1ed6340af3b 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -33,7 +33,7 @@ class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btTransform& transformA, const btTransform& transformB, btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ); + class btIDebugDraw* debugDraw); private : diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 8af16b9cf6f..8877579496b 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -50,7 +50,8 @@ m_marginA(objectA->getMargin()), m_marginB(objectB->getMargin()), m_ignoreMargin(false), m_lastUsedMethod(-1), -m_catchDegeneracies(1) +m_catchDegeneracies(1), +m_fixContactNormalDirection(1) { } btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) @@ -65,7 +66,8 @@ m_marginA(marginA), m_marginB(marginB), m_ignoreMargin(false), m_lastUsedMethod(-1), -m_catchDegeneracies(1) +m_catchDegeneracies(1), +m_fixContactNormalDirection(1) { } @@ -353,7 +355,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu m_minkowskiA,m_minkowskiB, localTransA,localTransB, m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, - debugDraw,input.m_stackAlloc + debugDraw ); @@ -438,6 +440,27 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu } #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/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index f0043b8b9f2..feeae686213 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -52,7 +52,7 @@ public: int m_curIter; int m_degenerateSimplex; int m_catchDegeneracies; - + int m_fixContactNormalDirection; btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index fe31f08d61a..fa45f49037e 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -26,11 +26,10 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc + class btIDebugDraw* debugDraw ) { - (void)stackAlloc; (void)v; bool check2d= convexA->isConvex2d() && convexB->isConvex2d(); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h index 6a8fe52f36f..fd533b4fc33 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h @@ -32,7 +32,7 @@ public: const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc + class btIDebugDraw* debugDraw ); }; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index f012889a70e..3999d400503 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -35,7 +35,7 @@ public: 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 kF_Terminator = 0xFFFFFFFF }; unsigned int m_flags; diff --git a/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h b/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h index c81813c92be..dffb06dfe94 100644 --- a/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h +++ b/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h @@ -31,7 +31,7 @@ public: virtual void setWalkDirection(const btVector3& walkDirection) = 0; virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0; - virtual void reset () = 0; + virtual void reset ( btCollisionWorld* collisionWorld ) = 0; virtual void warp (const btVector3& origin) = 0; virtual void preStep ( btCollisionWorld* collisionWorld) = 0; @@ -40,6 +40,7 @@ public: virtual void jump () = 0; virtual bool onGround () const = 0; + virtual void setUpInterpolate (bool value) = 0; }; #endif //BT_CHARACTER_CONTROLLER_INTERFACE_H diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp index 9db909a5469..8f1cd20bf45 100644 --- a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp +++ b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -14,6 +14,7 @@ subject to the following restrictions: */ +#include <stdio.h> #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/CollisionDispatch/btGhostObject.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" @@ -79,7 +80,7 @@ public: if (!convexResult.m_hitCollisionObject->hasContactResponse()) return btScalar(1.0); - + btVector3 hitNormalWorld; if (normalInWorldSpace) { @@ -149,7 +150,11 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho m_jumpSpeed = 10.0; // ? m_wasOnGround = false; m_wasJumping = false; + m_interpolateUp = true; setMaxSlope(btRadians(45.0)); + m_currentStepOffset = 0; + full_drop = false; + bounce_fix = false; } btKinematicCharacterController::~btKinematicCharacterController () @@ -190,9 +195,10 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* m_manifoldArray.resize(0); btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i]; + btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject); - btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject); - + btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject); + if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse())) continue; @@ -268,7 +274,10 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world) { // we moved up only a fraction of the step height m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction; - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + if (m_interpolateUp == true) + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + else + m_currentPosition = m_targetPosition; } m_verticalVelocity = 0.0; m_verticalOffset = 0.0; @@ -333,7 +342,8 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co { if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) { - updateTargetPositionBasedOnCollision (m_touchingNormal); + //interferes with step movement + //updateTargetPositionBasedOnCollision (m_touchingNormal); } } @@ -405,7 +415,8 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt) { - btTransform start, end; + btTransform start, end, end_double; + bool runonce = false; // phase 3: down /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0; @@ -414,44 +425,124 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; m_targetPosition -= (step_drop + gravity_drop);*/ + btVector3 orig_position = m_targetPosition; + btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; - if(downVelocity > 0.0 && downVelocity < m_stepHeight + + if(downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) - { - downVelocity = m_stepHeight; - } + downVelocity = m_fallSpeed; btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; - start.setIdentity (); - end.setIdentity (); + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - start.setOrigin (m_currentPosition); - end.setOrigin (m_targetPosition); + btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); - callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - - if (m_useGhostObjectSweepTest) + while (1) { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } else - { - collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + start.setIdentity (); + end.setIdentity (); + + end_double.setIdentity (); + + start.setOrigin (m_currentPosition); + end.setOrigin (m_targetPosition); + + //set double test for 2x the step drop, to check for a large drop vs small drop + end_double.setOrigin (m_targetPosition - step_drop); + + if (m_useGhostObjectSweepTest) + { + m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + + if (!callback.hasHit()) + { + //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial) + m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } + } else + { + collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + + if (!callback.hasHit()) + { + //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) + collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } + } + + btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; + bool has_hit = false; + if (bounce_fix == true) + has_hit = callback.hasHit() || callback2.hasHit(); + else + has_hit = callback2.hasHit(); + + if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false + && (m_wasOnGround || !m_wasJumping)) + { + //redo the velocity calculation when falling a small amount, for fast stairs motion + //for larger falls, use the smoother/slower interpolated movement by not touching the target position + + m_targetPosition = orig_position; + downVelocity = m_stepHeight; + + btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + m_targetPosition -= step_drop; + runonce = true; + continue; //re-run previous tests + } + break; } - if (callback.hasHit()) + if (callback.hasHit() || runonce == true) { // we dropped a fraction of the height -> hit floor - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + + btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2; + + //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY()); + + if (bounce_fix == true) + { + if (full_drop == true) + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + else + //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction); + } + else + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + + full_drop = false; + m_verticalVelocity = 0.0; m_verticalOffset = 0.0; m_wasJumping = false; } else { // we dropped the full height + full_drop = true; + + if (bounce_fix == true) + { + downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; + if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) + { + m_targetPosition += step_drop; //undo previous target change + downVelocity = m_fallSpeed; + step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + m_targetPosition -= step_drop; + } + } + //printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY()); + m_currentPosition = m_targetPosition; } } @@ -484,13 +575,24 @@ btScalar timeInterval m_useWalkDirection = false; m_walkDirection = velocity; m_normalizedDirection = getNormalizedVector(m_walkDirection); - m_velocityTimeInterval = timeInterval; + m_velocityTimeInterval += timeInterval; } - - -void btKinematicCharacterController::reset () -{ +void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld ) +{ + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + m_wasOnGround = false; + m_wasJumping = false; + m_walkDirection.setValue(0,0,0); + m_velocityTimeInterval = 0.0; + + //clear pair cache + btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache(); + while (cache->getOverlappingPairArray().size() > 0) + { + cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher()); + } } void btKinematicCharacterController::warp (const btVector3& origin) @@ -661,3 +763,8 @@ btVector3* btKinematicCharacterController::getUpAxisDirections() void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer) { } + +void btKinematicCharacterController::setUpInterpolate(bool value) +{ + m_interpolateUp = value; +} diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h index 8ec63735cd8..add6f30a687 100644 --- a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h +++ b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h @@ -81,6 +81,9 @@ protected: int m_upAxis; static btVector3* getUpAxisDirections(); + bool m_interpolateUp; + bool full_drop; + bool bounce_fix; btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal); btVector3 parallelComponent (const btVector3& direction, const btVector3& normal); @@ -133,7 +136,7 @@ public: virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval); - void reset (); + void reset ( btCollisionWorld* collisionWorld ); void warp (const btVector3& origin); void preStep ( btCollisionWorld* collisionWorld); @@ -161,6 +164,7 @@ public: } bool onGround () const; + void setUpInterpolate (bool value); }; #endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index 09c048beda8..1735b524dba 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -40,6 +40,15 @@ and swing 1 and 2 are along the z and y axes respectively. #include "btJacobianEntry.h" #include "btTypedConstraint.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define btConeTwistConstraintData2 btConeTwistConstraintDoubleData +#define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData" +#else +#define btConeTwistConstraintData2 btConeTwistConstraintData +#define btConeTwistConstraintDataName "btConeTwistConstraintData" +#endif //BT_USE_DOUBLE_PRECISION + + class btRigidBody; enum btConeTwistFlags @@ -295,7 +304,30 @@ public: }; -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 + + +struct btConeTwistConstraintDoubleData +{ + btTypedConstraintDoubleData m_typeConstraintData; + btTransformDoubleData m_rbAFrame; + btTransformDoubleData m_rbBFrame; + + //limits + double m_swingSpan1; + double m_swingSpan2; + double m_twistSpan; + double m_limitSoftness; + double m_biasFactor; + double m_relaxationFactor; + + double m_damping; + + + +}; + +#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION +///this structure is not used, except for loading pre-2.82 .bullet files struct btConeTwistConstraintData { btTypedConstraintData m_typeConstraintData; @@ -315,12 +347,12 @@ struct btConeTwistConstraintData char m_pad[4]; }; - - +#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION +// SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const { - return sizeof(btConeTwistConstraintData); + return sizeof(btConeTwistConstraintData2); } @@ -328,21 +360,21 @@ SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() cons ///fills the dataBuffer and returns the struct name (and 0 on failure) SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { - btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer; + btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*) dataBuffer; btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer); - m_rbAFrame.serializeFloat(cone->m_rbAFrame); - m_rbBFrame.serializeFloat(cone->m_rbBFrame); + m_rbAFrame.serialize(cone->m_rbAFrame); + m_rbBFrame.serialize(cone->m_rbBFrame); - cone->m_swingSpan1 = float(m_swingSpan1); - cone->m_swingSpan2 = float(m_swingSpan2); - cone->m_twistSpan = float(m_twistSpan); - cone->m_limitSoftness = float(m_limitSoftness); - cone->m_biasFactor = float(m_biasFactor); - cone->m_relaxationFactor = float(m_relaxationFactor); - cone->m_damping = float(m_damping); - - return "btConeTwistConstraintData"; + cone->m_swingSpan1 = m_swingSpan1; + cone->m_swingSpan2 = m_swingSpan2; + cone->m_twistSpan = m_twistSpan; + cone->m_limitSoftness = m_limitSoftness; + cone->m_biasFactor = m_biasFactor; + cone->m_relaxationFactor = m_relaxationFactor; + cone->m_damping = m_damping; + + return btConeTwistConstraintDataName; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index 6f673102be2..1ba1cd1e839 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -28,6 +28,14 @@ class btIDebugDraw; class btStackAlloc; class btDispatcher; /// btConstraintSolver provides solver interface + + +enum btConstraintSolverType +{ + BT_SEQUENTIAL_IMPULSE_SOLVER=1, + BT_MLCP_SOLVER=2 +}; + class btConstraintSolver { @@ -38,12 +46,16 @@ public: virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;} ///solve a group of constraints - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0; + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer,btDispatcher* dispatcher) = 0; - virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */, btStackAlloc* /* stackAlloc */) {;} + virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */) {;} ///clear internal cached data and reset random seed virtual void reset() = 0; + + virtual btConstraintSolverType getSolverType() const=0; + + }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp new file mode 100644 index 00000000000..f93a3280f35 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp @@ -0,0 +1,129 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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 "btFixedConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" +#include <new> + + +btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB) +:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB) +{ + m_pivotInA = frameInA.getOrigin(); + m_pivotInB = frameInB.getOrigin(); + m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse(); + +} + +btFixedConstraint::~btFixedConstraint () +{ +} + + +void btFixedConstraint::getInfo1 (btConstraintInfo1* info) +{ + info->m_numConstraintRows = 6; + info->nub = 6; +} + +void btFixedConstraint::getInfo2 (btConstraintInfo2* info) +{ + //fix the 3 linear degrees of freedom + + + 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_pivotInA; + { + 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_pivotInB; + + { + // btVector3 a2n = -a2; + btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + // set right hand side 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]); + } + + //fix the 3 angular degrees of freedom + + int start_row = 3; + int s = info->rowskip; + int start_index = start_row * s; + + // 3 rows to make body rotations equal + info->m_J1angularAxis[start_index] = 1; + info->m_J1angularAxis[start_index + s + 1] = 1; + info->m_J1angularAxis[start_index + s*2+2] = 1; + if ( info->m_J2angularAxis) + { + info->m_J2angularAxis[start_index] = -1; + info->m_J2angularAxis[start_index + s+1] = -1; + info->m_J2angularAxis[start_index + s*2+2] = -1; + } + + // set right hand side for the angular dofs + + btVector3 diff; + btScalar angle; + btMatrix3x3 mrelCur = worldOrnA *worldOrnB.inverse(); + btQuaternion qrelCur; + mrelCur.getRotation(qrelCur); + btTransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle); + diff*=-angle; + for (j=0; j<3; j++) + { + info->m_constraintError[(3+j)*info->rowskip] = k * diff[j]; + } + +}
\ No newline at end of file diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h new file mode 100644 index 00000000000..697e319e2ce --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h @@ -0,0 +1,49 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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_FIXED_CONSTRAINT_H +#define BT_FIXED_CONSTRAINT_H + +#include "btTypedConstraint.h" + +ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint +{ + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btQuaternion m_relTargetAB; + +public: + btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB); + + virtual ~btFixedConstraint(); + + + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void setParam(int num, btScalar value, int axis = -1) + { + btAssert(0); + } + virtual btScalar getParam(int num, int axis = -1) const + { + btAssert(0); + return 0.f; + } + +}; + +#endif //BT_FIXED_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h index 60f60094843..f9afcb91211 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h @@ -19,6 +19,18 @@ subject to the following restrictions: #define BT_GEAR_CONSTRAINT_H #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" + + +#ifdef BT_USE_DOUBLE_PRECISION +#define btGearConstraintData btGearConstraintDoubleData +#define btGearConstraintDataName "btGearConstraintDoubleData" +#else +#define btGearConstraintData btGearConstraintFloatData +#define btGearConstraintDataName "btGearConstraintFloatData" +#endif //BT_USE_DOUBLE_PRECISION + + + ///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio. ///See Bullet/Demos/ConstraintDemo for an example use. class btGearConstraint : public btTypedConstraint @@ -39,18 +51,102 @@ public: ///internal method used by the constraint solver, don't use them directly virtual void getInfo2 (btConstraintInfo2* info); + void setAxisA(btVector3& axisA) + { + m_axisInA = axisA; + } + void setAxisB(btVector3& axisB) + { + m_axisInB = axisB; + } + void setRatio(btScalar ratio) + { + m_ratio = ratio; + } + const btVector3& getAxisA() const + { + return m_axisInA; + } + const btVector3& getAxisB() const + { + return m_axisInB; + } + btScalar getRatio() const + { + return m_ratio; + } + + virtual void setParam(int num, btScalar value, int axis = -1) { + (void) num; + (void) value; + (void) axis; btAssert(0); - }; + } ///return the local value of parameter virtual btScalar getParam(int num, int axis = -1) const { + (void) num; + (void) axis; btAssert(0); return 0.f; } + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; +}; + + + + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btGearConstraintFloatData +{ + btTypedConstraintFloatData m_typeConstraintData; + + btVector3FloatData m_axisInA; + btVector3FloatData m_axisInB; + + float m_ratio; + char m_padding[4]; }; +struct btGearConstraintDoubleData +{ + btTypedConstraintDoubleData m_typeConstraintData; + + btVector3DoubleData m_axisInA; + btVector3DoubleData m_axisInB; + + double m_ratio; +}; + +SIMD_FORCE_INLINE int btGearConstraint::calculateSerializeBufferSize() const +{ + return sizeof(btGearConstraintData); +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btGearConstraintData* gear = (btGearConstraintData*)dataBuffer; + btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer); + + m_axisInA.serialize( gear->m_axisInA ); + m_axisInB.serialize( gear->m_axisInB ); + + gear->m_ratio = m_ratio; + + return btGearConstraintDataName; +} + + + + + + #endif //BT_GEAR_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 0409f95379b..431a524169e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -35,6 +35,14 @@ class btRigidBody; +#ifdef BT_USE_DOUBLE_PRECISION +#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2 +#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2" +#else +#define btGeneric6DofConstraintData2 btGeneric6DofConstraintData +#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData" +#endif //BT_USE_DOUBLE_PRECISION + //! Rotation Limit structure for generic joints class btRotationalLimitMotor @@ -561,7 +569,7 @@ public: }; -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 + struct btGeneric6DofConstraintData { btTypedConstraintData m_typeConstraintData; @@ -578,35 +586,51 @@ struct btGeneric6DofConstraintData int m_useOffsetForConstraintFrame; }; +struct btGeneric6DofConstraintDoubleData2 +{ + btTypedConstraintDoubleData m_typeConstraintData; + btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTransformDoubleData m_rbBFrame; + + btVector3DoubleData m_linearUpperLimit; + btVector3DoubleData m_linearLowerLimit; + + btVector3DoubleData m_angularUpperLimit; + btVector3DoubleData m_angularLowerLimit; + + int m_useLinearReferenceFrameA; + int m_useOffsetForConstraintFrame; +}; + SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const { - return sizeof(btGeneric6DofConstraintData); + return sizeof(btGeneric6DofConstraintData2); } ///fills the dataBuffer and returns the struct name (and 0 on failure) SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { - btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer; + btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer; btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); - m_frameInA.serializeFloat(dof->m_rbAFrame); - m_frameInB.serializeFloat(dof->m_rbBFrame); + 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] = float(m_angularLimits[i].m_loLimit); - dof->m_angularUpperLimit.m_floats[i] = float(m_angularLimits[i].m_hiLimit); - dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]); - dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]); + dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit; + dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit; + dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i]; + dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i]; } dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0; dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0; - return "btGeneric6DofConstraintData"; + return btGeneric6DofConstraintDataName; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h index 6fabb30369b..1b2e0f62ca8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h @@ -21,6 +21,15 @@ subject to the following restrictions: #include "btTypedConstraint.h" #include "btGeneric6DofConstraint.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintDoubleData2 +#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintDoubleData2" +#else +#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintData +#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintData" +#endif //BT_USE_DOUBLE_PRECISION + + /// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF @@ -65,7 +74,6 @@ public: }; -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btGeneric6DofSpringConstraintData { btGeneric6DofConstraintData m_6dofData; @@ -76,26 +84,37 @@ struct btGeneric6DofSpringConstraintData float m_springDamping[6]; }; +struct btGeneric6DofSpringConstraintDoubleData2 +{ + btGeneric6DofConstraintDoubleData2 m_6dofData; + + int m_springEnabled[6]; + double m_equilibriumPoint[6]; + double m_springStiffness[6]; + double m_springDamping[6]; +}; + + SIMD_FORCE_INLINE int btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const { - return sizeof(btGeneric6DofSpringConstraintData); + return sizeof(btGeneric6DofSpringConstraintData2); } ///fills the dataBuffer and returns the struct name (and 0 on failure) SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { - btGeneric6DofSpringConstraintData* dof = (btGeneric6DofSpringConstraintData*)dataBuffer; + btGeneric6DofSpringConstraintData2* dof = (btGeneric6DofSpringConstraintData2*)dataBuffer; btGeneric6DofConstraint::serialize(&dof->m_6dofData,serializer); int i; for (i=0;i<6;i++) { - dof->m_equilibriumPoint[i] = (float)m_equilibriumPoint[i]; - dof->m_springDamping[i] = (float)m_springDamping[i]; + dof->m_equilibriumPoint[i] = m_equilibriumPoint[i]; + dof->m_springDamping[i] = m_springDamping[i]; dof->m_springEnabled[i] = m_springEnabled[i]? 1 : 0; - dof->m_springStiffness[i] = (float)m_springStiffness[i]; + dof->m_springStiffness[i] = m_springStiffness[i]; } - return "btGeneric6DofSpringConstraintData"; + return btGeneric6DofSpringConstraintDataName; } #endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index a7f2cca5500..7c33ac24e05 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -28,8 +28,8 @@ subject to the following restrictions: class btRigidBody; #ifdef BT_USE_DOUBLE_PRECISION -#define btHingeConstraintData btHingeConstraintDoubleData -#define btHingeConstraintDataName "btHingeConstraintDoubleData" +#define btHingeConstraintData btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version +#define btHingeConstraintDataName "btHingeConstraintDoubleData2" #else #define btHingeConstraintData btHingeConstraintFloatData #define btHingeConstraintDataName "btHingeConstraintFloatData" @@ -302,7 +302,10 @@ public: }; -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 + +//only for backward compatibility +#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION +///this structure is not used, except for loading pre-2.82 .bullet files struct btHingeConstraintDoubleData { btTypedConstraintData m_typeConstraintData; @@ -321,7 +324,9 @@ struct btHingeConstraintDoubleData float m_relaxationFactor; }; -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION + + struct btHingeConstraintFloatData { btTypedConstraintData m_typeConstraintData; @@ -344,6 +349,30 @@ struct btHingeConstraintFloatData +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btHingeConstraintDoubleData2 +{ + btTypedConstraintDoubleData m_typeConstraintData; + btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTransformDoubleData m_rbBFrame; + int m_useReferenceFrameA; + int m_angularOnly; + int m_enableAngularMotor; + double m_motorTargetVelocity; + double m_maxMotorImpulse; + + double m_lowerLimit; + double m_upperLimit; + double m_limitSoftness; + double m_biasFactor; + double m_relaxationFactor; + char m_padding1[4]; + +}; + + + + SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const { return sizeof(btHingeConstraintData); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index 1e13416dfeb..91218949498 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -24,10 +24,10 @@ class btRigidBody; #ifdef BT_USE_DOUBLE_PRECISION -#define btPoint2PointConstraintData btPoint2PointConstraintDoubleData -#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData" +#define btPoint2PointConstraintData2 btPoint2PointConstraintDoubleData2 +#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData2" #else -#define btPoint2PointConstraintData btPoint2PointConstraintFloatData +#define btPoint2PointConstraintData2 btPoint2PointConstraintFloatData #define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData" #endif //BT_USE_DOUBLE_PRECISION @@ -134,24 +134,36 @@ struct btPoint2PointConstraintFloatData }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btPoint2PointConstraintDoubleData2 +{ + btTypedConstraintDoubleData m_typeConstraintData; + btVector3DoubleData m_pivotInA; + btVector3DoubleData m_pivotInB; +}; + +#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +///this structure is not used, except for loading pre-2.82 .bullet files +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btPoint2PointConstraintDoubleData { btTypedConstraintData m_typeConstraintData; btVector3DoubleData m_pivotInA; btVector3DoubleData m_pivotInB; }; +#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const { - return sizeof(btPoint2PointConstraintData); + return sizeof(btPoint2PointConstraintData2); } ///fills the dataBuffer and returns the struct name (and 0 on failure) SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { - btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer; + btPoint2PointConstraintData2* p2pData = (btPoint2PointConstraintData2*)dataBuffer; btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer); m_pivotInA.serialize(p2pData->m_pivotInA); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 5f9437b7add..be93e35434c 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -152,7 +152,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( #endif } -// Project Gauss Seidel or the equivalent Sequential Impulse +// Projected Gauss Seidel or the equivalent Sequential Impulse void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; @@ -280,7 +280,7 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n) -void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) +void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep) { btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; @@ -299,6 +299,9 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod solverBody->m_linearFactor = rb->getLinearFactor(); solverBody->m_linearVelocity = rb->getLinearVelocity(); 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(); @@ -308,6 +311,8 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod solverBody->m_linearFactor.setValue(1,1,1); solverBody->m_linearVelocity.setValue(0,0,0); solverBody->m_angularVelocity.setValue(0,0,0); + solverBody->m_externalForceImpulse.setValue(0,0,0); + solverBody->m_externalTorqueImpulse.setValue(0,0,0); } @@ -326,8 +331,7 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, -static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode); -static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode) +void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode) { @@ -351,8 +355,6 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr { - solverConstraint.m_contactNormal1 = normalAxis; - solverConstraint.m_contactNormal2 = -normalAxis; btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; @@ -368,15 +370,30 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; + if (body0) { + solverConstraint.m_contactNormal1 = normalAxis; btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1); solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor(); + }else + { + solverConstraint.m_contactNormal1.setZero(); + solverConstraint.m_relpos1CrossNormal.setZero(); + solverConstraint.m_angularComponentA .setZero(); } + + if (body1) { + solverConstraint.m_contactNormal2 = -normalAxis; btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2); solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor(); + } else + { + solverConstraint.m_contactNormal2.setZero(); + solverConstraint.m_relpos2CrossNormal.setZero(); + solverConstraint.m_angularComponentB.setZero(); } { @@ -401,9 +418,9 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity: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: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; @@ -414,8 +431,8 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_cfm = cfmSlip; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; + solverConstraint.m_lowerLimit = -solverConstraint.m_friction; + solverConstraint.m_upperLimit = solverConstraint.m_friction; } } @@ -481,9 +498,9 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity: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: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; @@ -494,8 +511,8 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_cfm = cfmSlip; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; + solverConstraint.m_lowerLimit = -solverConstraint.m_friction; + solverConstraint.m_upperLimit = solverConstraint.m_friction; } } @@ -517,7 +534,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConst } -int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body) +int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep) { int solverBodyIdA = -1; @@ -535,11 +552,19 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& { solverBodyIdA = m_tmpSolverBodyPool.size(); btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,&body); + initSolverBody(&solverBody,&body,timeStep); body.setCompanionId(solverBodyIdA); } else { - return 0;//assume first one is a fixed solver body + + if (m_fixedBodyId<0) + { + m_fixedBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&fixedBody,0,timeStep); + } + return m_fixedBodyId; +// return 0;//assume first one is a fixed solver body } } @@ -552,8 +577,8 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btVector3& vel, btScalar& rel_vel, btScalar& relaxation, - btVector3& rel_pos1, btVector3& rel_pos2) + btScalar& relaxation, + const btVector3& rel_pos1, const btVector3& rel_pos2) { const btVector3& pos1 = cp.getPositionWorldOnA(); @@ -567,8 +592,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); - rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); relaxation = 1.f; @@ -601,10 +626,24 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra solverConstraint.m_jacDiagABInv = denom; } - solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB; - solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB; - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + if (rb0) + { + solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + } else + { + solverConstraint.m_contactNormal1.setZero(); + solverConstraint.m_relpos1CrossNormal.setZero(); + } + if (rb1) + { + solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB; + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + }else + { + solverConstraint.m_contactNormal2.setZero(); + solverConstraint.m_relpos2CrossNormal.setZero(); + } btScalar restitution = 0.f; btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; @@ -616,8 +655,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - vel = vel1 - vel2; - rel_vel = cp.m_normalWorldOnB.dot(vel); + btVector3 vel = vel1 - vel2; + btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); @@ -648,10 +687,17 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra solverConstraint.m_appliedPushImpulse = 0.f; { - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rb0?bodyA->m_linearVelocity:btVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:btVector3(0,0,0)); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rb1?bodyB->m_linearVelocity:btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:btVector3(0,0,0)); + + btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0); + 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) + + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA); + 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; @@ -680,7 +726,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv; solverConstraint.m_rhsPenetration = 0.f; } else @@ -754,8 +800,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); - int solverBodyIdA = getOrInitSolverBody(*colObj0); - int solverBodyIdB = getOrInitSolverBody(*colObj1); + int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); + int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); // btRigidBody* bodyA = btRigidBody::upcast(colObj0); // btRigidBody* bodyB = btRigidBody::upcast(colObj1); @@ -780,19 +826,35 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m btVector3 rel_pos1; btVector3 rel_pos2; btScalar relaxation; - btScalar rel_vel; - btVector3 vel; + int frictionIndex = m_tmpSolverContactConstraintPool.size(); btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); -// btRigidBody* rb0 = btRigidBody::upcast(colObj0); -// btRigidBody* rb1 = btRigidBody::upcast(colObj1); + btRigidBody* rb0 = btRigidBody::upcast(colObj0); + btRigidBody* rb1 = btRigidBody::upcast(colObj1); solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_originalContactPoint = &cp; - setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); + rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); + + btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); + btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); + + 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(); @@ -801,9 +863,11 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); - btVector3 angVelA,angVelB; - solverBodyA->getAngularVelocity(angVelA); - solverBodyB->getAngularVelocity(angVelB); + btVector3 angVelA(0,0,0),angVelB(0,0,0); + if (rb0) + angVelA = rb0->getAngularVelocity(); + if (rb1) + angVelB = rb1->getAngularVelocity(); btVector3 relAngVel = angVelB-angVelA; if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) @@ -857,6 +921,10 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) { cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); @@ -864,17 +932,16 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } else { 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); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); @@ -882,9 +949,6 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) { @@ -899,8 +963,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); - setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); } + setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); @@ -909,10 +973,24 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m } } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) +{ + int i; + btPersistentManifold* manifold = 0; +// btCollisionObject* colObj0=0,*colObj1=0; + + + for (i=0;i<numManifolds;i++) + { + manifold = manifoldPtr[i]; + convertContact(manifold,infoGlobal); + } +} + +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { + m_fixedBodyId = -1; BT_PROFILE("solveGroupCacheFriendlySetup"); - (void)stackAlloc; (void)debugDrawer; m_maxOverrideNumSolverIterations = 0; @@ -996,14 +1074,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol m_tmpSolverBodyPool.reserve(numBodies+1); m_tmpSolverBodyPool.resize(0); - btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&fixedBody,0); + //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + //initSolverBody(&fixedBody,0); //convert all bodies for (int i=0;i<numBodies;i++) { - int bodyId = getOrInitSolverBody(*bodies[i]); + int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep); + btRigidBody* body = btRigidBody::upcast(bodies[i]); if (body && body->getInvMass()) { @@ -1012,9 +1091,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE) { gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce); + solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; } - solverBody.m_linearVelocity += body->getTotalForce()*body->getInvMass()*infoGlobal.m_timeStep; - solverBody.m_angularVelocity += (body->getTotalTorque()-gyroForce)*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; } } @@ -1084,8 +1162,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btRigidBody& rbA = constraint->getRigidBodyA(); btRigidBody& rbB = constraint->getRigidBodyB(); - int solverBodyIdA = getOrInitSolverBody(rbA); - int solverBodyIdB = getOrInitSolverBody(rbB); + int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep); + int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep); btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; @@ -1182,15 +1260,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } - ///fix rhs - ///todo: add force/torque accelerators + { btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()); + btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0); + btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0); + + 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) + + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA); + + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) + + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB); rel_vel = vel1Dotn+vel2Dotn; - btScalar restitution = 0.f; btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 btScalar velocityError = restitution - rel_vel * info2.m_damping; @@ -1199,6 +1284,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_appliedImpulse = 0.f; + } } } @@ -1206,18 +1292,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } - { - int i; - btPersistentManifold* manifold = 0; -// btCollisionObject* colObj0=0,*colObj1=0; - + convertContacts(manifoldPtr,numManifolds,infoGlobal); - for (i=0;i<numManifolds;i++) - { - manifold = manifoldPtr[i]; - convertContact(manifold,infoGlobal); - } - } } // btContactSolverInfo info = infoGlobal; @@ -1256,7 +1332,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } -btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) +btScalar btSequentialImpulseConstraintSolver::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(); @@ -1309,14 +1385,14 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration { for (int j=0;j<numConstraints;j++) { - if (constraints[j]->isEnabled()) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } + 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 @@ -1376,7 +1452,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration 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); + //resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); } @@ -1395,7 +1472,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration 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); + //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); } } @@ -1437,14 +1515,14 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration { for (int j=0;j<numConstraints;j++) { - if (constraints[j]->isEnabled()) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } + 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(); @@ -1492,7 +1570,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration } -void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { int iteration; if (infoGlobal.m_splitImpulse) @@ -1532,20 +1610,20 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte } } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { BT_PROFILE("solveGroupCacheFriendlyIterations"); { ///this is a special step to resolve penetrations (just for contacts) - solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); + solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; 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,stackAlloc); + solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); } } @@ -1610,9 +1688,15 @@ 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); + + m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity( + m_tmpSolverBodyPool[i].m_angularVelocity+ + m_tmpSolverBodyPool[i].m_externalTorqueImpulse); - m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity); - m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity); if (infoGlobal.m_splitImpulse) m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform); @@ -1632,15 +1716,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo /// btSequentialImpulseConstraintSolver Sequentially applies impulses -btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) +btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/) { BT_PROFILE("solveGroup"); //you need to provide at least some bodies - solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); + solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer); - solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); + solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer); solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 2eea6be0db2..180d2a385d3 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -18,7 +18,6 @@ subject to the following restrictions: class btIDebugDraw; class btPersistentManifold; -class btStackAlloc; class btDispatcher; class btCollisionObject; #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" @@ -43,7 +42,7 @@ protected: btAlignedObjectArray<int> m_orderFrictionConstraintPool; btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool; int m_maxOverrideNumSolverIterations; - + int m_fixedBodyId; 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, @@ -57,10 +56,11 @@ protected: btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); btSolverConstraint& addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f); - + void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, - const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation, - btVector3& rel_pos1, btVector3& rel_pos2); + const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2); + + static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode); void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); @@ -71,6 +71,8 @@ protected: btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); + virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); + void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); @@ -83,8 +85,8 @@ protected: const btSolverConstraint& contactConstraint); //internal method - int getOrInitSolverBody(btCollisionObject& body); - void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); + int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep); + void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep); void resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); @@ -97,12 +99,12 @@ protected: protected: - virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); - btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + virtual btScalar 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,btStackAlloc* stackAlloc); - virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); public: @@ -112,7 +114,7 @@ public: btSequentialImpulseConstraintSolver(); virtual ~btSequentialImpulseConstraintSolver(); - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher); + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); @@ -132,6 +134,11 @@ public: return m_btSeed2; } + + virtual btConstraintSolverType getSolverType() const + { + return BT_SEQUENTIAL_IMPULSE_SOLVER; + } }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index ca8e715bc47..57ebb47d8e7 100755 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -25,7 +25,13 @@ TODO: #ifndef BT_SLIDER_CONSTRAINT_H #define BT_SLIDER_CONSTRAINT_H - +#ifdef BT_USE_DOUBLE_PRECISION +#define btSliderConstraintData2 btSliderConstraintDoubleData +#define btSliderConstraintDataName "btSliderConstraintDoubleData" +#else +#define btSliderConstraintData2 btSliderConstraintData +#define btSliderConstraintDataName "btSliderConstraintData" +#endif //BT_USE_DOUBLE_PRECISION #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" @@ -283,7 +289,10 @@ public: }; + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 + + struct btSliderConstraintData { btTypedConstraintData m_typeConstraintData; @@ -302,31 +311,48 @@ struct btSliderConstraintData }; +struct btSliderConstraintDoubleData +{ + btTypedConstraintDoubleData m_typeConstraintData; + btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTransformDoubleData m_rbBFrame; + + double m_linearUpperLimit; + double m_linearLowerLimit; + + double m_angularUpperLimit; + double m_angularLowerLimit; + + int m_useLinearReferenceFrameA; + int m_useOffsetForConstraintFrame; + +}; + SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const { - return sizeof(btSliderConstraintData); + return sizeof(btSliderConstraintData2); } ///fills the dataBuffer and returns the struct name (and 0 on failure) SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { - btSliderConstraintData* sliderData = (btSliderConstraintData*) dataBuffer; + btSliderConstraintData2* sliderData = (btSliderConstraintData2*) dataBuffer; btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer); - m_frameInA.serializeFloat(sliderData->m_rbAFrame); - m_frameInB.serializeFloat(sliderData->m_rbBFrame); + m_frameInA.serialize(sliderData->m_rbAFrame); + m_frameInB.serialize(sliderData->m_rbBFrame); - sliderData->m_linearUpperLimit = float(m_upperLinLimit); - sliderData->m_linearLowerLimit = float(m_lowerLinLimit); + sliderData->m_linearUpperLimit = m_upperLinLimit; + sliderData->m_linearLowerLimit = m_lowerLinLimit; - sliderData->m_angularUpperLimit = float(m_upperAngLimit); - sliderData->m_angularLowerLimit = float(m_lowerAngLimit); + sliderData->m_angularUpperLimit = m_upperAngLimit; + sliderData->m_angularLowerLimit = m_lowerAngLimit; sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA; sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame; - return "btSliderConstraintData"; + return btSliderConstraintDataName; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h index ccc45996c8b..27ccefe4169 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -118,6 +118,8 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody btVector3 m_turnVelocity; btVector3 m_linearVelocity; btVector3 m_angularVelocity; + btVector3 m_externalForceImpulse; + btVector3 m_externalTorqueImpulse; btRigidBody* m_originalBody; void setWorldTransform(const btTransform& worldTransform) @@ -130,6 +132,17 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody return m_worldTransform; } + + + SIMD_FORCE_INLINE void getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity ) const + { + if (m_originalBody) + velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity+m_externalTorqueImpulse).cross(rel_pos); + else + velocity.setValue(0,0,0); + } + + SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const { if (m_originalBody) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h index 2ade61b2f69..5515e6b311c 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -55,6 +55,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint { void* m_originalContactPoint; btScalar m_unusedPadding4; + int m_numRowsForNonContactConstraint; }; int m_overrideNumSolverIterations; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 465c0746c58..27fdd9d3df8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -109,7 +109,7 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal ///fills the dataBuffer and returns the struct name (and 0 on failure) const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { - btTypedConstraintData* tcd = (btTypedConstraintData*) dataBuffer; + btTypedConstraintData2* tcd = (btTypedConstraintData2*) dataBuffer; tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA); tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB); @@ -123,14 +123,14 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali tcd->m_objectType = m_objectType; tcd->m_needsFeedback = m_needsFeedback; tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations; - tcd->m_breakingImpulseThreshold = float(m_breakingImpulseThreshold); + tcd->m_breakingImpulseThreshold = m_breakingImpulseThreshold; tcd->m_isEnabled = m_isEnabled? 1: 0; tcd->m_userConstraintId =m_userConstraintId; tcd->m_userConstraintType =m_userConstraintType; - tcd->m_appliedImpulse = float(m_appliedImpulse); - tcd->m_dbgDrawSize = float(m_dbgDrawSize ); + tcd->m_appliedImpulse = m_appliedImpulse; + tcd->m_dbgDrawSize = m_dbgDrawSize; tcd->m_disableCollisionsBetweenLinkedBodies = false; @@ -142,7 +142,7 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali if (m_rbB.getConstraintRef(i) == this) tcd->m_disableCollisionsBetweenLinkedBodies = true; - return "btTypedConstraintData"; + return btTypedConstraintDataName; } btRigidBody& btTypedConstraint::getFixedBody() diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index 441fa375050..b58f984d0fb 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -21,6 +21,15 @@ subject to the following restrictions: #include "btSolverConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define btTypedConstraintData2 btTypedConstraintDoubleData +#define btTypedConstraintDataName "btTypedConstraintDoubleData" +#else +#define btTypedConstraintData2 btTypedConstraintFloatData +#define btTypedConstraintDataName "btTypedConstraintFloatData" +#endif //BT_USE_DOUBLE_PRECISION + + class btSerializer; //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility @@ -34,6 +43,7 @@ enum btTypedConstraintType CONTACT_CONSTRAINT_TYPE, D6_SPRING_CONSTRAINT_TYPE, GEAR_CONSTRAINT_TYPE, + FIXED_CONSTRAINT_TYPE, MAX_CONSTRAINT_TYPE }; @@ -356,6 +366,33 @@ SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScal } ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btTypedConstraintFloatData +{ + btRigidBodyFloatData *m_rbA; + btRigidBodyFloatData *m_rbB; + char *m_name; + + int m_objectType; + int m_userConstraintType; + int m_userConstraintId; + int m_needsFeedback; + + float m_appliedImpulse; + float m_dbgDrawSize; + + int m_disableCollisionsBetweenLinkedBodies; + int m_overrideNumSolverIterations; + + float m_breakingImpulseThreshold; + int m_isEnabled; + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 + +#define BT_BACKWARDS_COMPATIBLE_SERIALIZATION +#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION +///this structure is not used, except for loading pre-2.82 .bullet files struct btTypedConstraintData { btRigidBodyData *m_rbA; @@ -377,10 +414,35 @@ struct btTypedConstraintData int m_isEnabled; }; +#endif //BACKWARDS_COMPATIBLE + +struct btTypedConstraintDoubleData +{ + btRigidBodyDoubleData *m_rbA; + btRigidBodyDoubleData *m_rbB; + char *m_name; + + int m_objectType; + int m_userConstraintType; + int m_userConstraintId; + int m_needsFeedback; + + double m_appliedImpulse; + double m_dbgDrawSize; + + int m_disableCollisionsBetweenLinkedBodies; + int m_overrideNumSolverIterations; + + double m_breakingImpulseThreshold; + int m_isEnabled; + char padding[4]; + +}; + SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const { - return sizeof(btTypedConstraintData); + return sizeof(btTypedConstraintData2); } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 9ff2d9f1173..fb8a4068e23 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -87,7 +87,6 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal btTypedConstraint** m_sortedConstraints; int m_numConstraints; btIDebugDraw* m_debugDrawer; - btStackAlloc* m_stackAlloc; btDispatcher* m_dispatcher; btAlignedObjectArray<btCollisionObject*> m_bodies; @@ -104,7 +103,6 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal m_sortedConstraints(NULL), m_numConstraints(0), m_debugDrawer(NULL), - m_stackAlloc(stackAlloc), m_dispatcher(dispatcher) { @@ -135,7 +133,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal if (islandId<0) { ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); } else { //also add all non-contact constraints/joints for this island @@ -163,7 +161,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal if (m_solverInfo->m_minimumSolverBatchSize<=1) { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); } else { @@ -190,7 +188,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal 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_stackAlloc,m_dispatcher); + 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); m_constraints.resize(0); @@ -210,7 +208,9 @@ m_gravity(0,-10,0), m_localTime(0), m_synchronizeAllMotionStates(false), m_applySpeculativeContactRestitution(false), -m_profileTimings(0) +m_profileTimings(0), +m_fixedTimeStep(0), +m_latencyMotionStateInterpolation(true) { if (!m_constraintSolver) @@ -232,7 +232,7 @@ m_profileTimings(0) { void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback),16); - m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, m_stackAlloc, dispatcher); + m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, 0, dispatcher); } } @@ -359,7 +359,9 @@ void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) { btTransform interpolatedTransform; btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform); + body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(), + (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(), + interpolatedTransform); body->getMotionState()->setWorldTransform(interpolatedTransform); } } @@ -403,6 +405,7 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, if (maxSubSteps) { //fixed timestep with interpolation + m_fixedTimeStep = fixedTimeStep; m_localTime += timeStep; if (m_localTime >= fixedTimeStep) { @@ -413,7 +416,8 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, { //variable timestep fixedTimeStep = timeStep; - m_localTime = timeStep; + m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep; + m_fixedTimeStep = 0; if (btFuzzyZero(timeStep)) { numSimulationSubSteps = 0; @@ -724,7 +728,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) m_solverIslandCallback->processConstraints(); - m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc); + m_constraintSolver->allSolved(solverInfo, m_debugDrawer); } @@ -746,12 +750,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) { - if (colObj0->isActive() || colObj1->isActive()) - { - - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), - (colObj1)->getIslandTag()); - } + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); } } } @@ -770,12 +769,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) { - if (colObj0->isActive() || colObj1->isActive()) - { - - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), - (colObj1)->getIslandTag()); - } + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); } } } @@ -1131,7 +1125,6 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { //don't integrate/update velocities here, it happens in the constraint solver - //damping body->applyDamping(timeStep); body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index fa934c49d2b..d8a34b7da3d 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -53,6 +53,7 @@ protected: //for variable timesteps btScalar m_localTime; + btScalar m_fixedTimeStep; //for variable timesteps bool m_ownsIslandManager; @@ -64,6 +65,8 @@ protected: int m_profileTimings; + bool m_latencyMotionStateInterpolation; + btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds; virtual void predictUnconstraintMotion(btScalar timeStep); @@ -74,7 +77,7 @@ protected: virtual void solveConstraints(btContactSolverInfo& solverInfo); - void updateActivationState(btScalar timeStep); + virtual void updateActivationState(btScalar timeStep); void updateActions(btScalar timeStep); @@ -216,6 +219,16 @@ public: ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo) virtual void serialize(btSerializer* serializer); + ///Interpolate motion state between previous and current transform, instead of current and next transform. + ///This can relieve discontinuities in the rendering, due to penetrations + void setLatencyMotionStateInterpolation(bool latencyInterpolation ) + { + m_latencyMotionStateInterpolation = latencyInterpolation; + } + bool getLatencyMotionStateInterpolation() const + { + return m_latencyMotionStateInterpolation; + } }; #endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 7d5c621f850..35dd1400fe7 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -33,7 +33,8 @@ enum btDynamicsWorldType BT_SIMPLE_DYNAMICS_WORLD=1, BT_DISCRETE_DYNAMICS_WORLD=2, BT_CONTINUOUS_DYNAMICS_WORLD=3, - BT_SOFT_RIGID_DYNAMICS_WORLD=4 + BT_SOFT_RIGID_DYNAMICS_WORLD=4, + BT_GPU_DYNAMICS_WORLD=5 }; ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index f0e07f942af..ed90fb44115 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -363,11 +363,13 @@ public: inline void setLinearVelocity(const btVector3& lin_vel) { + m_updateRevision++; m_linearVelocity = lin_vel; } inline void setAngularVelocity(const btVector3& ang_vel) { + m_updateRevision++; m_angularVelocity = ang_vel; } @@ -484,11 +486,13 @@ public: void setAngularFactor(const btVector3& angFac) { + m_updateRevision++; m_angularFactor = angFac; } void setAngularFactor(btScalar angFac) { + m_updateRevision++; m_angularFactor.setValue(angFac,angFac,angFac); } const btVector3& getAngularFactor() const diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 5fc2f3cf8f7..35dd38840f6 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -78,8 +78,8 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b btContactSolverInfo infoGlobal; infoGlobal.m_timeStep = timeStep; m_constraintSolver->prepareSolve(0,numManifolds); - m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1); - m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc); + m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_dispatcher1); + m_constraintSolver->allSolved(infoGlobal,m_debugDrawer); } ///integrate transforms diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp new file mode 100644 index 00000000000..56a1c55d9ae --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -0,0 +1,1009 @@ +/* + * PURPOSE: + * Class representing an articulated rigid body. Stores the body's + * current state, allows forces and torques to be set, handles + * timestepping and implements Featherstone's algorithm. + * + * 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) + + 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 "btMultiBody.h" +#include "btMultiBodyLink.h" +#include "btMultiBodyLinkCollider.h" + +// #define INCLUDE_GYRO_TERM + +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 + const btVector3 &top_in, // top part of input vector + const btVector3 &bottom_in, // bottom part of input vector + btVector3 &top_out, // top part of output vector + btVector3 &bottom_out) // bottom part of output vector + { + top_out = rotation_matrix * top_in; + bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in; + } + + void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix, + const btVector3 &displacement, + const btVector3 &top_in, + const btVector3 &bottom_in, + btVector3 &top_out, + btVector3 &bottom_out) + { + top_out = rotation_matrix.transpose() * top_in; + bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in)); + } + + btScalar SpatialDotProduct(const btVector3 &a_top, + const btVector3 &a_bottom, + const btVector3 &b_top, + const btVector3 &b_bottom) + { + return a_bottom.dot(b_top) + a_top.dot(b_bottom); + } +} + + +// +// Implementation of class btMultiBody +// + +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), + + fixed_base(fixed_base_), + awake(true), + can_sleep(can_sleep_), + sleep_timer(0), + m_baseCollider(0), + m_linearDamping(0.04f), + m_angularDamping(0.04f), + m_useGyroTerm(true), + m_maxAppliedImpulse(1000.f), + m_hasSelfCollision(true) +{ + links.resize(n_links); + + 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); +} + +btMultiBody::~btMultiBody() +{ +} + +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, + 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; + + links[i].updateCache(); +} + +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, + 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; + if (disableParentCollision) + links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + links[i].updateCache(); +} + + + + + +int btMultiBody::getParent(int i) const +{ + return links[i].parent; +} + +btScalar btMultiBody::getLinkMass(int i) const +{ + return links[i].mass; +} + +const btVector3 & btMultiBody::getLinkInertia(int i) const +{ + return links[i].inertia; +} + +btScalar btMultiBody::getJointPos(int i) const +{ + return links[i].joint_pos; +} + +btScalar btMultiBody::getJointVel(int i) const +{ + return m_real_buf[6 + i]; +} + +void btMultiBody::setJointPos(int i, btScalar q) +{ + links[i].joint_pos = q; + links[i].updateCache(); +} + +void btMultiBody::setJointVel(int i, btScalar qdot) +{ + m_real_buf[6 + i] = qdot; +} + +const btVector3 & btMultiBody::getRVector(int i) const +{ + return links[i].cached_r_vector; +} + +const btQuaternion & btMultiBody::getParentToLocalRot(int i) const +{ + return links[i].cached_rot_parent_to_this; +} + +btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const +{ + btVector3 result = local_pos; + while (i != -1) { + // 'result' is in frame i. transform it to frame parent(i) + result += getRVector(i); + result = quatRotate(getParentToLocalRot(i).inverse(),result); + i = getParent(i); + } + + // 'result' is now in the base frame. transform it to world frame + result = quatRotate(getWorldToBaseRot().inverse() ,result); + result += getBasePos(); + + return result; +} + +btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const +{ + if (i == -1) { + // world to base + return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos())); + } else { + // find position in parent frame, then transform to current frame + return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i); + } +} + +btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const +{ + btVector3 result = local_dir; + while (i != -1) { + result = quatRotate(getParentToLocalRot(i).inverse() , result); + i = getParent(i); + } + result = quatRotate(getWorldToBaseRot().inverse() , result); + return result; +} + +btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const +{ + if (i == -1) { + return quatRotate(getWorldToBaseRot(), world_dir); + } else { + return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir)); + } +} + +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()); + + for (int i = 0; i < num_links; ++i) { + const int parent = links[i].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, + 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; + } +} + +btScalar btMultiBody::getKineticEnergy() const +{ + int num_links = getNumLinks(); + // TODO: would be better not to allocate memory here + btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1); + btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1); + 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]); + + 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]); + } + + return 0.5f * result; +} + +btVector3 btMultiBody::getAngularMomentum() const +{ + int num_links = getNumLinks(); + // TODO: would be better not to allocate memory here + btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1); + btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1); + 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])); + + 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]))); + } + + return result; +} + + +void btMultiBody::clearForcesAndTorques() +{ + base_force.setValue(0, 0, 0); + base_torque.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; + } +} + +void btMultiBody::clearVelocities() +{ + for (int i = 0; i < 6 + getNumLinks(); ++i) + { + m_real_buf[i] = 0.f; + } +} +void btMultiBody::addLinkForce(int i, const btVector3 &f) +{ + links[i].applied_force += f; +} + +void btMultiBody::addLinkTorque(int i, const btVector3 &t) +{ + links[i].applied_torque += t; +} + +void btMultiBody::addJointTorque(int i, btScalar Q) +{ + links[i].joint_torque += Q; +} + +const btVector3 & btMultiBody::getLinkForce(int i) const +{ + return links[i].applied_force; +} + +const btVector3 & btMultiBody::getLinkTorque(int i) const +{ + return links[i].applied_torque; +} + +btScalar btMultiBody::getJointTorque(int i) const +{ + return links[i].joint_torque; +} + + +inline btMatrix3x3 vecMulVecTranspose(const btVector3& v0, const btVector3& v1Transposed) +{ + btVector3 row0 = btVector3( + v0.x() * v1Transposed.x(), + v0.x() * v1Transposed.y(), + v0.x() * v1Transposed.z()); + btVector3 row1 = btVector3( + v0.y() * v1Transposed.x(), + v0.y() * v1Transposed.y(), + v0.y() * v1Transposed.z()); + btVector3 row2 = btVector3( + v0.z() * v1Transposed.x(), + v0.z() * v1Transposed.y(), + v0.z() * v1Transposed.z()); + + btMatrix3x3 m(row0[0],row0[1],row0[2], + row1[0],row1[1],row1[2], + row2[0],row2[1],row2[2]); + return m; +} + + +void btMultiBody::stepVelocities(btScalar dt, + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v, + btAlignedObjectArray<btMatrix3x3> &scratch_m) +{ + // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) + // and the base linear & angular accelerations. + + // We apply damping forces in this routine as well as any external forces specified by the + // caller (via addBaseForce etc). + + // output should point to an array of 6 + num_links reals. + // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), + // num_links joint acceleration values. + + int num_links = getNumLinks(); + + const btScalar DAMPING_K1_LINEAR = m_linearDamping; + const btScalar DAMPING_K2_LINEAR = m_linearDamping; + + const btScalar DAMPING_K1_ANGULAR = m_angularDamping; + const btScalar DAMPING_K2_ANGULAR= m_angularDamping; + + btVector3 base_vel = getBaseVel(); + btVector3 base_omega = getBaseOmega(); + + // 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_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 + 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]; + + // Cached 3x3 rotation matrices from parent frame to this frame. + btMatrix3x3 * rot_from_parent = &matrix_buf[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; + + // 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); + + 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); + + if (m_useGyroTerm) + zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( base_inertia * vel_top_angular[0] ); + + zero_acc_bottom_linear[0] += base_inertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm()); + + } + + + + 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]); + + rot_from_world[0] = rot_from_parent[0]; + + 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); + + + 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; + + // 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]; + + zero_acc_bottom_linear[i+1] = + - (rot_from_world[i+1] * links[i].applied_torque); + if (m_useGyroTerm) + { + zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( links[i].inertia * vel_top_angular[i+1] ); + } + + 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()); + + // 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]); + } + + + // '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]); + + const int parent = links[i].parent; + + + // Ip += pXi * (Ii - hi hi' / Di) * iXp + const btScalar one_over_di = 1.0f / D[i]; + + + + + 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]); + + + 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); + + 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; + } + + + // Second 'upward' loop + // (part of TreeForwardDynamics in Mirtich) + + if (fixed_base) + { + accel_top[0] = accel_bottom[0] = btVector3(0,0,0); + } + 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]; + } + + } + + // 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]; + 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]; + output[3] = vdot_out[0]; + output[4] = vdot_out[1]; + output[5] = vdot_out[2]; + // Final step: add the accelerations (times dt) to the velocities. + applyDeltaVee(output, dt); + + +} + + + +void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const +{ + int num_links = getNumLinks(); + ///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; + } 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 invI_upper_left = (tmp * Binv); + btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); + tmp = cached_inertia_top_left * 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_top; + btVector3 tmp; + tmp = invIupper_right * rhs_bot; + vtop += tmp; + btVector3 vbot = invI_lower_left*rhs_top; + tmp = invI_lower_right * rhs_bot; + vbot += tmp; + result[0] = vtop[0]; + result[1] = vtop[1]; + result[2] = vtop[2]; + result[3] = vbot[0]; + result[4] = vbot[1]; + result[5] = vbot[2]; + } + + } +} + + +void btMultiBody::calcAccelerationDeltas(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]; + 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; + + // rot_from_parent (cached from calcAccelerations) + const btMatrix3x3 * rot_from_parent = &matrix_buf[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; + + 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 + // -- set to force/torque on the base, zero otherwise + 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] * input_force); + zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque); + } + for (int i = 0; i < num_links; ++i) + { + zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0); + } + + // 'Downward' loop. + for (int i = num_links - 1; i >= 0; --i) + { + + 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; + } + + // ptr to the joint accel part of the output + btScalar * joint_accel = output + 6; + + // Second 'upward' loop + if (fixed_base) + { + accel_top[0] = accel_bottom[0] = btVector3(0,0,0); + } else + { + 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]; + } + + } + + // 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; + } + + // transform base accelerations back to the world frame. + btVector3 omegadot_out; + omegadot_out = rot_from_parent[0].transpose() * accel_top[0]; + 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]; + + output[3] = vdot_out[0]; + output[4] = vdot_out[1]; + output[5] = vdot_out[2]; +} + +void btMultiBody::stepPositions(btScalar dt) +{ + 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) + { + 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 + { + base_quat = base_quat * btQuaternion(base_omega / omega_norm,-omega_times_dt); + } + + // Make sure the quaternion represents a valid rotation. + // (Not strictly necessary, but helps prevent any round-off errors from building up.) + base_quat.normalize(); + + // 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(); + } +} + +void btMultiBody::fillContactJacobian(int link, + const btVector3 &contact_point, + const btVector3 &normal, + btScalar *jac, + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v, + btAlignedObjectArray<btMatrix3x3> &scratch_m) const +{ + // temporary space + int num_links = getNumLinks(); + scratch_v.resize(2*num_links + 2); + 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; + btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); + + scratch_r.resize(num_links); + btScalar * results = num_links > 0 ? &scratch_r[0] : 0; + + btMatrix3x3 * rot_from_world = &scratch_m[0]; + + const btVector3 p_minus_com_world = contact_point - base_pos; + + 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; + + // 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]; + // then v coefficients + jac[3] = normal[0]; + jac[4] = normal[1]; + jac[5] = normal[2]; + + // Set remaining jac values to zero for now. + for (int i = 6; i < 6 + num_links; ++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. + // (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...) + + // calculate required normals & positions in the local frames. + 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); + 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 ); + } + } + + // Now copy through to output. + while (link != -1) { + jac[6 + link] = results[link]; + link = links[link].parent; + } + } +} + +void btMultiBody::wakeUp() +{ + awake = true; +} + +void btMultiBody::goToSleep() +{ + awake = false; +} + +void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) +{ + int num_links = getNumLinks(); + extern bool gDisableDeactivation; + if (!can_sleep || gDisableDeactivation) + { + awake = true; + sleep_timer = 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]; + } + + if (motion < SLEEP_EPSILON) { + sleep_timer += timestep; + if (sleep_timer > SLEEP_TIMEOUT) { + goToSleep(); + } + } else { + sleep_timer = 0; + if (!awake) + wakeUp(); + } +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h new file mode 100644 index 00000000000..7177bebbff5 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h @@ -0,0 +1,466 @@ +/* + * PURPOSE: + * Class representing an articulated rigid body. Stores the body's + * current state, allows forces and torques to be set, handles + * timestepping and implements Featherstone's algorithm. + * + * 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) + + 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_H +#define BT_MULTIBODY_H + +#include "LinearMath/btScalar.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btAlignedObjectArray.h" + + +#include "btMultiBodyLink.h" +class btMultiBodyLinkCollider; + +class btMultiBody +{ +public: + + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + // + // 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(); + + 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 + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &zero_rot_parent_to_this, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &joint_axis, // in my frame + const btVector3 &parent_axis_position, // vector from parent COM to joint axis, in PARENT frame + const btVector3 &my_axis_position, // vector from joint axis to my COM, in MY frame + bool disableParentCollision=false); + + const btMultibodyLink& getLink(int index) const + { + return links[index]; + } + + btMultibodyLink& getLink(int index) + { + return links[index]; + } + + + void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base + { + m_baseCollider = collider; + } + const btMultiBodyLinkCollider* getBaseCollider() const + { + return m_baseCollider; + } + btMultiBodyLinkCollider* getBaseCollider() + { + return m_baseCollider; + } + + // + // get parent + // input: link num from 0 to num_links-1 + // output: link num from 0 to num_links-1, OR -1 to mean the base. + // + int getParent(int link_num) const; + + + // + // get number of links, masses, moments of inertia + // + + int getNumLinks() const { return links.size(); } + btScalar getBaseMass() const { return base_mass; } + const btVector3 & getBaseInertia() const { return base_inertia; } + 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; } + + + // + // get/set pos/vel/rot/omega for the base link + // + + const btVector3 & getBasePos() const { return base_pos; } // in world frame + const btVector3 getBaseVel() const + { + return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]); + } // in world frame + const btQuaternion & getWorldToBaseRot() const + { + return base_quat; + } // 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 + + void setBasePos(const btVector3 &pos) + { + base_pos = pos; + } + void setBaseVel(const btVector3 &vel) + { + + m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2]; + } + void setWorldToBaseRot(const btQuaternion &rot) + { + base_quat = rot; + } + void setBaseOmega(const btVector3 &omega) + { + m_real_buf[0]=omega[0]; + m_real_buf[1]=omega[1]; + m_real_buf[2]=omega[2]; + } + + + // + // get/set pos/vel for child links (i = 0 to num_links-1) + // + + btScalar getJointPos(int i) const; + btScalar getJointVel(int i) const; + + void setJointPos(int i, btScalar q); + void setJointVel(int i, btScalar qdot); + + // + // direct access to velocities as a vector of 6 + num_links elements. + // (omega first, then v, then joint velocities.) + // + const btScalar * getVelocityVector() const + { + return &m_real_buf[0]; + } +/* btScalar * getVelocityVector() + { + return &real_buf[0]; + } + */ + + // + // get the frames of reference (positions and orientations) of the child links + // (i = 0 to num_links-1) + // + + const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords + const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + + + // + // transform vectors in local frame of link i to world frame (or vice versa) + // + btVector3 localPosToWorld(int i, const btVector3 &vec) const; + btVector3 localDirToWorld(int i, const btVector3 &vec) const; + btVector3 worldPosToLocal(int i, const btVector3 &vec) const; + btVector3 worldDirToLocal(int i, const btVector3 &vec) const; + + + // + // calculate kinetic energy and angular momentum + // useful for debugging. + // + + btScalar getKineticEnergy() const; + btVector3 getAngularMomentum() const; + + + // + // set external forces and torques. Note all external forces/torques are given in the WORLD frame. + // + + void clearForcesAndTorques(); + void clearVelocities(); + + void addBaseForce(const btVector3 &f) + { + base_force += f; + } + void addBaseTorque(const btVector3 &t) { base_torque += 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; } + const btVector3 & getLinkForce(int i) const; + const btVector3 & getLinkTorque(int i) const; + btScalar getJointTorque(int i) const; + + + // + // dynamics routines. + // + + // timestep the velocities (given the external forces/torques set using addBaseForce etc). + // also sets up caches for calcAccelerationDeltas. + // + // Note: the caller must provide three vectors which are used as + // temporary scratch space. The idea here is to reduce dynamic + // memory allocation: the same scratch vectors can be re-used + // again and again for different Multibodies, instead of each + // btMultiBody allocating (and then deallocating) their own + // individual scratch buffers. This gives a considerable speed + // improvement, at least on Windows (where dynamic memory + // allocation appears to be fairly slow). + // + void stepVelocities(btScalar dt, + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v, + btAlignedObjectArray<btMatrix3x3> &scratch_m); + + // calcAccelerationDeltas + // 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, + 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) + { + + for (int i = 0; i < 6 + getNumLinks(); ++i) + { + m_real_buf[i] += delta_vee[i]; + } + + } + void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) + { + 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; + } + + for (int i = 0; i < 6 + getNumLinks(); ++i) + { + sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; + m_real_buf[i] += delta_vee[i] * multiplier; + } + } + + // timestep the positions (given current velocities). + void stepPositions(btScalar dt); + + + // + // contacts + // + + // 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, + const btVector3 &contact_point, + const btVector3 &normal, + btScalar *jac, + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v, + btAlignedObjectArray<btMatrix3x3> &scratch_m) const; + + + // + // sleeping + // + void setCanSleep(bool canSleep) + { + can_sleep = canSleep; + } + + bool isAwake() const { return awake; } + void wakeUp(); + void goToSleep(); + void checkMotionAndSleepIfRequired(btScalar timestep); + + bool hasFixedBase() const + { + return fixed_base; + } + + int getCompanionId() const + { + return m_companionId; + } + void setCompanionId(int id) + { + //printf("for %p setCompanionId(%d)\n",this, id); + m_companionId = id; + } + + void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links + { + links.resize(numLinks); + } + + btScalar getLinearDamping() const + { + return m_linearDamping; + } + void setLinearDamping( btScalar damp) + { + m_linearDamping = damp; + } + btScalar getAngularDamping() const + { + return m_angularDamping; + } + + bool getUseGyroTerm() const + { + return m_useGyroTerm; + } + void setUseGyroTerm(bool useGyro) + { + m_useGyroTerm = useGyro; + } + btScalar getMaxAppliedImpulse() const + { + return m_maxAppliedImpulse; + } + void setMaxAppliedImpulse(btScalar maxImp) + { + m_maxAppliedImpulse = maxImp; + } + + void setHasSelfCollision(bool hasSelfCollision) + { + m_hasSelfCollision = hasSelfCollision; + } + bool hasSelfCollision() const + { + return m_hasSelfCollision; + } + +private: + btMultiBody(const btMultiBody &); // not implemented + void operator=(const btMultiBody &); // not implemented + + void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; + + void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const; + + +private: + + btMultiBodyLinkCollider* m_baseCollider;//can be NULL + + btVector3 base_pos; // position of COM of base (world frame) + btQuaternion base_quat; // rotates world points into base frame + + btScalar base_mass; // mass of the base + btVector3 base_inertia; // 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. + btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders; + + // + // real_buf: + // offset size array + // 0 6 + num_links v (base_omega; base_vel; joint_vels) + // 6+num_links num_links D + // + // vector_buf: + // offset size array + // 0 num_links h_top + // num_links num_links h_bottom + // + // matrix_buf: + // offset size array + // 0 num_links+1 rot_from_parent + // + + btAlignedObjectArray<btScalar> m_real_buf; + btAlignedObjectArray<btVector3> vector_buf; + btAlignedObjectArray<btMatrix3x3> matrix_buf; + + //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; + + bool fixed_base; + + // Sleep parameters. + bool awake; + bool can_sleep; + btScalar sleep_timer; + + int m_companionId; + btScalar m_linearDamping; + btScalar m_angularDamping; + bool m_useGyroTerm; + btScalar m_maxAppliedImpulse; + bool m_hasSelfCollision; +}; + +#endif diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp new file mode 100644 index 00000000000..44e04c3a132 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -0,0 +1,527 @@ +#include "btMultiBodyConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + +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_isUnilateral(isUnilateral), + 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) +{ + + + + constraintRow.m_multiBodyA = m_bodyA; + constraintRow.m_multiBodyB = m_bodyB; + + btMultiBody* multiBodyA = constraintRow.m_multiBodyA; + btMultiBody* multiBodyB = constraintRow.m_multiBodyB; + + if (multiBodyA) + { + + const int ndofA = multiBodyA->getNumLinks() + 6; + + constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (constraintRow.m_deltaVelAindex <0) + { + constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA); + } + + constraintRow.m_jacAindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofA); + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + for (int i=0;i<ndofA;i++) + data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i]; + + btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; + multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v); + } + + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + + constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (constraintRow.m_deltaVelBindex <0) + { + constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); + } + + constraintRow.m_jacBindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofB); + + for (int i=0;i<ndofB;i++) + data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i]; + + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v); + } + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA =0; + btScalar* lambdaB =0; + int ndofA = 0; + if (multiBodyA) + { + ndofA = multiBodyA->getNumLinks() + 6; + jacA = &data.m_jacobians[constraintRow.m_jacAindex]; + lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l =lambdaA[i]; + denom0 += j*l; + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + jacB = &data.m_jacobians[constraintRow.m_jacBindex]; + lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l =lambdaB[i]; + denom1 += j*l; + } + + } + + if (multiBodyA && (multiBodyA==multiBodyB)) + { + // ndof1 == ndof2 in this case + for (int i = 0; i < ndofA; ++i) + { + denom1 += jacB[i] * lambdaA[i]; + denom1 += jacA[i] * lambdaB[i]; + } + } + + btScalar d = denom0+denom1; + if (btFabs(d)>SIMD_EPSILON) + { + + constraintRow.m_jacDiagABInv = 1.f/(d); + } else + { + constraintRow.m_jacDiagABInv = 1.f; + } + + } + + + //compute rhs and remaining constraintRow fields + + + + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumLinks() + 6; + btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumLinks() + 6; + btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } + + constraintRow.m_friction = 0.f; + + constraintRow.m_appliedImpulse = 0.f; + constraintRow.m_appliedPushImpulse = 0.f; + + btScalar velocityError = desiredVelocity - rel_vel;// * damping; + + btScalar erp = infoGlobal.m_erp2; + + btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; + + if (!infoGlobal.m_splitImpulse) + { + //combine position and velocity into rhs + constraintRow.m_rhs = velocityImpulse; + constraintRow.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + constraintRow.m_rhs = velocityImpulse; + constraintRow.m_rhsPenetration = 0.f; + } + + + constraintRow.m_cfm = 0.f; + constraintRow.m_lowerLimit = lowerLimit; + constraintRow.m_upperLimit = upperLimit; + + } + return rel_vel; +} + + +void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +{ + for (int i = 0; i < ndof; ++i) + 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) +{ + + + 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; + + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = 1.f; + + if (multiBodyA) + { + const int ndofA = multiBodyA->getNumLinks() + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + 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()); + + 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); + btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + } else + { + btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = contactNormalOnB; + } + + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); + } + + solverConstraint.m_jacBindex = data.m_jacobians.size(); + + data.m_jacobians.resize(data.m_jacobians.size()+ndofB); + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + + 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_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; + int ndofA = 0; + if (multiBodyA) + { + ndofA = multiBodyA->getNumLinks() + 6; + jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l =lambdaA[i]; + denom0 += j*l; + } + } else + { + if (rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l =lambdaB[i]; + denom1 += j*l; + } + + } else + { + if (rb1) + { + 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; + } + + } + + + //compute rhs and remaining solverConstraint fields + + + + btScalar restitution = 0.f; + btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop; + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumLinks() + 6; + btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } else + { + if (rb0) + { + rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + } + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumLinks() + 6; + btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } else + { + if (rb1) + { + 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; + }; + } + + + ///warm starting (or zero if disabled) + /* + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); + } else + { + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + } + if (multiBodyB) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); + } else + { + if (rb1) + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); + } + } + } else + */ + { + solverConstraint.m_appliedImpulse = 0.f; + } + + solverConstraint.m_appliedPushImpulse = 0.f; + + { + + + btScalar positionalError = 0.f; + btScalar velocityError = restitution - rel_vel;// * damping; + + + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + + if (penetration>0) + { + positionalError = 0; + velocityError = -penetration / infoGlobal.m_timeStep; + + } else + { + positionalError = -penetration * erp/infoGlobal.m_timeStep; + } + + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = -m_maxAppliedImpulse; + solverConstraint.m_upperLimit = m_maxAppliedImpulse; + } + +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h new file mode 100644 index 00000000000..9fa317330b3 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -0,0 +1,166 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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_CONSTRAINT_H +#define BT_MULTIBODY_CONSTRAINT_H + +#include "LinearMath/btScalar.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "btMultiBody.h" + +class btMultiBody; +struct btSolverInfo; + +#include "btMultiBodySolverConstraint.h" + +struct btMultiBodyJacobianData +{ + btAlignedObjectArray<btScalar> m_jacobians; + btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse; + btAlignedObjectArray<btScalar> m_deltaVelocities; + btAlignedObjectArray<btScalar> scratch_r; + btAlignedObjectArray<btVector3> scratch_v; + btAlignedObjectArray<btMatrix3x3> scratch_m; + btAlignedObjectArray<btSolverBody>* m_solverBodyPool; + int m_fixedBodyId; + +}; + + +class btMultiBodyConstraint +{ +protected: + + btMultiBody* m_bodyA; + btMultiBody* m_bodyB; + int m_linkA; + int m_linkB; + + int m_num_rows; + int m_jac_size_A; + int m_jac_size_both; + int m_pos_offset; + + bool m_isUnilateral; + + btScalar m_maxAppliedImpulse; + + + // data block laid out as follows: + // cached impulses. (one per row.) + // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) + // positions. (one per row.) + btAlignedObjectArray<btScalar> m_data; + + 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); + +public: + + btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); + virtual ~btMultiBodyConstraint(); + + + + 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; + } + + btMultiBody* getMultiBodyA() + { + return m_bodyA; + } + btMultiBody* getMultiBodyB() + { + return m_bodyB; + } + + // 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]; + } + + void setPosition(int row, btScalar pos) + { + m_data[m_pos_offset + row] = pos; + } + + + bool isUnilateral() const + { + return m_isUnilateral; + } + + // 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]; + } + const btScalar* jacobianA(int row) const + { + return &m_data[m_num_rows + (row * m_jac_size_both)]; + } + btScalar* jacobianB(int row) + { + 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_num_rows + (row * m_jac_size_both) + m_jac_size_A]; + } + + btScalar getMaxAppliedImpulse() const + { + return m_maxAppliedImpulse; + } + void setMaxAppliedImpulse(btScalar maxImp) + { + m_maxAppliedImpulse = maxImp; + } + + +}; + +#endif //BT_MULTIBODY_CONSTRAINT_H + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp new file mode 100644 index 00000000000..577f846225b --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -0,0 +1,795 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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 "btMultiBodyConstraintSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "btMultiBodyLinkCollider.h" + +#include "BulletDynamics/ConstraintSolver/btSolverBody.h" +#include "btMultiBodyConstraint.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" + +#include "LinearMath/btQuickprof.h" + +btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + + //solve featherstone non-contact constraints + + //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); + for (int j=0;j<m_multiBodyNonContactConstraints.size();j++) + { + btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j]; + //if (iteration < constraint.m_overrideNumSolverIterations) + //resolveSingleConstraintRowGenericMultiBody(constraint); + resolveSingleConstraintRowGeneric(constraint); + } + + //solve featherstone normal contact + for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++) + { + btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j]; + if (iteration < infoGlobal.m_numIterations) + resolveSingleConstraintRowGeneric(constraint); + } + + //solve featherstone frictional contact + + for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++) + { + if (iteration < infoGlobal.m_numIterations) + { + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + //adjust friction limits here + if (totalImpulse>btScalar(0)) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + resolveSingleConstraintRowGeneric(frictionConstraint); + } + } + } + return val; +} + +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + m_multiBodyNonContactConstraints.resize(0); + m_multiBodyNormalContactConstraints.resize(0); + m_multiBodyFrictionContactConstraints.resize(0); + m_data.m_jacobians.resize(0); + m_data.m_deltaVelocitiesUnitImpulse.resize(0); + m_data.m_deltaVelocities.resize(0); + + for (int i=0;i<numBodies;i++) + { + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]); + if (fcA) + { + fcA->m_multiBody->setCompanionId(-1); + } + } + + btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); + + return val; +} + +void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +{ + for (int i = 0; i < ndof; ++i) + m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; +} + +void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) +{ + + btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; + btScalar deltaVelADotn=0; + btScalar deltaVelBDotn=0; + btSolverBody* bodyA = 0; + btSolverBody* bodyB = 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]; + } else + { + bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; + deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + } + + 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]; + } else + { + bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; + deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + } + + + 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); + } else + { + bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,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); + } else + { + bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + } + +} + + +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, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + + BT_PROFILE("setupMultiBodyContactConstraint"); + btVector3 rel_pos1; + btVector3 rel_pos2; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; + btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = 1.f; + + if (multiBodyA) + { + const int ndofA = multiBodyA->getNumLinks() + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + solverConstraint.m_jacAindex = m_data.m_jacobians.size(); + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + 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); + 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); + } 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; + } + + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); + } + + solverConstraint.m_jacBindex = m_data.m_jacobians.size(); + + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); + 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); + } 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; + } + + { + + 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 = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l =lambdaA[i]; + denom0 += j*l; + } + } else + { + if (rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + contactNormal.dot(vec); + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l =lambdaB[i]; + denom1 += j*l; + } + + } else + { + if (rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + contactNormal.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; + } + + } + + + //compute rhs and remaining solverConstraint fields + + + + btScalar restitution = 0.f; + btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop; + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumLinks() + 6; + btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } else + { + if (rb0) + { + rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + } + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumLinks() + 6; + btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } else + { + if (rb1) + { + rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + } + } + + solverConstraint.m_friction = cp.m_combinedFriction; + + + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + }; + } + + + ///warm starting (or zero if disabled) + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); + } else + { + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + } + if (multiBodyB) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); + } else + { + if (rb1) + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); + } + } + } else + { + solverConstraint.m_appliedImpulse = 0.f; + } + + solverConstraint.m_appliedPushImpulse = 0.f; + + { + + + btScalar positionalError = 0.f; + btScalar velocityError = restitution - rel_vel;// * damping; + + + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + + if (penetration>0) + { + positionalError = 0; + velocityError = -penetration / infoGlobal.m_timeStep; + + } else + { + positionalError = -penetration * erp/infoGlobal.m_timeStep; + } + + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } + +} + + + + +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +{ + BT_PROFILE("addMultiBodyFrictionConstraint"); + btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); + solverConstraint.m_frictionIndex = frictionIndex; + bool isFriction = true; + + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB? fcB->m_multiBody : 0; + + int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); + return solverConstraint; +} + +void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) +{ + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB? fcB->m_multiBody : 0; + + btCollisionObject* colObj0=0,*colObj1=0; + + colObj0 = (btCollisionObject*)manifold->getBody0(); + colObj1 = (btCollisionObject*)manifold->getBody1(); + + 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]; + + + ///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++) + { + + btManifoldPoint& cp = manifold->getContactPoint(j); + + if (cp.getDistance() <= manifold->getContactProcessingThreshold()) + { + + btScalar relaxation; + + int frictionIndex = m_multiBodyNormalContactConstraints.size(); + + btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); + + btRigidBody* rb0 = btRigidBody::upcast(colObj0); + btRigidBody* rb1 = btRigidBody::upcast(colObj1); + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + bool isFriction = false; + setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction); + +// const btVector3& pos1 = cp.getPositionWorldOnA(); +// const btVector3& pos2 = cp.getPositionWorldOnB(); + + /////setup the friction constraints +#define ENABLE_FRICTION +#ifdef ENABLE_FRICTION + solverConstraint.m_frictionIndex = frictionIndex; +#if ROLLING_FRICTION + btVector3 angVelA(0,0,0),angVelB(0,0,0); + if (rb0) + angVelA = rb0->getAngularVelocity(); + if (rb1) + angVelB = rb1->getAngularVelocity(); + btVector3 relAngVel = angVelB-angVelA; + + if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) + { + //only a single rollingFriction per manifold + rollingFriction--; + if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) + { + relAngVel.normalize(); + applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + if (relAngVel.length()>0.001) + addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } else + { + addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + btVector3 axis0,axis1; + btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); + applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + if (axis0.length()>0.001) + 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); + + } + } +#endif //ROLLING_FRICTION + + ///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 + ///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, + ///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 + /// + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) + {/* + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); + if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) + { + cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); + if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); + cp.m_lateralFrictionDir2.normalize();//?? + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } else + */ + { + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + 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; + } + } + + } else + { + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1); + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2); + + //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); + //todo: + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + } + + +#endif //ENABLE_FRICTION + + } + } +} + +void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) +{ + btPersistentManifold* manifold = 0; + + for (int i=0;i<numManifolds;i++) + { + btPersistentManifold* manifold= manifoldPtr[i]; + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + if (!fcA && !fcB) + { + //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case + convertContact(manifold,infoGlobal); + } else + { + convertMultiBodyContact(manifold,infoGlobal); + } + } + + //also convert the multibody constraints, if any + + + for (int i=0;i<m_tmpNumMultiBodyConstraints;i++) + { + btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i]; + m_data.m_solverBodyPool = &m_tmpSolverBodyPool; + m_data.m_fixedBodyId = m_fixedBodyId; + + c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal); + } + +} + + + +btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) +{ + return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); +} + + +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) +{ + //printf("solveMultiBodyGroup start\n"); + m_tmpMultiBodyConstraints = multiBodyConstraints; + m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; + + btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); + + m_tmpMultiBodyConstraints = 0; + m_tmpNumMultiBodyConstraints = 0; + + +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h new file mode 100644 index 00000000000..0f4cd69c029 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -0,0 +1,85 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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_CONSTRAINT_SOLVER_H +#define BT_MULTIBODY_CONSTRAINT_SOLVER_H + +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "btMultiBodySolverConstraint.h" + + +class btMultiBody; + +#include "btMultiBodyConstraint.h" + + + +ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver +{ + +protected: + + btMultiBodyConstraintArray m_multiBodyNonContactConstraints; + + btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; + btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; + + btMultiBodyJacobianData m_data; + + //temp storage for multi body constraints for a specific island/group called by 'solveGroup' + btMultiBodyConstraint** m_tmpMultiBodyConstraints; + 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); + + + void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, + btScalar* jacA,btScalar* jacB, + btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, + const btContactSolverInfo& infoGlobal); + + void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); + 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 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); + +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 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); +}; + + + + + +#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp new file mode 100644 index 00000000000..0910f8f6abb --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -0,0 +1,578 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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 "btMultiBodyDynamicsWorld.h" +#include "btMultiBodyConstraintSolver.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" +#include "LinearMath/btQuickprof.h" +#include "btMultiBodyConstraint.h" + + + + +void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask) +{ + m_multiBodies.push_back(body); + +} + +void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) +{ + m_multiBodies.remove(body); +} + +void btMultiBodyDynamicsWorld::calculateSimulationIslands() +{ + BT_PROFILE("calculateSimulationIslands"); + + getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); + + { + //merge islands based on speculative contact manifolds too + 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()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + } + } + } + + { + int i; + int numConstraints = int(m_constraints.size()); + for (i=0;i< numConstraints ; i++ ) + { + btTypedConstraint* constraint = m_constraints[i]; + if (constraint->isEnabled()) + { + const btRigidBody* colObj0 = &constraint->getRigidBodyA(); + const btRigidBody* colObj1 = &constraint->getRigidBodyB(); + + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + } + } + } + } + + //merge islands linked by Featherstone link colliders + for (int i=0;i<m_multiBodies.size();i++) + { + btMultiBody* body = m_multiBodies[i]; + { + btMultiBodyLinkCollider* prev = body->getBaseCollider(); + + for (int b=0;b<body->getNumLinks();b++) + { + btMultiBodyLinkCollider* cur = body->getLink(b).m_collider; + + if (((cur) && (!(cur)->isStaticOrKinematicObject())) && + ((prev) && (!(prev)->isStaticOrKinematicObject()))) + { + int tagPrev = prev->getIslandTag(); + int tagCur = cur->getIslandTag(); + getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur); + } + if (cur && !cur->isStaticOrKinematicObject()) + prev = cur; + + } + } + } + + //merge islands linked by multibody constraints + { + for (int i=0;i<this->m_multiBodyConstraints.size();i++) + { + btMultiBodyConstraint* c = m_multiBodyConstraints[i]; + int tagA = c->getIslandIdA(); + int tagB = c->getIslandIdB(); + if (tagA>=0 && tagB>=0) + getSimulationIslandManager()->getUnionFind().unite(tagA, tagB); + } + } + + //Store the island id in each body + getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); + +} + + +void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) +{ + BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState"); + + + + for ( int i=0;i<m_multiBodies.size();i++) + { + btMultiBody* body = m_multiBodies[i]; + if (body) + { + body->checkMotionAndSleepIfRequired(timeStep); + if (!body->isAwake()) + { + btMultiBodyLinkCollider* col = body->getBaseCollider(); + if (col && col->getActivationState() == ACTIVE_TAG) + { + col->setActivationState( WANTS_DEACTIVATION); + col->setDeactivationTime(0.f); + } + for (int b=0;b<body->getNumLinks();b++) + { + btMultiBodyLinkCollider* col = body->getLink(b).m_collider; + if (col && col->getActivationState() == ACTIVE_TAG) + { + col->setActivationState( WANTS_DEACTIVATION); + col->setDeactivationTime(0.f); + } + } + } else + { + btMultiBodyLinkCollider* col = body->getBaseCollider(); + if (col && col->getActivationState() != DISABLE_DEACTIVATION) + col->setActivationState( ACTIVE_TAG ); + + for (int b=0;b<body->getNumLinks();b++) + { + btMultiBodyLinkCollider* col = body->getLink(b).m_collider; + if (col && col->getActivationState() != DISABLE_DEACTIVATION) + col->setActivationState( ACTIVE_TAG ); + } + } + + } + } + + btDiscreteDynamicsWorld::updateActivationState(timeStep); +} + + +SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) +{ + int islandId; + + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); + return islandId; + +} + + +class btSortConstraintOnIslandPredicate2 +{ + public: + + bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetConstraintIslandId2(rhs); + lIslandId0 = btGetConstraintIslandId2(lhs); + return lIslandId0 < rIslandId0; + } +}; + + + +SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) +{ + int islandId; + + int islandTagA = lhs->getIslandIdA(); + int islandTagB = lhs->getIslandIdB(); + islandId= islandTagA>=0?islandTagA:islandTagB; + return islandId; + +} + + +class btSortMultiBodyConstraintOnIslandPredicate +{ + public: + + bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); + lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } +}; + +struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback +{ + btContactSolverInfo* m_solverInfo; + btMultiBodyConstraintSolver* m_solver; + btMultiBodyConstraint** m_multiBodySortedConstraints; + int m_numMultiBodyConstraints; + + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; + + btAlignedObjectArray<btCollisionObject*> m_bodies; + btAlignedObjectArray<btPersistentManifold*> m_manifolds; + btAlignedObjectArray<btTypedConstraint*> m_constraints; + btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; + + + MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver, + btDispatcher* dispatcher) + :m_solverInfo(NULL), + m_solver(solver), + m_multiBodySortedConstraints(NULL), + m_numConstraints(0), + m_debugDrawer(NULL), + m_dispatcher(dispatcher) + { + + } + + MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) + { + btAssert(0); + (void)other; + return *this; + } + + SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) + { + btAssert(solverInfo); + m_solverInfo = solverInfo; + + m_multiBodySortedConstraints = sortedMultiBodyConstraints; + m_numMultiBodyConstraints = numMultiBodyConstraints; + m_sortedConstraints = sortedConstraints; + m_numConstraints = numConstraints; + + m_debugDrawer = debugDrawer; + m_bodies.resize (0); + m_manifolds.resize (0); + m_constraints.resize (0); + m_multiBodyConstraints.resize(0); + } + + + virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) + { + if (islandId<0) + { + ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + } else + { + //also add all non-contact constraints/joints for this island + btTypedConstraint** startConstraint = 0; + btMultiBodyConstraint** startMultiBodyConstraint = 0; + + int numCurConstraints = 0; + int numCurMultiBodyConstraints = 0; + + int i; + + //find the first constraint for this island + + for (i=0;i<m_numConstraints;i++) + { + if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) + { + startConstraint = &m_sortedConstraints[i]; + break; + } + } + //count the number of constraints in this island + for (;i<m_numConstraints;i++) + { + if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) + { + numCurConstraints++; + } + } + + for (i=0;i<m_numMultiBodyConstraints;i++) + { + if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) + { + + startMultiBodyConstraint = &m_multiBodySortedConstraints[i]; + break; + } + } + //count the number of multi body constraints in this island + for (;i<m_numMultiBodyConstraints;i++) + { + if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) + { + numCurMultiBodyConstraints++; + } + } + + if (m_solverInfo->m_minimumSolverBatchSize<=1) + { + 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++) + m_manifolds.push_back(manifolds[i]); + for (i=0;i<numCurConstraints;i++) + m_constraints.push_back(startConstraint[i]); + + for (i=0;i<numCurMultiBodyConstraints;i++) + m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]); + + if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize) + { + processConstraints(); + } else + { + //printf("deferred\n"); + } + } + } + } + void processConstraints() + { + + 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; + + 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); + m_manifolds.resize(0); + m_constraints.resize(0); + m_multiBodyConstraints.resize(0); + } + +}; + + + +btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) + :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), + m_multiBodyConstraintSolver(constraintSolver) +{ + //split impulse is not yet supported for Featherstone hierarchies + getSolverInfo().m_splitImpulse = false; + getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS; + m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher); +} + +btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () +{ + delete m_solverMultiBodyIslandCallback; +} + + + + +void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +{ + + btAlignedObjectArray<btScalar> scratch_r; + btAlignedObjectArray<btVector3> scratch_v; + btAlignedObjectArray<btMatrix3x3> scratch_m; + + + BT_PROFILE("solveConstraints"); + + m_sortedConstraints.resize( m_constraints.size()); + int i; + for (i=0;i<getNumConstraints();i++) + { + m_sortedConstraints[i] = m_constraints[i]; + } + m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); + btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + + m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); + for (i=0;i<m_multiBodyConstraints.size();i++) + { + m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; + } + m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); + + btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; + + + m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); + + /// solve all the constraints for this island + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback); + + + { + BT_PROFILE("btMultiBody addForce and 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) + { + scratch_r.resize(bod->getNumLinks()+1); + scratch_v.resize(bod->getNumLinks()+1); + scratch_m.resize(bod->getNumLinks()+1); + + bod->clearForcesAndTorques(); + bod->addBaseForce(m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); + } + + bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); + } + } + } + + m_solverMultiBodyIslandCallback->processConstraints(); + + m_constraintSolver->allSolved(solverInfo, m_debugDrawer); + +} + +void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) +{ + btDiscreteDynamicsWorld::integrateTransforms(timeStep); + + { + BT_PROFILE("btMultiBody stepPositions"); + //integrate and update the Featherstone hierarchies + btAlignedObjectArray<btQuaternion> world_to_local; + btAlignedObjectArray<btVector3> local_origin; + + for (int b=0;b<m_multiBodies.size();b++) + { + btMultiBody* bod = m_multiBodies[b]; + 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) + { + int nLinks = bod->getNumLinks(); + + ///base + num links + world_to_local.resize(nLinks+1); + local_origin.resize(nLinks+1); + + bod->stepPositions(timeStep); + + + + world_to_local[0] = bod->getWorldToBaseRot(); + local_origin[0] = bod->getBasePos(); + + if (bod->getBaseCollider()) + { + btVector3 posr = local_origin[0]; + float pos[4]={posr.x(),posr.y(),posr.z(),1}; + float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + + bod->getBaseCollider()->setWorldTransform(tr); + + } + + for (int k=0;k<bod->getNumLinks();k++) + { + const int parent = bod->getParent(k); + world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1]; + local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k))); + } + + + for (int m=0;m<bod->getNumLinks();m++) + { + btMultiBodyLinkCollider* col = bod->getLink(m).m_collider; + if (col) + { + int link = col->m_link; + btAssert(link == m); + + int index = link+1; + + btVector3 posr = local_origin[index]; + float pos[4]={posr.x(),posr.y(),posr.z(),1}; + float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + + col->setWorldTransform(tr); + } + } + } else + { + bod->clearVelocities(); + } + } + } +} + + + +void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint) +{ + m_multiBodyConstraints.push_back(constraint); +} + +void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint) +{ + m_multiBodyConstraints.remove(constraint); +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h new file mode 100644 index 00000000000..ad57a346dcc --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -0,0 +1,56 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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_DYNAMICS_WORLD_H +#define BT_MULTIBODY_DYNAMICS_WORLD_H + +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" + + +class btMultiBody; +class btMultiBodyConstraint; +class btMultiBodyConstraintSolver; +struct MultiBodyInplaceSolverIslandCallback; + +///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet +///This implementation is still preliminary/experimental. +class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld +{ +protected: + btAlignedObjectArray<btMultiBody*> m_multiBodies; + btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; + btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints; + btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; + MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; + + virtual void calculateSimulationIslands(); + virtual void updateActivationState(btScalar timeStep); + virtual void solveConstraints(btContactSolverInfo& solverInfo); + virtual void integrateTransforms(btScalar timeStep); +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 void addMultiBodyConstraint( btMultiBodyConstraint* constraint); + + virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); +}; +#endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp new file mode 100644 index 00000000000..ea309e8857d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -0,0 +1,133 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyJointLimitConstraint.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + + +btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) + :btMultiBodyConstraint(body,body,link,link,2,true), + m_lowerBound(lower), + m_upperBound(upper) +{ + // 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; + + // row 1: the upper bound + jacobianB(1)[6 + link] = -1; +} +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->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++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + return -1; +} + + +void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + 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. + + // row 0: the lower bound + setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); + + // row 1: the upper bound + setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); + + for (int row=0;row<getNumRows();row++) + { + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + 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); + { + btScalar penetration = getPosition(row); + btScalar positionalError = 0.f; + btScalar velocityError = - rel_vel;// * damping; + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + if (penetration>0) + { + positionalError = 0; + velocityError = -penetration / infoGlobal.m_timeStep; + } else + { + positionalError = -penetration * erp/infoGlobal.m_timeStep; + } + + btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + constraintRow.m_rhs = penetrationImpulse+velocityImpulse; + constraintRow.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + constraintRow.m_rhs = velocityImpulse; + constraintRow.m_rhsPenetration = penetrationImpulse; + } + } + } + +} + + + + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h new file mode 100644 index 00000000000..0c7fc170822 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -0,0 +1,44 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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_LIMIT_CONSTRAINT_H +#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" +struct btSolverInfo; + +class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint +{ +protected: + + btScalar m_lowerBound; + btScalar m_upperBound; +public: + + btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper); + virtual ~btMultiBodyJointLimitConstraint(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + +}; + +#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 new file mode 100644 index 00000000000..ab5a430231b --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -0,0 +1,89 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyJointMotor.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + + +btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) + :btMultiBodyConstraint(body,body,link,link,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; +} +btMultiBodyJointMotor::~btMultiBodyJointMotor() +{ +} + +int btMultiBodyJointMotor::getIslandIdA() const +{ + 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 btMultiBodyJointMotor::getIslandIdB() const +{ + 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; +} + + +void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + 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. + + + + 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); + } + +} + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h new file mode 100644 index 00000000000..43869348141 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -0,0 +1,47 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_JOINT_MOTOR_H +#define BT_MULTIBODY_JOINT_MOTOR_H + +#include "btMultiBodyConstraint.h" +struct btSolverInfo; + +class btMultiBodyJointMotor : public btMultiBodyConstraint +{ +protected: + + + btScalar m_desiredVelocity; + +public: + + btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse); + virtual ~btMultiBodyJointMotor(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + +}; + +#endif //BT_MULTIBODY_JOINT_MOTOR_H + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h new file mode 100644 index 00000000000..fbd54c45db3 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -0,0 +1,110 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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_LINK_H +#define BT_MULTIBODY_LINK_H + +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btVector3.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +enum btMultiBodyLinkFlags +{ + BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1 +}; +// +// Link struct +// + +struct btMultibodyLink +{ + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btScalar joint_pos; // qi + + 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. + + btQuaternion zero_rot_parent_to_this; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant. + + // "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; + + btVector3 d_vector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only. + + // e_vector is constant, but depends on the joint type + // prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.) + // revolute: vector from parent's COM to the pivot point, in PARENT's frame. + btVector3 e_vector; + + bool is_revolute; // true = revolute, false = prismatic + + 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 applied_force; // In WORLD frame + btVector3 applied_torque; // In WORLD frame + btScalar joint_torque; + + class btMultiBodyLinkCollider* m_collider; + int m_flags; + + // 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_collider(0), + m_flags(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); + } + + // routine to update cached_rot_parent_to_this and cached_r_vector + void updateCache() + { + 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 + { + // cached_rot_parent_to_this never changes, so no need to update + cached_r_vector = e_vector + joint_pos * axis_bottom; + } + } +}; + + +#endif //BT_MULTIBODY_LINK_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h new file mode 100644 index 00000000000..27f12514a6d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -0,0 +1,92 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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_FEATHERSTONE_LINK_COLLIDER_H +#define BT_FEATHERSTONE_LINK_COLLIDER_H + +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +#include "btMultiBody.h" + +class btMultiBodyLinkCollider : public btCollisionObject +{ +//protected: +public: + + btMultiBody* m_multiBody; + int m_link; + + + btMultiBodyLinkCollider (btMultiBody* multiBody,int link) + :m_multiBody(multiBody), + m_link(link) + { + m_checkCollideWith = true; + //we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands + //this means that some constraints might point to bodies that are not in the islands, causing crashes + //if (link>=0 || (multiBody && !multiBody->hasFixedBase())) + { + m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); + } + // else + //{ + // m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT); + //} + + m_internalType = CO_FEATHERSTONE_LINK; + } + static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj) + { + if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) + return (btMultiBodyLinkCollider*)colObj; + return 0; + } + static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj) + { + if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) + return (btMultiBodyLinkCollider*)colObj; + return 0; + } + + virtual bool checkCollideWithOverride(const btCollisionObject* co) const + { + const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co); + if (!other) + return true; + if (other->m_multiBody != this->m_multiBody) + return true; + if (!m_multiBody->hasSelfCollision()) + return false; + + //check if 'link' has collision disabled + 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) + 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) + return false; + } + return true; + } +}; + +#endif //BT_FEATHERSTONE_LINK_COLLIDER_H + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp new file mode 100644 index 00000000000..f6690049156 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -0,0 +1,143 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyPoint2Point.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + +btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) + :btMultiBodyConstraint(body,0,link,-1,3,false), + m_rigidBodyA(0), + m_rigidBodyB(bodyB), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB) +{ +} + +btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) + :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,3,false), + m_rigidBodyA(0), + m_rigidBodyB(0), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB) +{ +} + + +btMultiBodyPoint2Point::~btMultiBodyPoint2Point() +{ +} + + +int btMultiBodyPoint2Point::getIslandIdA() const +{ + if (m_rigidBodyA) + return m_rigidBodyA->getIslandTag(); + + if (m_bodyA) + { + 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 btMultiBodyPoint2Point::getIslandIdB() const +{ + if (m_rigidBodyB) + return m_rigidBodyB->getIslandTag(); + if (m_bodyB) + { + 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; +} + + + +void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + +// int i=1; + for (int i=0;i<3;i++) + { + + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + + constraintRow.m_solverBodyIdA = data.m_fixedBodyId; + constraintRow.m_solverBodyIdB = data.m_fixedBodyId; + + + btVector3 contactNormalOnB(0,0,0); + contactNormalOnB[i] = -1; + + 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 + { + if (m_bodyA) + pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + } + btVector3 pivotBworld = m_pivotInB; + if (m_rigidBodyB) + { + constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); + pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; + } else + { + 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; + + } +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h new file mode 100644 index 00000000000..26ca12b406d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -0,0 +1,60 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_POINT2POINT_H +#define BT_MULTIBODY_POINT2POINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodyPoint2Point : public btMultiBodyConstraint +{ +protected: + + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + + +public: + + btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB); + btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB); + + virtual ~btMultiBodyPoint2Point(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + +}; + +#endif //BT_MULTIBODY_POINT2POINT_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h new file mode 100644 index 00000000000..cf06dfb9ebf --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -0,0 +1,82 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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_SOLVER_CONSTRAINT_H +#define BT_MULTIBODY_SOLVER_CONSTRAINT_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" + +class btMultiBody; +#include "BulletDynamics/ConstraintSolver/btSolverBody.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" + +///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. +ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + + int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1 + btVector3 m_relpos1CrossNormal; + btVector3 m_contactNormal1; + int m_jacAindex; + + int m_deltaVelBindex; + btVector3 m_relpos2CrossNormal; + btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always + int m_jacBindex; + + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; + + mutable btSimdScalar m_appliedPushImpulse; + mutable btSimdScalar m_appliedImpulse; + + btScalar m_friction; + btScalar m_jacDiagABInv; + btScalar m_rhs; + btScalar m_cfm; + + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_rhsPenetration; + union + { + void* m_originalContactPoint; + btScalar m_unusedPadding4; + }; + + int m_overrideNumSolverIterations; + int m_frictionIndex; + + int m_solverBodyIdA; + btMultiBody* m_multiBodyA; + int m_linkA; + + int m_solverBodyIdB; + btMultiBody* m_multiBodyB; + int m_linkB; + + enum btSolverConstraintType + { + BT_SOLVER_CONTACT_1D = 0, + BT_SOLVER_FRICTION_1D + }; +}; + +typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray; + +#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp new file mode 100644 index 00000000000..3bf7b5c1311 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp @@ -0,0 +1,2079 @@ +/************************************************************************* +* * +* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * +* All rights reserved. Email: russ@q12.org Web: www.q12.org * +* * +* This library is free software; you can redistribute it and/or * +* modify it under the terms of EITHER: * +* (1) The GNU Lesser General Public License as published by the Free * +* Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. The text of the GNU Lesser * +* General Public License is included with this library in the * +* file LICENSE.TXT. * +* (2) The BSD-style license that is included with this library in * +* the file LICENSE-BSD.TXT. * +* * +* This library is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * +* LICENSE.TXT and LICENSE-BSD.TXT for more details. * +* * +*************************************************************************/ + +/* + + +THE ALGORITHM +------------- + +solve A*x = b+w, with x and w subject to certain LCP conditions. +each x(i),w(i) must lie on one of the three line segments in the following +diagram. each line segment corresponds to one index set : + + w(i) + /|\ | : + | | : + | |i in N : + w>0 | |state[i]=0 : + | | : + | | : i in C + w=0 + +-----------------------+ + | : | + | : | + w<0 | : |i in N + | : |state[i]=1 + | : | + | : | + +-------|-----------|-----------|----------> x(i) + lo 0 hi + +the Dantzig algorithm proceeds as follows: + for i=1:n + * if (x(i),w(i)) is not on the line, push x(i) and w(i) positive or + negative towards the line. as this is done, the other (x(j),w(j)) + for j<i are constrained to be on the line. if any (x,w) reaches the + end of a line segment then it is switched between index sets. + * i is added to the appropriate index set depending on what line segment + it hits. + +we restrict lo(i) <= 0 and hi(i) >= 0. this makes the algorithm a bit +simpler, because the starting point for x(i),w(i) is always on the dotted +line x=0 and x will only ever increase in one direction, so it can only hit +two out of the three line segments. + + +NOTES +----- + +this is an implementation of "lcp_dantzig2_ldlt.m" and "lcp_dantzig_lohi.m". +the implementation is split into an LCP problem object (btLCP) and an LCP +driver function. most optimization occurs in the btLCP object. + +a naive implementation of the algorithm requires either a lot of data motion +or a lot of permutation-array lookup, because we are constantly re-ordering +rows and columns. to avoid this and make a more optimized algorithm, a +non-trivial data structure is used to represent the matrix A (this is +implemented in the fast version of the btLCP object). + +during execution of this algorithm, some indexes in A are clamped (set C), +some are non-clamped (set N), and some are "don't care" (where x=0). +A,x,b,w (and other problem vectors) are permuted such that the clamped +indexes are first, the unclamped indexes are next, and the don't-care +indexes are last. this permutation is recorded in the array `p'. +initially p = 0..n-1, and as the rows and columns of A,x,b,w are swapped, +the corresponding elements of p are swapped. + +because the C and N elements are grouped together in the rows of A, we can do +lots of work with a fast dot product function. if A,x,etc were not permuted +and we only had a permutation array, then those dot products would be much +slower as we would have a permutation array lookup in some inner loops. + +A is accessed through an array of row pointers, so that element (i,j) of the +permuted matrix is A[i][j]. this makes row swapping fast. for column swapping +we still have to actually move the data. + +during execution of this algorithm we maintain an L*D*L' factorization of +the clamped submatrix of A (call it `AC') which is the top left nC*nC +submatrix of A. there are two ways we could arrange the rows/columns in AC. + +(1) AC is always permuted such that L*D*L' = AC. this causes a problem +when a row/column is removed from C, because then all the rows/columns of A +between the deleted index and the end of C need to be rotated downward. +this results in a lot of data motion and slows things down. +(2) L*D*L' is actually a factorization of a *permutation* of AC (which is +itself a permutation of the underlying A). this is what we do - the +permutation is recorded in the vector C. call this permutation A[C,C]. +when a row/column is removed from C, all we have to do is swap two +rows/columns and manipulate C. + +*/ + + +#include "btDantzigLCP.h" + +#include <string.h>//memcpy + +bool s_error = false; + +//*************************************************************************** +// code generation parameters + + +#define btLCP_FAST // use fast btLCP object + +// option 1 : matrix row pointers (less data copying) +#define BTROWPTRS +#define BTATYPE btScalar ** +#define BTAROW(i) (m_A[i]) + +// option 2 : no matrix row pointers (slightly faster inner loops) +//#define NOROWPTRS +//#define BTATYPE btScalar * +//#define BTAROW(i) (m_A+(i)*m_nskip) + +#define BTNUB_OPTIMIZATIONS + + + +/* solve L*X=B, with B containing 1 right hand sides. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * B is an n*1 matrix that contains the right hand sides. + * B is stored by columns and its leading dimension is also lskip. + * B is overwritten with X. + * this processes blocks of 2*2. + * if this is in the factorizer source file, n must be a multiple of 2. + */ + +static void btSolveL1_1 (const btScalar *L, btScalar *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + btScalar Z11,m11,Z21,m21,p1,q1,p2,*ex; + const btScalar *ell; + int i,j; + /* compute all 2 x 1 blocks of X */ + for (i=0; i < n; i+=2) { + /* compute all 2 x 1 block of X, from rows i..i+2-1 */ + /* set the Z matrix to 0 */ + Z11=0; + Z21=0; + ell = L + i*lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-2; j >= 0; j -= 2) { + /* compute outer product and add it to the Z matrix */ + p1=ell[0]; + q1=ex[0]; + m11 = p1 * q1; + p2=ell[lskip1]; + m21 = p2 * q1; + Z11 += m11; + Z21 += m21; + /* compute outer product and add it to the Z matrix */ + p1=ell[1]; + q1=ex[1]; + m11 = p1 * q1; + p2=ell[1+lskip1]; + m21 = p2 * q1; + /* advance pointers */ + ell += 2; + ex += 2; + Z11 += m11; + Z21 += m21; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 2; + for (; j > 0; j--) { + /* compute outer product and add it to the Z matrix */ + p1=ell[0]; + q1=ex[0]; + m11 = p1 * q1; + p2=ell[lskip1]; + m21 = p2 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + Z11 += m11; + Z21 += m21; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1*Z11; + ex[1] = Z21; + /* end of outer loop */ + } +} + +/* solve L*X=B, with B containing 2 right hand sides. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * B is an n*2 matrix that contains the right hand sides. + * B is stored by columns and its leading dimension is also lskip. + * B is overwritten with X. + * this processes blocks of 2*2. + * if this is in the factorizer source file, n must be a multiple of 2. + */ + +static void btSolveL1_2 (const btScalar *L, btScalar *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + btScalar Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex; + const btScalar *ell; + int i,j; + /* compute all 2 x 2 blocks of X */ + for (i=0; i < n; i+=2) { + /* compute all 2 x 2 block of X, from rows i..i+2-1 */ + /* set the Z matrix to 0 */ + Z11=0; + Z12=0; + Z21=0; + Z22=0; + ell = L + i*lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-2; j >= 0; j -= 2) { + /* compute outer product and add it to the Z matrix */ + p1=ell[0]; + q1=ex[0]; + m11 = p1 * q1; + q2=ex[lskip1]; + m12 = p1 * q2; + p2=ell[lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + /* compute outer product and add it to the Z matrix */ + p1=ell[1]; + q1=ex[1]; + m11 = p1 * q1; + q2=ex[1+lskip1]; + m12 = p1 * q2; + p2=ell[1+lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + /* advance pointers */ + ell += 2; + ex += 2; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 2; + for (; j > 0; j--) { + /* compute outer product and add it to the Z matrix */ + p1=ell[0]; + q1=ex[0]; + m11 = p1 * q1; + q2=ex[lskip1]; + m12 = p1 * q2; + p2=ell[lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + /* advance pointers */ + ell += 1; + ex += 1; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + Z12 = ex[lskip1] - Z12; + ex[lskip1] = Z12; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1*Z11; + ex[1] = Z21; + Z22 = ex[1+lskip1] - Z22 - p1*Z12; + ex[1+lskip1] = Z22; + /* end of outer loop */ + } +} + + +void btFactorLDLT (btScalar *A, btScalar *d, int n, int nskip1) +{ + int i,j; + btScalar sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22; + if (n < 1) return; + + for (i=0; i<=n-2; i += 2) { + /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */ + btSolveL1_2 (A,A+i*nskip1,i,nskip1); + /* scale the elements in a 2 x i block at A(i,0), and also */ + /* compute Z = the outer product matrix that we'll need. */ + Z11 = 0; + Z21 = 0; + Z22 = 0; + ell = A+i*nskip1; + dee = d; + for (j=i-6; j >= 0; j -= 6) { + p1 = ell[0]; + p2 = ell[nskip1]; + dd = dee[0]; + q1 = p1*dd; + q2 = p2*dd; + ell[0] = q1; + ell[nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[1]; + p2 = ell[1+nskip1]; + dd = dee[1]; + q1 = p1*dd; + q2 = p2*dd; + ell[1] = q1; + ell[1+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[2]; + p2 = ell[2+nskip1]; + dd = dee[2]; + q1 = p1*dd; + q2 = p2*dd; + ell[2] = q1; + ell[2+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[3]; + p2 = ell[3+nskip1]; + dd = dee[3]; + q1 = p1*dd; + q2 = p2*dd; + ell[3] = q1; + ell[3+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[4]; + p2 = ell[4+nskip1]; + dd = dee[4]; + q1 = p1*dd; + q2 = p2*dd; + ell[4] = q1; + ell[4+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[5]; + p2 = ell[5+nskip1]; + dd = dee[5]; + q1 = p1*dd; + q2 = p2*dd; + ell[5] = q1; + ell[5+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + ell += 6; + dee += 6; + } + /* compute left-over iterations */ + j += 6; + for (; j > 0; j--) { + p1 = ell[0]; + p2 = ell[nskip1]; + dd = dee[0]; + q1 = p1*dd; + q2 = p2*dd; + ell[0] = q1; + ell[nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + ell++; + dee++; + } + /* solve for diagonal 2 x 2 block at A(i,i) */ + Z11 = ell[0] - Z11; + Z21 = ell[nskip1] - Z21; + Z22 = ell[1+nskip1] - Z22; + dee = d + i; + /* factorize 2 x 2 block Z,dee */ + /* factorize row 1 */ + dee[0] = btRecip(Z11); + /* factorize row 2 */ + sum = 0; + q1 = Z21; + q2 = q1 * dee[0]; + Z21 = q2; + sum += q1*q2; + dee[1] = btRecip(Z22 - sum); + /* done factorizing 2 x 2 block */ + ell[nskip1] = Z21; + } + /* compute the (less than 2) rows at the bottom */ + switch (n-i) { + case 0: + break; + + case 1: + btSolveL1_1 (A,A+i*nskip1,i,nskip1); + /* scale the elements in a 1 x i block at A(i,0), and also */ + /* compute Z = the outer product matrix that we'll need. */ + Z11 = 0; + ell = A+i*nskip1; + dee = d; + for (j=i-6; j >= 0; j -= 6) { + p1 = ell[0]; + dd = dee[0]; + q1 = p1*dd; + ell[0] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[1]; + dd = dee[1]; + q1 = p1*dd; + ell[1] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[2]; + dd = dee[2]; + q1 = p1*dd; + ell[2] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[3]; + dd = dee[3]; + q1 = p1*dd; + ell[3] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[4]; + dd = dee[4]; + q1 = p1*dd; + ell[4] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[5]; + dd = dee[5]; + q1 = p1*dd; + ell[5] = q1; + m11 = p1*q1; + Z11 += m11; + ell += 6; + dee += 6; + } + /* compute left-over iterations */ + j += 6; + for (; j > 0; j--) { + p1 = ell[0]; + dd = dee[0]; + q1 = p1*dd; + ell[0] = q1; + m11 = p1*q1; + Z11 += m11; + ell++; + dee++; + } + /* solve for diagonal 1 x 1 block at A(i,i) */ + Z11 = ell[0] - Z11; + dee = d + i; + /* factorize 1 x 1 block Z,dee */ + /* factorize row 1 */ + dee[0] = btRecip(Z11); + /* done factorizing 1 x 1 block */ + break; + + //default: *((char*)0)=0; /* this should never happen! */ + } +} + +/* solve L*X=B, with B containing 1 right hand sides. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * B is an n*1 matrix that contains the right hand sides. + * B is stored by columns and its leading dimension is also lskip. + * B is overwritten with X. + * this processes blocks of 4*4. + * if this is in the factorizer source file, n must be a multiple of 4. + */ + +void btSolveL1 (const btScalar *L, btScalar *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + btScalar Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex; + const btScalar *ell; + int lskip2,lskip3,i,j; + /* compute lskip values */ + lskip2 = 2*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 */ + /* set the Z matrix to 0 */ + Z11=0; + Z21=0; + Z31=0; + Z41=0; + ell = L + i*lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-12; j >= 0; j -= 12) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + p2=ell[lskip1]; + p3=ell[lskip2]; + p4=ell[lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[1]; + q1=ex[1]; + p2=ell[1+lskip1]; + p3=ell[1+lskip2]; + p4=ell[1+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[2]; + q1=ex[2]; + p2=ell[2+lskip1]; + p3=ell[2+lskip2]; + p4=ell[2+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[3]; + q1=ex[3]; + p2=ell[3+lskip1]; + p3=ell[3+lskip2]; + p4=ell[3+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[4]; + q1=ex[4]; + p2=ell[4+lskip1]; + p3=ell[4+lskip2]; + p4=ell[4+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[5]; + q1=ex[5]; + p2=ell[5+lskip1]; + p3=ell[5+lskip2]; + p4=ell[5+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[6]; + q1=ex[6]; + p2=ell[6+lskip1]; + p3=ell[6+lskip2]; + p4=ell[6+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[7]; + q1=ex[7]; + p2=ell[7+lskip1]; + p3=ell[7+lskip2]; + p4=ell[7+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[8]; + q1=ex[8]; + p2=ell[8+lskip1]; + p3=ell[8+lskip2]; + p4=ell[8+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[9]; + q1=ex[9]; + p2=ell[9+lskip1]; + p3=ell[9+lskip2]; + p4=ell[9+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[10]; + q1=ex[10]; + p2=ell[10+lskip1]; + p3=ell[10+lskip2]; + p4=ell[10+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[11]; + q1=ex[11]; + p2=ell[11+lskip1]; + p3=ell[11+lskip2]; + p4=ell[11+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* advance pointers */ + ell += 12; + ex += 12; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 12; + for (; j > 0; j--) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + p2=ell[lskip1]; + p3=ell[lskip2]; + p4=ell[lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1*Z11; + ex[1] = Z21; + p1 = ell[lskip2]; + p2 = ell[1+lskip2]; + Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21; + ex[2] = Z31; + p1 = ell[lskip3]; + p2 = ell[1+lskip3]; + p3 = ell[2+lskip3]; + Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31; + ex[3] = Z41; + /* end of outer loop */ + } + /* compute rows at end that are not a multiple of block size */ + for (; i < n; i++) { + /* compute all 1 x 1 block of X, from rows i..i+1-1 */ + /* set the Z matrix to 0 */ + Z11=0; + ell = L + i*lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-12; j >= 0; j -= 12) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[1]; + q1=ex[1]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[2]; + q1=ex[2]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[3]; + q1=ex[3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[4]; + q1=ex[4]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[5]; + q1=ex[5]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[6]; + q1=ex[6]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[7]; + q1=ex[7]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[8]; + q1=ex[8]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[9]; + q1=ex[9]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[10]; + q1=ex[10]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[11]; + q1=ex[11]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* advance pointers */ + ell += 12; + ex += 12; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 12; + for (; j > 0; j--) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + } +} + +/* solve L^T * x=b, with b containing 1 right hand side. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * b is an n*1 matrix that contains the right hand side. + * b is overwritten with x. + * this processes blocks of 4. + */ + +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; + /* 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; + /* 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 */ + /* set the Z matrix to 0 */ + Z11=0; + Z21=0; + Z31=0; + Z41=0; + ell = L - i; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-4; j >= 0; j -= 4) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1=ell[0]; + q1=ex[-1]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1=ell[0]; + q1=ex[-2]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1=ell[0]; + q1=ex[-3]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + ex -= 4; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 4; + for (; j > 0; j--) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + ex -= 1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[-1]; + Z21 = ex[-1] - Z21 - p1*Z11; + ex[-1] = Z21; + p1 = ell[-2]; + p2 = ell[-2+lskip1]; + Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21; + ex[-2] = Z31; + p1 = ell[-3]; + p2 = ell[-3+lskip1]; + p3 = ell[-3+lskip2]; + Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31; + ex[-3] = Z41; + /* end of outer loop */ + } + /* compute rows at end that are not a multiple of block size */ + for (; i < n; i++) { + /* compute all 1 x 1 block of X, from rows i..i+1-1 */ + /* set the Z matrix to 0 */ + Z11=0; + ell = L - i; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-4; j >= 0; j -= 4) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1=ell[0]; + q1=ex[-1]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1=ell[0]; + q1=ex[-2]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1=ell[0]; + q1=ex[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + ex -= 4; + Z11 += m11; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 4; + for (; j > 0; j--) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + ex -= 1; + Z11 += m11; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + } +} + + + +void btVectorScale (btScalar *a, const btScalar *d, int n) +{ + btAssert (a && d && n >= 0); + for (int i=0; i<n; i++) { + a[i] *= d[i]; + } +} + +void btSolveLDLT (const btScalar *L, const btScalar *d, btScalar *b, int n, int nskip) +{ + btAssert (L && d && b && n > 0 && nskip >= n); + btSolveL1 (L,b,n,nskip); + btVectorScale (b,d,n); + btSolveL1T (L,b,n,nskip); +} + + + +//*************************************************************************** + +// swap row/column i1 with i2 in the n*n matrix A. the leading dimension of +// A is nskip. this only references and swaps the lower triangle. +// if `do_fast_row_swaps' is nonzero and row pointers are being used, then +// rows will be swapped by exchanging row pointers. otherwise the data will +// be copied. + +static void btSwapRowsAndCols (BTATYPE A, int n, int i1, int i2, int nskip, + int do_fast_row_swaps) +{ + btAssert (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && + nskip >= n && i1 < i2); + +# ifdef BTROWPTRS + btScalar *A_i1 = A[i1]; + btScalar *A_i2 = A[i2]; + for (int i=i1+1; i<i2; ++i) { + btScalar *A_i_i1 = A[i] + i1; + A_i1[i] = *A_i_i1; + *A_i_i1 = A_i2[i]; + } + A_i1[i2] = A_i1[i1]; + A_i1[i1] = A_i2[i1]; + A_i2[i1] = A_i2[i2]; + // swap rows, by swapping row pointers + if (do_fast_row_swaps) { + A[i1] = A_i2; + A[i2] = A_i1; + } + else { + // Only swap till i2 column to match A plain storage variant. + for (int k = 0; k <= i2; ++k) { + btScalar tmp = A_i1[k]; + A_i1[k] = A_i2[k]; + A_i2[k] = tmp; + } + } + // swap columns the hard way + for (int j=i2+1; j<n; ++j) { + btScalar *A_j = A[j]; + btScalar tmp = A_j[i1]; + A_j[i1] = A_j[i2]; + A_j[i2] = tmp; + } +# else + btScalar *A_i1 = A+i1*nskip; + btScalar *A_i2 = A+i2*nskip; + for (int k = 0; k < i1; ++k) { + btScalar tmp = A_i1[k]; + A_i1[k] = A_i2[k]; + A_i2[k] = tmp; + } + btScalar *A_i = A_i1 + nskip; + for (int i=i1+1; i<i2; A_i+=nskip, ++i) { + btScalar tmp = A_i2[i]; + A_i2[i] = A_i[i1]; + A_i[i1] = tmp; + } + { + btScalar tmp = A_i1[i1]; + A_i1[i1] = A_i2[i2]; + A_i2[i2] = tmp; + } + btScalar *A_j = A_i2 + nskip; + for (int j=i2+1; j<n; A_j+=nskip, ++j) { + btScalar tmp = A_j[i1]; + A_j[i1] = A_j[i2]; + A_j[i2] = tmp; + } +# endif +} + + +// swap two indexes in the n*n LCP problem. i1 must be <= i2. + +static void btSwapProblem (BTATYPE A, btScalar *x, btScalar *b, btScalar *w, btScalar *lo, + btScalar *hi, int *p, bool *state, int *findex, + int n, int i1, int i2, int nskip, + int do_fast_row_swaps) +{ + btScalar tmpr; + int tmpi; + bool tmpb; + btAssert (n>0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && i1 <= i2); + if (i1==i2) return; + + btSwapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps); + + tmpr = x[i1]; + x[i1] = x[i2]; + x[i2] = tmpr; + + tmpr = b[i1]; + b[i1] = b[i2]; + b[i2] = tmpr; + + tmpr = w[i1]; + w[i1] = w[i2]; + w[i2] = tmpr; + + tmpr = lo[i1]; + lo[i1] = lo[i2]; + lo[i2] = tmpr; + + tmpr = hi[i1]; + hi[i1] = hi[i2]; + hi[i2] = tmpr; + + tmpi = p[i1]; + p[i1] = p[i2]; + p[i2] = tmpi; + + tmpb = state[i1]; + state[i1] = state[i2]; + state[i2] = tmpb; + + if (findex) { + tmpi = findex[i1]; + findex[i1] = findex[i2]; + findex[i2] = tmpi; + } +} + + + + +//*************************************************************************** +// btLCP manipulator object. this represents an n*n LCP problem. +// +// two index sets C and N are kept. each set holds a subset of +// the variable indexes 0..n-1. an index can only be in one set. +// initially both sets are empty. +// +// the index set C is special: solutions to A(C,C)\A(C,i) can be generated. + +//*************************************************************************** +// fast implementation of btLCP. see the above definition of btLCP for +// interface comments. +// +// `p' records the permutation of A,x,b,w,etc. p is initially 1:n and is +// permuted as the other vectors/matrices are permuted. +// +// A,x,b,w,lo,hi,state,findex,p,c are permuted such that sets C,N have +// contiguous indexes. the don't-care indexes follow N. +// +// an L*D*L' factorization is maintained of A(C,C), and whenever indexes are +// added or removed from the set C the factorization is updated. +// thus L*D*L'=A[C,C], i.e. a permuted top left nC*nC submatrix of A. +// the leading dimension of the matrix L is always `nskip'. +// +// at the start there may be other indexes that are unbounded but are not +// included in `nub'. btLCP will permute the matrix so that absolutely all +// unbounded vectors are at the start. thus there may be some initial +// permutation. +// +// the algorithms here assume certain patterns, particularly with respect to +// index transfer. + +#ifdef btLCP_FAST + +struct btLCP +{ + const int m_n; + const int m_nskip; + int m_nub; + int m_nC, m_nN; // size of each index set + BTATYPE const m_A; // A rows + btScalar *const m_x, * const m_b, *const m_w, *const m_lo,* const m_hi; // permuted LCP problem data + btScalar *const m_L, *const m_d; // L*D*L' factorization of set C + btScalar *const m_Dell, *const m_ell, *const m_tmp; + bool *const m_state; + 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 *_Dell, btScalar *_ell, btScalar *_tmp, + 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 + void transfer_i_from_N_to_C (int i); + void transfer_i_from_C_to_N (int i, btAlignedObjectArray<btScalar>& scratch); + int numC() const { return m_nC; } + int numN() const { return m_nN; } + int indexC (int i) const { return i; } + int indexN (int i) const { return i+m_nC; } + btScalar Aii (int i) const { return BTAROW(i)[i]; } + btScalar AiC_times_qC (int i, btScalar *q) const { return btLargeDot (BTAROW(i), q, m_nC); } + btScalar AiN_times_qN (int i, btScalar *q) const { return btLargeDot (BTAROW(i)+m_nC, q+m_nC, m_nN); } + void pN_equals_ANC_times_qC (btScalar *p, btScalar *q); + void pN_plusequals_ANi (btScalar *p, int i, int sign=1); + void pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q); + void pN_plusequals_s_times_qN (btScalar *p, btScalar s, btScalar *q); + void solve1 (btScalar *a, int i, int dir=1, int only_transfer=0); + void unpermute(); +}; + + +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 *_Dell, btScalar *_ell, btScalar *_tmp, + 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), +#else + 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) +{ + { + btSetZero (m_x,m_n); + } + + { +# ifdef BTROWPTRS + // make matrix row pointers + btScalar *aptr = _Adata; + BTATYPE A = m_A; + const int n = m_n, nskip = m_nskip; + for (int k=0; k<n; aptr+=nskip, ++k) A[k] = aptr; +# endif + } + + { + int *p = m_p; + const int n = m_n; + for (int k=0; k<n; ++k) p[k]=k; // initially unpermuted + } + + /* + // for testing, we can do some random swaps in the area i > nub + { + const int n = m_n; + const int nub = m_nub; + if (nub < n) { + for (int k=0; k<100; k++) { + int i1,i2; + do { + i1 = dRandInt(n-nub)+nub; + i2 = dRandInt(n-nub)+nub; + } + while (i1 > i2); + //printf ("--> %d %d\n",i1,i2); + btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,n,i1,i2,m_nskip,0); + } + } + */ + + // permute the problem so that *all* the unbounded variables are at the + // start, i.e. look for unbounded variables not included in `nub'. we can + // potentially push up `nub' this way and get a bigger initial factorization. + // note that when we swap rows/cols here we must not just swap row pointers, + // as the initial factorization relies on the data being all in one chunk. + // variables that have findex >= 0 are *not* considered to be unbounded even + // if lo=-inf and hi=inf - this is because these limits may change during the + // solution process. + + { + int *findex = m_findex; + btScalar *lo = m_lo, *hi = m_hi; + const int n = m_n; + for (int k = m_nub; k<n; ++k) { + if (findex && findex[k] >= 0) continue; + if (lo[k]==-BT_INFINITY && hi[k]==BT_INFINITY) { + btSwapProblem (m_A,m_x,m_b,m_w,lo,hi,m_p,m_state,findex,n,m_nub,k,m_nskip,0); + m_nub++; + } + } + } + + // if there are unbounded variables at the start, factorize A up to that + // point and solve for x. this puts all indexes 0..nub-1 into C. + if (m_nub > 0) { + const int nub = m_nub; + { + btScalar *Lrow = m_L; + const int nskip = m_nskip; + for (int j=0; j<nub; Lrow+=nskip, ++j) memcpy(Lrow,BTAROW(j),(j+1)*sizeof(btScalar)); + } + btFactorLDLT (m_L,m_d,nub,m_nskip); + memcpy (m_x,m_b,nub*sizeof(btScalar)); + btSolveLDLT (m_L,m_d,m_x,nub,m_nskip); + btSetZero (m_w,nub); + { + int *C = m_C; + for (int k=0; k<nub; ++k) C[k] = k; + } + m_nC = nub; + } + + // permute the indexes > nub such that all findex variables are at the end + if (m_findex) { + const int nub = m_nub; + int *findex = m_findex; + int num_at_end = 0; + for (int k=m_n-1; k >= nub; k--) { + if (findex[k] >= 0) { + btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,findex,m_n,k,m_n-1-num_at_end,m_nskip,1); + num_at_end++; + } + } + } + + // print info about indexes + /* + { + const int n = m_n; + const int nub = m_nub; + for (int k=0; k<n; k++) { + if (k<nub) printf ("C"); + else if (m_lo[k]==-BT_INFINITY && m_hi[k]==BT_INFINITY) printf ("c"); + else printf ("."); + } + printf ("\n"); + } + */ +} + + +void btLCP::transfer_i_to_C (int i) +{ + { + if (m_nC > 0) { + // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C)) + { + const int nC = m_nC; + btScalar *const Ltgt = m_L + nC*m_nskip, *ell = m_ell; + for (int j=0; j<nC; ++j) Ltgt[j] = ell[j]; + } + const int nC = m_nC; + m_d[nC] = btRecip (BTAROW(i)[i] - btLargeDot(m_ell,m_Dell,nC)); + } + else { + m_d[0] = btRecip (BTAROW(i)[i]); + } + + btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1); + + const int nC = m_nC; + m_C[nC] = nC; + m_nC = nC + 1; // nC value is outdated after this line + } + +} + + +void btLCP::transfer_i_from_N_to_C (int i) +{ + { + if (m_nC > 0) { + { + btScalar *const aptr = BTAROW(i); + btScalar *Dell = m_Dell; + const int *C = m_C; +# ifdef BTNUB_OPTIMIZATIONS + // if nub>0, initial part of aptr unpermuted + const int nub = m_nub; + int j=0; + for ( ; j<nub; ++j) Dell[j] = aptr[j]; + const int nC = m_nC; + for ( ; j<nC; ++j) Dell[j] = aptr[C[j]]; +# else + const int nC = m_nC; + for (int j=0; j<nC; ++j) Dell[j] = aptr[C[j]]; +# endif + } + btSolveL1 (m_L,m_Dell,m_nC,m_nskip); + { + const int nC = m_nC; + btScalar *const Ltgt = m_L + nC*m_nskip; + btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d; + for (int j=0; j<nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j]; + } + const int nC = m_nC; + m_d[nC] = btRecip (BTAROW(i)[i] - btLargeDot(m_ell,m_Dell,nC)); + } + else { + m_d[0] = btRecip (BTAROW(i)[i]); + } + + btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1); + + const int nC = m_nC; + m_C[nC] = nC; + m_nN--; + m_nC = nC + 1; // nC value is outdated after this line + } + + // @@@ TO DO LATER + // if we just finish here then we'll go back and re-solve for + // delta_x. but actually we can be more efficient and incrementally + // update delta_x here. but if we do this, we wont have ell and Dell + // to use in updating the factorization later. + +} + +void btRemoveRowCol (btScalar *A, int n, int nskip, int r) +{ + btAssert(A && n > 0 && nskip >= n && r >= 0 && r < n); + if (r >= n-1) return; + if (r > 0) { + { + const size_t move_size = (n-r-1)*sizeof(btScalar); + btScalar *Adst = A + r; + for (int i=0; i<r; Adst+=nskip,++i) { + btScalar *Asrc = Adst + 1; + memmove (Adst,Asrc,move_size); + } + } + { + const size_t cpy_size = r*sizeof(btScalar); + btScalar *Adst = A + r * nskip; + for (int i=r; i<(n-1); ++i) { + btScalar *Asrc = Adst + nskip; + memcpy (Adst,Asrc,cpy_size); + Adst = Asrc; + } + } + } + { + const size_t cpy_size = (n-r-1)*sizeof(btScalar); + btScalar *Adst = A + r * (nskip + 1); + for (int i=r; i<(n-1); ++i) { + btScalar *Asrc = Adst + (nskip + 1); + memcpy (Adst,Asrc,cpy_size); + Adst = Asrc - 1; + } + } +} + + + + +void btLDLTAddTL (btScalar *L, btScalar *d, const btScalar *a, int n, int nskip, btAlignedObjectArray<btScalar>& scratch) +{ + btAssert (L && d && a && n > 0 && nskip >= n); + + if (n < 2) return; + scratch.resize(2*nskip); + btScalar *W1 = &scratch[0]; + + btScalar *W2 = W1 + nskip; + + W1[0] = btScalar(0.0); + W2[0] = btScalar(0.0); + for (int j=1; j<n; ++j) { + W1[j] = W2[j] = (btScalar) (a[j] * SIMDSQRT12); + } + btScalar W11 = (btScalar) ((btScalar(0.5)*a[0]+1)*SIMDSQRT12); + btScalar W21 = (btScalar) ((btScalar(0.5)*a[0]-1)*SIMDSQRT12); + + btScalar alpha1 = btScalar(1.0); + btScalar alpha2 = btScalar(1.0); + + { + btScalar dee = d[0]; + btScalar alphanew = alpha1 + (W11*W11)*dee; + btAssert(alphanew != btScalar(0.0)); + dee /= alphanew; + btScalar gamma1 = W11 * dee; + dee *= alpha1; + alpha1 = alphanew; + alphanew = alpha2 - (W21*W21)*dee; + dee /= alphanew; + //btScalar gamma2 = W21 * dee; + alpha2 = alphanew; + btScalar k1 = btScalar(1.0) - W21*gamma1; + btScalar k2 = W21*gamma1*W11 - W21; + btScalar *ll = L + nskip; + for (int p=1; p<n; ll+=nskip, ++p) { + btScalar Wp = W1[p]; + btScalar ell = *ll; + W1[p] = Wp - W11*ell; + W2[p] = k1*Wp + k2*ell; + } + } + + btScalar *ll = L + (nskip + 1); + for (int j=1; j<n; ll+=nskip+1, ++j) { + btScalar k1 = W1[j]; + btScalar k2 = W2[j]; + + btScalar dee = d[j]; + btScalar alphanew = alpha1 + (k1*k1)*dee; + btAssert(alphanew != btScalar(0.0)); + dee /= alphanew; + btScalar gamma1 = k1 * dee; + dee *= alpha1; + alpha1 = alphanew; + alphanew = alpha2 - (k2*k2)*dee; + dee /= alphanew; + btScalar gamma2 = k2 * dee; + dee *= alpha2; + d[j] = dee; + alpha2 = alphanew; + + btScalar *l = ll + nskip; + for (int p=j+1; p<n; l+=nskip, ++p) { + btScalar ell = *l; + btScalar Wp = W1[p] - k1 * ell; + ell += gamma1 * Wp; + W1[p] = Wp; + Wp = W2[p] - k2 * ell; + ell -= gamma2 * Wp; + W2[p] = Wp; + *l = ell; + } + } +} + + +#define _BTGETA(i,j) (A[i][j]) +//#define _GETA(i,j) (A[(i)*nskip+(j)]) +#define BTGETA(i,j) ((i > j) ? _BTGETA(i,j) : _BTGETA(j,i)) + +inline size_t btEstimateLDLTAddTLTmpbufSize(int nskip) +{ + return nskip * 2 * sizeof(btScalar); +} + + +void btLDLTRemove (btScalar **A, const int *p, btScalar *L, btScalar *d, + int n1, int n2, int r, int nskip, btAlignedObjectArray<btScalar>& scratch) +{ + btAssert(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 && + n1 >= n2 && nskip >= n1); + #ifdef BT_DEBUG + for (int i=0; i<n2; ++i) + btAssert(p[i] >= 0 && p[i] < n1); + #endif + + if (r==n2-1) { + return; // deleting last row/col is easy + } + else { + size_t LDLTAddTL_size = btEstimateLDLTAddTLTmpbufSize(nskip); + btAssert(LDLTAddTL_size % sizeof(btScalar) == 0); + scratch.resize(nskip * 2+n2); + btScalar *tmp = &scratch[0]; + if (r==0) { + btScalar *a = (btScalar *)((char *)tmp + LDLTAddTL_size); + const int p_0 = p[0]; + for (int i=0; i<n2; ++i) { + a[i] = -BTGETA(p[i],p_0); + } + a[0] += btScalar(1.0); + btLDLTAddTL (L,d,a,n2,nskip,scratch); + } + else { + btScalar *t = (btScalar *)((char *)tmp + LDLTAddTL_size); + { + btScalar *Lcurr = L + r*nskip; + for (int i=0; i<r; ++Lcurr, ++i) { + btAssert(d[i] != btScalar(0.0)); + t[i] = *Lcurr / d[i]; + } + } + btScalar *a = t + r; + { + btScalar *Lcurr = L + r*nskip; + const int *pp_r = p + r, p_r = *pp_r; + const int n2_minus_r = n2-r; + for (int i=0; i<n2_minus_r; Lcurr+=nskip,++i) { + a[i] = btLargeDot(Lcurr,t,r) - BTGETA(pp_r[i],p_r); + } + } + a[0] += btScalar(1.0); + btLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip, scratch); + } + } + + // snip out row/column r from L and d + btRemoveRowCol (L,n2,nskip,r); + if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*sizeof(btScalar)); +} + + +void btLCP::transfer_i_from_C_to_N (int i, btAlignedObjectArray<btScalar>& scratch) +{ + { + int *C = m_C; + // remove a row/column from the factorization, and adjust the + // indexes (black magic!) + int last_idx = -1; + const int nC = m_nC; + int j = 0; + for ( ; j<nC; ++j) { + if (C[j]==nC-1) { + last_idx = j; + } + if (C[j]==i) { + btLDLTRemove (m_A,C,m_L,m_d,m_n,nC,j,m_nskip,scratch); + int k; + if (last_idx == -1) { + for (k=j+1 ; k<nC; ++k) { + if (C[k]==nC-1) { + break; + } + } + btAssert (k < nC); + } + else { + k = last_idx; + } + C[k] = C[j]; + if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int)); + break; + } + } + btAssert (j < nC); + + btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,i,nC-1,m_nskip,1); + + m_nN++; + m_nC = nC - 1; // nC value is outdated after this line + } + +} + + +void btLCP::pN_equals_ANC_times_qC (btScalar *p, btScalar *q) +{ + // we could try to make this matrix-vector multiplication faster using + // outer product matrix tricks, e.g. with the dMultidotX() functions. + // but i tried it and it actually made things slower on random 100x100 + // problems because of the overhead involved. so we'll stick with the + // simple method for now. + const int nC = m_nC; + btScalar *ptgt = p + nC; + const int nN = m_nN; + for (int i=0; i<nN; ++i) { + ptgt[i] = btLargeDot (BTAROW(i+nC),q,nC); + } +} + + +void btLCP::pN_plusequals_ANi (btScalar *p, int i, int sign) +{ + const int nC = m_nC; + btScalar *aptr = BTAROW(i) + nC; + btScalar *ptgt = p + nC; + if (sign > 0) { + const int nN = m_nN; + for (int j=0; j<nN; ++j) ptgt[j] += aptr[j]; + } + else { + const int nN = m_nN; + for (int j=0; j<nN; ++j) ptgt[j] -= aptr[j]; + } +} + +void btLCP::pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q) +{ + const int nC = m_nC; + for (int i=0; i<nC; ++i) { + p[i] += s*q[i]; + } +} + +void btLCP::pN_plusequals_s_times_qN (btScalar *p, btScalar s, btScalar *q) +{ + const int nC = m_nC; + btScalar *ptgt = p + nC, *qsrc = q + nC; + const int nN = m_nN; + for (int i=0; i<nN; ++i) { + ptgt[i] += s*qsrc[i]; + } +} + +void btLCP::solve1 (btScalar *a, int i, int dir, int only_transfer) +{ + // the `Dell' and `ell' that are computed here are saved. if index i is + // later added to the factorization then they can be reused. + // + // @@@ question: do we need to solve for entire delta_x??? yes, but + // only if an x goes below 0 during the step. + + if (m_nC > 0) { + { + btScalar *Dell = m_Dell; + int *C = m_C; + btScalar *aptr = BTAROW(i); +# ifdef BTNUB_OPTIMIZATIONS + // if nub>0, initial part of aptr[] is guaranteed unpermuted + const int nub = m_nub; + int j=0; + for ( ; j<nub; ++j) Dell[j] = aptr[j]; + const int nC = m_nC; + for ( ; j<nC; ++j) Dell[j] = aptr[C[j]]; +# else + const int nC = m_nC; + for (int j=0; j<nC; ++j) Dell[j] = aptr[C[j]]; +# endif + } + btSolveL1 (m_L,m_Dell,m_nC,m_nskip); + { + btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d; + const int nC = m_nC; + for (int j=0; j<nC; ++j) ell[j] = Dell[j] * d[j]; + } + + if (!only_transfer) { + btScalar *tmp = m_tmp, *ell = m_ell; + { + const int nC = m_nC; + for (int j=0; j<nC; ++j) tmp[j] = ell[j]; + } + btSolveL1T (m_L,tmp,m_nC,m_nskip); + if (dir > 0) { + int *C = m_C; + btScalar *tmp = m_tmp; + const int nC = m_nC; + for (int j=0; j<nC; ++j) a[C[j]] = -tmp[j]; + } else { + int *C = m_C; + btScalar *tmp = m_tmp; + const int nC = m_nC; + for (int j=0; j<nC; ++j) a[C[j]] = tmp[j]; + } + } + } +} + + +void btLCP::unpermute() +{ + // now we have to un-permute x and w + { + memcpy (m_tmp,m_x,m_n*sizeof(btScalar)); + btScalar *x = m_x, *tmp = m_tmp; + const int *p = m_p; + const int n = m_n; + for (int j=0; j<n; ++j) x[p[j]] = tmp[j]; + } + { + memcpy (m_tmp,m_w,m_n*sizeof(btScalar)); + btScalar *w = m_w, *tmp = m_tmp; + const int *p = m_p; + const int n = m_n; + for (int j=0; j<n; ++j) w[p[j]] = tmp[j]; + } +} + +#endif // btLCP_FAST + + +//*************************************************************************** +// an optimized Dantzig LCP driver routine for the lo-hi LCP problem. + +bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, + btScalar* outer_w, int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory& scratchMem) +{ + s_error = false; + +// printf("btSolveDantzigLCP n=%d\n",n); + btAssert (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n); + btAssert(outer_w); + +#ifdef BT_DEBUG + { + // check restrictions on lo and hi + for (int k=0; k<n; ++k) + btAssert (lo[k] <= 0 && hi[k] >= 0); + } +# endif + + + // if all the variables are unbounded then we can just factor, solve, + // and return + if (nub >= n) + { + + + int nskip = (n); + btFactorLDLT (A, outer_w, n, nskip); + btSolveLDLT (A, outer_w, b, n, nskip); + memcpy (x, b, n*sizeof(btScalar)); + + return !s_error; + } + + const int nskip = (n); + scratchMem.L.resize(n*nskip); + + scratchMem.d.resize(n); + + btScalar *w = outer_w; + scratchMem.delta_w.resize(n); + scratchMem.delta_x.resize(n); + scratchMem.Dell.resize(n); + scratchMem.ell.resize(n); + scratchMem.Arows.resize(n); + scratchMem.p.resize(n); + scratchMem.C.resize(n); + + // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i) + scratchMem.state.resize(n); + + + // create LCP object. note that tmp is set to delta_w to save space, this + // optimization relies on knowledge of how tmp is used, so be careful! + btLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,&scratchMem.L[0],&scratchMem.d[0],&scratchMem.Dell[0],&scratchMem.ell[0],&scratchMem.delta_w[0],&scratchMem.state[0],findex,&scratchMem.p[0],&scratchMem.C[0],&scratchMem.Arows[0]); + int adj_nub = lcp.getNub(); + + // loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the + // LCP conditions then i is added to the appropriate index set. otherwise + // x(i),w(i) is driven either +ve or -ve to force it to the valid region. + // as we drive x(i), x(C) is also adjusted to keep w(C) at zero. + // while driving x(i) we maintain the LCP conditions on the other variables + // 0..i-1. we do this by watching out for other x(i),w(i) values going + // outside the valid region, and then switching them between index sets + // when that happens. + + bool hit_first_friction_index = false; + for (int i=adj_nub; i<n; ++i) + { + s_error = false; + // the index i is the driving index and indexes i+1..n-1 are "dont care", + // i.e. when we make changes to the system those x's will be zero and we + // don't care what happens to those w's. in other words, we only consider + // an (i+1)*(i+1) sub-problem of A*x=b+w. + + // if we've hit the first friction index, we have to compute the lo and + // hi values based on the values of x already computed. we have been + // permuting the indexes, so the values stored in the findex vector are + // no longer valid. thus we have to temporarily unpermute the x vector. + // for the purposes of this computation, 0*infinity = 0 ... so if the + // contact constraint's normal force is 0, there should be no tangential + // force applied. + + if (!hit_first_friction_index && findex && findex[i] >= 0) { + // un-permute x into delta_w, which is not being used at the moment + for (int j=0; j<n; ++j) scratchMem.delta_w[scratchMem.p[j]] = x[j]; + + // set lo and hi values + for (int k=i; k<n; ++k) { + btScalar wfk = scratchMem.delta_w[findex[k]]; + if (wfk == 0) { + hi[k] = 0; + lo[k] = 0; + } + else { + hi[k] = btFabs (hi[k] * wfk); + lo[k] = -hi[k]; + } + } + hit_first_friction_index = true; + } + + // thus far we have not even been computing the w values for indexes + // greater than i, so compute w[i] now. + w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i]; + + // if lo=hi=0 (which can happen for tangential friction when normals are + // 0) then the index will be assigned to set N with some state. however, + // set C's line has zero size, so the index will always remain in set N. + // with the "normal" switching logic, if w changed sign then the index + // would have to switch to set C and then back to set N with an inverted + // state. this is pointless, and also computationally expensive. to + // prevent this from happening, we use the rule that indexes with lo=hi=0 + // will never be checked for set changes. this means that the state for + // these indexes may be incorrect, but that doesn't matter. + + // see if x(i),w(i) is in a valid region + if (lo[i]==0 && w[i] >= 0) { + lcp.transfer_i_to_N (i); + scratchMem.state[i] = false; + } + else if (hi[i]==0 && w[i] <= 0) { + lcp.transfer_i_to_N (i); + scratchMem.state[i] = true; + } + else if (w[i]==0) { + // this is a degenerate case. by the time we get to this test we know + // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve, + // and similarly that hi > 0. this means that the line segment + // corresponding to set C is at least finite in extent, and we are on it. + // NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C() + lcp.solve1 (&scratchMem.delta_x[0],i,0,1); + + lcp.transfer_i_to_C (i); + } + else { + // we must push x(i) and w(i) + for (;;) { + int dir; + btScalar dirf; + // find direction to push on x(i) + if (w[i] <= 0) { + dir = 1; + dirf = btScalar(1.0); + } + else { + dir = -1; + dirf = btScalar(-1.0); + } + + // compute: delta_x(C) = -dir*A(C,C)\A(C,i) + lcp.solve1 (&scratchMem.delta_x[0],i,dir); + + // note that delta_x[i] = dirf, but we wont bother to set it + + // compute: delta_w = A*delta_x ... note we only care about + // delta_w(N) and delta_w(i), the rest is ignored + lcp.pN_equals_ANC_times_qC (&scratchMem.delta_w[0],&scratchMem.delta_x[0]); + lcp.pN_plusequals_ANi (&scratchMem.delta_w[0],i,dir); + scratchMem.delta_w[i] = lcp.AiC_times_qC (i,&scratchMem.delta_x[0]) + lcp.Aii(i)*dirf; + + // find largest step we can take (size=s), either to drive x(i),w(i) + // to the valid LCP region or to drive an already-valid variable + // outside the valid region. + + int cmd = 1; // index switching command + int si = 0; // si = index to switch if cmd>3 + btScalar s = -w[i]/scratchMem.delta_w[i]; + if (dir > 0) { + if (hi[i] < BT_INFINITY) { + btScalar s2 = (hi[i]-x[i])*dirf; // was (hi[i]-x[i])/dirf // step to x(i)=hi(i) + if (s2 < s) { + s = s2; + cmd = 3; + } + } + } + else { + if (lo[i] > -BT_INFINITY) { + btScalar s2 = (lo[i]-x[i])*dirf; // was (lo[i]-x[i])/dirf // step to x(i)=lo(i) + if (s2 < s) { + s = s2; + cmd = 2; + } + } + } + + { + const int numN = lcp.numN(); + for (int k=0; k < numN; ++k) { + const int indexN_k = lcp.indexN(k); + if (!scratchMem.state[indexN_k] ? scratchMem.delta_w[indexN_k] < 0 : scratchMem.delta_w[indexN_k] > 0) { + // don't bother checking if lo=hi=0 + if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue; + btScalar s2 = -w[indexN_k] / scratchMem.delta_w[indexN_k]; + if (s2 < s) { + s = s2; + cmd = 4; + si = indexN_k; + } + } + } + } + + { + const int numC = lcp.numC(); + for (int k=adj_nub; k < numC; ++k) { + const int indexC_k = lcp.indexC(k); + if (scratchMem.delta_x[indexC_k] < 0 && lo[indexC_k] > -BT_INFINITY) { + btScalar s2 = (lo[indexC_k]-x[indexC_k]) / scratchMem.delta_x[indexC_k]; + if (s2 < s) { + s = s2; + cmd = 5; + si = indexC_k; + } + } + if (scratchMem.delta_x[indexC_k] > 0 && hi[indexC_k] < BT_INFINITY) { + btScalar s2 = (hi[indexC_k]-x[indexC_k]) / scratchMem.delta_x[indexC_k]; + if (s2 < s) { + s = s2; + cmd = 6; + si = indexC_k; + } + } + } + } + + //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C", + // "C->NL","C->NH"}; + //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i); + + // if s <= 0 then we've got a problem. if we just keep going then + // we're going to get stuck in an infinite loop. instead, just cross + // our fingers and exit with the current solution. + if (s <= btScalar(0.0)) + { +// printf("LCP internal error, s <= 0 (s=%.4e)",(double)s); + if (i < n) { + btSetZero (x+i,n-i); + btSetZero (w+i,n-i); + } + s_error = true; + break; + } + + // apply x = x + s * delta_x + lcp.pC_plusequals_s_times_qC (x, s, &scratchMem.delta_x[0]); + x[i] += s * dirf; + + // apply w = w + s * delta_w + lcp.pN_plusequals_s_times_qN (w, s, &scratchMem.delta_w[0]); + w[i] += s * scratchMem.delta_w[i]; + +// void *tmpbuf; + // switch indexes between sets if necessary + switch (cmd) { + case 1: // done + w[i] = 0; + lcp.transfer_i_to_C (i); + break; + case 2: // done + x[i] = lo[i]; + scratchMem.state[i] = false; + lcp.transfer_i_to_N (i); + break; + case 3: // done + x[i] = hi[i]; + scratchMem.state[i] = true; + lcp.transfer_i_to_N (i); + break; + case 4: // keep going + w[si] = 0; + lcp.transfer_i_from_N_to_C (si); + break; + case 5: // keep going + x[si] = lo[si]; + scratchMem.state[si] = false; + lcp.transfer_i_from_C_to_N (si, scratchMem.m_scratch); + break; + case 6: // keep going + x[si] = hi[si]; + scratchMem.state[si] = true; + lcp.transfer_i_from_C_to_N (si, scratchMem.m_scratch); + break; + } + + if (cmd <= 3) break; + } // for (;;) + } // else + + if (s_error) + { + break; + } + } // for (int i=adj_nub; i<n; ++i) + + lcp.unpermute(); + + + return !s_error; +} + diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h new file mode 100644 index 00000000000..903832770ae --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h @@ -0,0 +1,77 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of * + * The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i) +satisfies one of + (1) x = lo, w >= 0 + (2) x = hi, w <= 0 + (3) lo < x < hi, w = 0 +A is a matrix of dimension n*n, everything else is a vector of size n*1. +lo and hi can be +/- dInfinity as needed. the first `nub' variables are +unbounded, i.e. hi and lo are assumed to be +/- dInfinity. + +we restrict lo(i) <= 0 and hi(i) >= 0. + +the original data (A,b) may be modified by this function. + +if the `findex' (friction index) parameter is nonzero, it points to an array +of index values. in this case constraints that have findex[i] >= 0 are +special. all non-special constraints are solved for, then the lo and hi values +for the special constraints are set: + hi[i] = abs( hi[i] * x[findex[i]] ) + lo[i] = -hi[i] +and the solution continues. this mechanism allows a friction approximation +to be implemented. the first `nub' variables are assumed to have findex < 0. + +*/ + + +#ifndef _BT_LCP_H_ +#define _BT_LCP_H_ + +#include <stdlib.h> +#include <stdio.h> +#include <assert.h> + + +#include "LinearMath/btScalar.h" +#include "LinearMath/btAlignedObjectArray.h" + +struct btDantzigScratchMemory +{ + btAlignedObjectArray<btScalar> m_scratch; + btAlignedObjectArray<btScalar> L; + btAlignedObjectArray<btScalar> d; + btAlignedObjectArray<btScalar> delta_w; + btAlignedObjectArray<btScalar> delta_x; + btAlignedObjectArray<btScalar> Dell; + btAlignedObjectArray<btScalar> ell; + btAlignedObjectArray<btScalar*> Arows; + btAlignedObjectArray<int> p; + btAlignedObjectArray<int> C; + btAlignedObjectArray<bool> state; +}; + +//return false if solving failed +bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w, + int nub, btScalar *lo, btScalar *hi, int *findex,btDantzigScratchMemory& scratch); + + + +#endif //_BT_LCP_H_ diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h new file mode 100644 index 00000000000..2a2f2d3d32d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h @@ -0,0 +1,112 @@ +/* +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_DANTZIG_SOLVER_H +#define BT_DANTZIG_SOLVER_H + +#include "btMLCPSolverInterface.h" +#include "btDantzigLCP.h" + + +class btDantzigSolver : public btMLCPSolverInterface +{ +protected: + + btScalar m_acceptableUpperLimitSolution; + + btAlignedObjectArray<char> m_tempBuffer; + + btAlignedObjectArray<btScalar> m_A; + btAlignedObjectArray<btScalar> m_b; + btAlignedObjectArray<btScalar> m_x; + btAlignedObjectArray<btScalar> m_lo; + btAlignedObjectArray<btScalar> m_hi; + btAlignedObjectArray<int> m_dependencies; + btDantzigScratchMemory m_scratchMemory; +public: + + btDantzigSolver() + :m_acceptableUpperLimitSolution(btScalar(1000)) + { + } + + 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) + { + bool result = true; + int n = b.rows(); + if (n) + { + int nub = 0; + btAlignedObjectArray<btScalar> ww; + ww.resize(n); + + + const btScalar* Aptr = A.getBufferPointer(); + m_A.resize(n*n); + for (int i=0;i<n*n;i++) + { + m_A[i] = Aptr[i]; + + } + + m_b.resize(n); + m_x.resize(n); + m_lo.resize(n); + m_hi.resize(n); + m_dependencies.resize(n); + for (int i=0;i<n;i++) + { + m_lo[i] = lo[i]; + m_hi[i] = hi[i]; + m_b[i] = b[i]; + m_x[i] = x[i]; + m_dependencies[i] = limitDependency[i]; + } + + + result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0],m_scratchMemory); + if (!result) + return result; + +// printf("numAllocas = %d\n",numAllocas); + for (int i=0;i<n;i++) + { + volatile btScalar xx = m_x[i]; + if (xx != m_x[i]) + return false; + if (x[i] >= m_acceptableUpperLimitSolution) + { + return false; + } + + if (x[i] <= -m_acceptableUpperLimitSolution) + { + return false; + } + } + + for (int i=0;i<n;i++) + { + x[i] = m_x[i]; + } + + } + + return result; + } +}; + +#endif //BT_DANTZIG_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp new file mode 100644 index 00000000000..8888d3961f5 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -0,0 +1,626 @@ +/* +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 + +#include "btMLCPSolver.h" +#include "LinearMath/btMatrixX.h" +#include "LinearMath/btQuickprof.h" +#include "btSolveProjectedGaussSeidel.h" + +btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver) +:m_solver(solver), +m_fallback(0) +{ +} + +btMLCPSolver::~btMLCPSolver() +{ +} + +bool gUseMatrixMultiply = false; +bool interleaveContactAndFriction = false; + +btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies, numBodiesUnUsed, manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + + { + BT_PROFILE("gather constraint data"); + + int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2; + + + int numBodies = m_tmpSolverBodyPool.size(); + m_allConstraintArray.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()); + + int dindex = 0; + for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++) + { + m_allConstraintArray.push_back(m_tmpSolverNonContactConstraintPool[i]); + m_limitDependencies[dindex++] = -1; + } + + ///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead + + int firstContactConstraintOffset=dindex; + + if (interleaveContactAndFriction) + { + for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++) + { + m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]); + m_limitDependencies[dindex++] = -1; + m_allConstraintArray.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_limitDependencies[dindex++] = findex+firstContactConstraintOffset; + } + } + } else + { + for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++) + { + m_allConstraintArray.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_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset; + } + + } + + + if (!m_allConstraintArray.size()) + { + m_A.resize(0,0); + m_b.resize(0); + m_x.resize(0); + m_lo.resize(0); + m_hi.resize(0); + return 0.f; + } + } + + + if (gUseMatrixMultiply) + { + BT_PROFILE("createMLCP"); + createMLCP(infoGlobal); + } + else + { + BT_PROFILE("createMLCPFast"); + createMLCPFast(infoGlobal); + } + + return 0.f; +} + +bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal) +{ + bool result = true; + + if (m_A.rows()==0) + return true; + + //if using split impulse, we solve 2 separate (M)LCPs + if (infoGlobal.m_splitImpulse) + { + btMatrixXu Acopy = m_A; + btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies; +// printf("solve first LCP\n"); + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + if (result) + result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations ); + + } else + { + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + } + return result; +} + +struct btJointNode +{ + int jointIndex; // pointer to enclosing dxJoint object + int otherBodyIndex; // *other* body this joint is connected to + int nextJointNodeIndex;//-1 for null + int constraintRowIndex; +}; + + + +void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) +{ + int numContactRows = interleaveContactAndFriction ? 3 : 1; + + int numConstraintRows = m_allConstraintArray.size(); + int n = numConstraintRows; + { + BT_PROFILE("init b (rhs)"); + m_b.resize(numConstraintRows); + m_bSplit.resize(numConstraintRows); + //m_b.setZero(); + for (int i=0;i<numConstraintRows ;i++) + { + if (m_allConstraintArray[i].m_jacDiagABInv) + { + m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv; + m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv; + } + + } + } + + btScalar* w = 0; + int nub = 0; + + m_lo.resize(numConstraintRows); + m_hi.resize(numConstraintRows); + + { + BT_PROFILE("init lo/ho"); + + for (int i=0;i<numConstraintRows;i++) + { + if (0)//m_limitDependencies[i]>=0) + { + m_lo[i] = -BT_INFINITY; + m_hi[i] = BT_INFINITY; + } else + { + m_lo[i] = m_allConstraintArray[i].m_lowerLimit; + m_hi[i] = m_allConstraintArray[i].m_upperLimit; + } + } + } + + // + int m=m_allConstraintArray.size(); + + int numBodies = m_tmpSolverBodyPool.size(); + btAlignedObjectArray<int> bodyJointNodeArray; + { + BT_PROFILE("bodyJointNodeArray.resize"); + bodyJointNodeArray.resize(numBodies,-1); + } + btAlignedObjectArray<btJointNode> jointNodeArray; + { + BT_PROFILE("jointNodeArray.reserve"); + jointNodeArray.reserve(2*m_allConstraintArray.size()); + } + + static btMatrixXu J3; + { + BT_PROFILE("J3.resize"); + J3.resize(2*m,8); + } + static btMatrixXu JinvM3; + { + BT_PROFILE("JinvM3.resize/setZero"); + + JinvM3.resize(2*m,8); + JinvM3.setZero(); + J3.setZero(); + } + int cur=0; + int rowOffset = 0; + static btAlignedObjectArray<int> ofs; + { + BT_PROFILE("ofs resize"); + ofs.resize(0); + ofs.resizeNoInitialize(m_allConstraintArray.size()); + } + { + BT_PROFILE("Compute J and JinvM"); + int c=0; + + int numRows = 0; + + for (int i=0;i<m_allConstraintArray.size();i+=numRows,c++) + { + ofs[c] = rowOffset; + 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; + + numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ; + if (orgBodyA) + { + { + int slotA=-1; + //find free jointNode slot for sbA + slotA =jointNodeArray.size(); + jointNodeArray.expand();//NonInitializing(); + int prevSlot = bodyJointNodeArray[sbA]; + bodyJointNodeArray[sbA] = slotA; + jointNodeArray[slotA].nextJointNodeIndex = prevSlot; + jointNodeArray[slotA].jointIndex = c; + jointNodeArray[slotA].constraintRowIndex = i; + jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1; + } + 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(); + + 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]); + JinvM3.setElem(cur,r,normalInvMass[r]); + JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); + } + J3.setElem(cur,3,0); + JinvM3.setElem(cur,3,0); + J3.setElem(cur,7,0); + JinvM3.setElem(cur,7,0); + } + } else + { + cur += numRows; + } + if (orgBodyB) + { + + { + int slotB=-1; + //find free jointNode slot for sbA + slotB =jointNodeArray.size(); + jointNodeArray.expand();//NonInitializing(); + int prevSlot = bodyJointNodeArray[sbB]; + bodyJointNodeArray[sbB] = slotB; + jointNodeArray[slotB].nextJointNodeIndex = prevSlot; + jointNodeArray[slotB].jointIndex = c; + jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1; + jointNodeArray[slotB].constraintRowIndex = i; + } + + 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(); + + 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]); + JinvM3.setElem(cur,r,normalInvMassB[r]); + JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); + } + J3.setElem(cur,3,0); + JinvM3.setElem(cur,3,0); + J3.setElem(cur,7,0); + JinvM3.setElem(cur,7,0); + } + } + else + { + cur += numRows; + } + rowOffset+=numRows; + + } + + } + + + //compute JinvM = J*invM. + const btScalar* JinvM = JinvM3.getBufferPointer(); + + const btScalar* Jptr = J3.getBufferPointer(); + { + BT_PROFILE("m_A.resize"); + m_A.resize(n,n); + } + + { + BT_PROFILE("m_A.setZero"); + m_A.setZero(); + } + int c=0; + { + int numRows = 0; + BT_PROFILE("Compute A"); + for (int i=0;i<m_allConstraintArray.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; + + numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ; + + const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__; + + { + int startJointNodeA = bodyJointNodeArray[sbA]; + while (startJointNodeA>=0) + { + int j0 = jointNodeArray[startJointNodeA].jointIndex; + int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; + if (j0<c) + { + + int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows; + size_t ofsother = (m_allConstraintArray[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]); + } + startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; + } + } + + { + int startJointNodeB = bodyJointNodeArray[sbB]; + while (startJointNodeB>=0) + { + int j1 = jointNodeArray[startJointNodeB].jointIndex; + int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; + + 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; + m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, + Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]); + } + startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; + } + } + } + + { + BT_PROFILE("compute diagonal"); + // compute diagonal blocks of m_A + + int row__ = 0; + int numJointRows = m_allConstraintArray.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; + btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + + const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; + + const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__; + const btScalar *Jrow = Jptr + 2*8*(size_t)row__; + m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__); + if (orgBodyB) + { + m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__); + } + row__ += infom; + jj++; + } + } + } + + ///todo: use proper cfm values from the constraints (getInfo2) + if (1) + { + // add cfm to the diagonal of m_A + for ( int i=0; i<m_A.rows(); ++i) + { + float cfm = 0.00001f; + m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep); + } + } + + ///fill the upper triangle of the matrix, to make it symmetric + { + BT_PROFILE("fill the upper triangle "); + m_A.copyLowerToUpperTriangle(); + } + + { + BT_PROFILE("resize/init x"); + m_x.resize(numConstraintRows); + m_xSplit.resize(numConstraintRows); + + if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING) + { + for (int i=0;i<m_allConstraintArray.size();i++) + { + const btSolverConstraint& c = m_allConstraintArray[i]; + m_x[i]=c.m_appliedImpulse; + m_xSplit[i] = c.m_appliedPushImpulse; + } + } else + { + m_x.setZero(); + m_xSplit.setZero(); + } + } + +} + +void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) +{ + int numBodies = this->m_tmpSolverBodyPool.size(); + int numConstraintRows = m_allConstraintArray.size(); + + m_b.resize(numConstraintRows); + if (infoGlobal.m_splitImpulse) + m_bSplit.resize(numConstraintRows); + + for (int i=0;i<numConstraintRows ;i++) + { + if (m_allConstraintArray[i].m_jacDiagABInv) + { + m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv; + if (infoGlobal.m_splitImpulse) + m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv; + } + } + + static btMatrixXu Minv; + Minv.resize(6*numBodies,6*numBodies); + Minv.setZero(); + for (int i=0;i<numBodies;i++) + { + const btSolverBody& rb = m_tmpSolverBodyPool[i]; + const btVector3& invMass = rb.m_invMass; + setElem(Minv,i*6+0,i*6+0,invMass[0]); + setElem(Minv,i*6+1,i*6+1,invMass[1]); + setElem(Minv,i*6+2,i*6+2,invMass[2]); + btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody; + + for (int r=0;r<3;r++) + for (int c=0;c<3;c++) + setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0); + } + + static btMatrixXu J; + J.resize(numConstraintRows,6*numBodies); + J.setZero(); + + m_lo.resize(numConstraintRows); + m_hi.resize(numConstraintRows); + + for (int i=0;i<numConstraintRows;i++) + { + + m_lo[i] = m_allConstraintArray[i].m_lowerLimit; + m_hi[i] = m_allConstraintArray[i].m_upperLimit; + + int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA; + int bodyIndex1 = m_allConstraintArray[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]); + } + 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]); + } + } + + static btMatrixXu J_transpose; + J_transpose= J.transpose(); + + static btMatrixXu tmp; + + { + { + BT_PROFILE("J*Minv"); + tmp = J*Minv; + + } + { + BT_PROFILE("J*tmp"); + m_A = tmp*J_transpose; + } + } + + if (1) + { + ///todo: use proper cfm values from the constraints (getInfo2) + // add cfm to the diagonal of m_A + for ( int i=0; i<m_A.rows(); ++i) + { + float cfm = 0.0001f; + m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep); + } + } + + m_x.resize(numConstraintRows); + if (infoGlobal.m_splitImpulse) + m_xSplit.resize(numConstraintRows); +// m_x.setZero(); + + for (int i=0;i<m_allConstraintArray.size();i++) + { + const btSolverConstraint& c = m_allConstraintArray[i]; + m_x[i]=c.m_appliedImpulse; + if (infoGlobal.m_splitImpulse) + m_xSplit[i] = c.m_appliedPushImpulse; + } + +} + + +btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + bool result = true; + { + BT_PROFILE("solveMLCP"); +// printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols()); + result = solveMLCP(infoGlobal); + } + + //check if solution is valid, and otherwise fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations + if (result) + { + BT_PROFILE("process MLCP results"); + for (int i=0;i<m_allConstraintArray.size();i++) + { + { + btSolverConstraint& c = m_allConstraintArray[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; + + 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]); + 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]); + c.m_appliedPushImpulse = m_xSplit[i]; + } + c.m_appliedImpulse = m_x[i]; + } + } + } + else + { + m_fallback++; + btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + } + + 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 new file mode 100644 index 00000000000..a5dae2d28f6 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h @@ -0,0 +1,81 @@ +/* +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_MLCP_SOLVER_H +#define BT_MLCP_SOLVER_H + +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "LinearMath/btMatrixX.h" +#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h" + +class btMLCPSolver : public btSequentialImpulseConstraintSolver +{ + +protected: + + btMatrixXu m_A; + btVectorXu m_b; + btVectorXu m_x; + btVectorXu m_lo; + btVectorXu m_hi; + + ///when using 'split impulse' we solve two separate (M)LCPs + btVectorXu m_bSplit; + btVectorXu m_xSplit; + btVectorXu m_bSplit1; + btVectorXu m_xSplit2; + + btAlignedObjectArray<int> m_limitDependencies; + btConstraintArray m_allConstraintArray; + btMLCPSolverInterface* m_solver; + int m_fallback; + + 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); + + //return true is it solves the problem successfully + virtual bool solveMLCP(const btContactSolverInfo& infoGlobal); + +public: + + btMLCPSolver( btMLCPSolverInterface* solver); + virtual ~btMLCPSolver(); + + void setMLCPSolver(btMLCPSolverInterface* solver) + { + m_solver = solver; + } + + int getNumFallbacks() const + { + return m_fallback; + } + void setNumFallbacks(int num) + { + m_fallback = num; + } + + virtual btConstraintSolverType getSolverType() const + { + return BT_MLCP_SOLVER; + } + +}; + + +#endif //BT_MLCP_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h new file mode 100644 index 00000000000..25bb3f6d327 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h @@ -0,0 +1,33 @@ +/* +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_MLCP_SOLVER_INTERFACE_H +#define BT_MLCP_SOLVER_INTERFACE_H + +#include "LinearMath/btMatrixX.h" + +class btMLCPSolverInterface +{ +public: + virtual ~btMLCPSolverInterface() + { + } + + //return true is it solves the problem successfully + 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)=0; +}; + +#endif //BT_MLCP_SOLVER_INTERFACE_H diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h new file mode 100644 index 00000000000..9ec31a6d4e4 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h @@ -0,0 +1,151 @@ +/* +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_PATH_SOLVER_H +#define BT_PATH_SOLVER_H + +//#define BT_USE_PATH +#ifdef BT_USE_PATH + +extern "C" { +#include "PATH/SimpleLCP.h" +#include "PATH/License.h" +#include "PATH/Error_Interface.h" +}; + void __stdcall MyError(Void *data, Char *msg) +{ + printf("Path Error: %s\n",msg); +} + void __stdcall MyWarning(Void *data, Char *msg) +{ + printf("Path Warning: %s\n",msg); +} + +Error_Interface e; + + + +#include "btMLCPSolverInterface.h" +#include "Dantzig/lcp.h" + +class btPathSolver : public btMLCPSolverInterface +{ +public: + + btPathSolver() + { + License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0"); + e.error_data = 0; + e.warning = MyWarning; + e.error = MyError; + Error_SetInterface(&e); + } + + + 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) + { + MCP_Termination status; + + + int numVariables = b.rows(); + if (0==numVariables) + return true; + + /* - variables - the number of variables in the problem + - m_nnz - the number of nonzeros in the M matrix + - m_i - a vector of size m_nnz containing the row indices for M + - m_j - a vector of size m_nnz containing the column indices for M + - m_ij - a vector of size m_nnz containing the data for M + - q - a vector of size variables + - lb - a vector of size variables containing the lower bounds on x + - ub - a vector of size variables containing the upper bounds on x + */ + btAlignedObjectArray<double> values; + btAlignedObjectArray<int> rowIndices; + btAlignedObjectArray<int> colIndices; + + for (int i=0;i<A.rows();i++) + { + for (int j=0;j<A.cols();j++) + { + if (A(i,j)!=0.f) + { + //add 1, because Path starts at 1, instead of 0 + rowIndices.push_back(i+1); + colIndices.push_back(j+1); + values.push_back(A(i,j)); + } + } + } + int numNonZero = rowIndices.size(); + btAlignedObjectArray<double> zResult; + zResult.resize(numVariables); + btAlignedObjectArray<double> rhs; + btAlignedObjectArray<double> upperBounds; + btAlignedObjectArray<double> lowerBounds; + for (int i=0;i<numVariables;i++) + { + upperBounds.push_back(hi[i]); + lowerBounds.push_back(lo[i]); + rhs.push_back(-b[i]); + } + + + SimpleLCP(numVariables,numNonZero,&rowIndices[0],&colIndices[0],&values[0],&rhs[0],&lowerBounds[0],&upperBounds[0], &status, &zResult[0]); + + if (status != MCP_Solved) + { + static const char* gReturnMsgs[] = { + "Invalid return", + "MCP_Solved: The problem was solved", + "MCP_NoProgress: A stationary point was found", + "MCP_MajorIterationLimit: Major iteration limit met", + "MCP_MinorIterationLimit: Cumulative minor iteration limit met", + "MCP_TimeLimit: Ran out of time", + "MCP_UserInterrupt: Control-C, typically", + "MCP_BoundError: Problem has a bound error", + "MCP_DomainError: Could not find starting point", + "MCP_Infeasible: Problem has no solution", + "MCP_Error: An error occurred within the code", + "MCP_LicenseError: License could not be found", + "MCP_OK" + }; + + printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl; + printf("using Projected Gauss Seidel fallback\n"); + + return false; + } else + { + for (int i=0;i<numVariables;i++) + { + x[i] = zResult[i]; + //check for #NAN + if (x[i] != zResult[i]) + return false; + } + return true; + + } + + } +}; + +#endif //BT_USE_PATH + + +#endif //BT_PATH_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h new file mode 100644 index 00000000000..44fbfeadddc --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h @@ -0,0 +1,80 @@ +/* +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_SOLVE_PROJECTED_GAUSS_SEIDEL_H +#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H + + +#include "btMLCPSolverInterface.h" + +class btSolveProjectedGaussSeidel : public btMLCPSolverInterface +{ +public: + 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) + { + //A is a m-n matrix, m rows, n columns + btAssert(A.rows() == b.rows()); + + int i, j, numRows = A.rows(); + + float delta; + + for (int k = 0; k <numIterations; k++) + { + for (i = 0; i <numRows; i++) + { + delta = 0.0f; + if (useSparsity) + { + for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++) + { + int j = A.m_rowNonZeroElements1[i][h]; + if (j != i)//skip main diagonal + { + delta += A(i,j) * x[j]; + } + } + } else + { + for (j = 0; j <i; j++) + delta += A(i,j) * x[j]; + for (j = i+1; j<numRows; j++) + delta += A(i,j) * x[j]; + } + + float aDiag = A(i,i); + x [i] = (b [i] - delta) / A(i,i); + float s = 1.f; + + if (limitDependency[i]>=0) + { + s = x[limitDependency[i]]; + if (s<0) + s=1; + } + + if (x[i]<lo[i]*s) + x[i]=lo[i]*s; + if (x[i]>hi[i]*s) + x[i]=hi[i]*s; + } + } + return true; + } + +}; + +#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp index e8c8fdb6402..9429d56e05a 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp @@ -1767,7 +1767,23 @@ void btSoftBody::predictMotion(btScalar dt) { Node& n=m_nodes[i]; n.m_q = n.m_x; - n.m_v += n.m_f*n.m_im*m_sst.sdt; + btVector3 deltaV = n.m_f*n.m_im*m_sst.sdt; + { + btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement/m_sst.sdt; + for (int c=0;c<3;c++) + { + if (deltaV[c]>clampDeltaV) + { + deltaV[c] = clampDeltaV; + } + if (deltaV[c]<-clampDeltaV) + { + deltaV[c]=-clampDeltaV; + } + } + } + n.m_v += deltaV; n.m_x += n.m_v*m_sst.sdt; n.m_f = btVector3(0,0,0); } diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.h b/extern/bullet2/src/BulletSoftBody/btSoftBody.h index 2116c34f06d..ee1a3d95228 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.h @@ -45,6 +45,7 @@ struct btSoftBodyWorldInfo btScalar air_density; btScalar water_density; btScalar water_offset; + btScalar m_maxDisplacement; btVector3 water_normal; btBroadphaseInterface* m_broadphase; btDispatcher* m_dispatcher; @@ -55,6 +56,7 @@ struct btSoftBodyWorldInfo :air_density((btScalar)1.2), water_density(0), water_offset(0), + m_maxDisplacement(1000.f),//avoid soft body from 'exploding' so use some upper threshold of maximum motion that a node can travel per frame water_normal(0,0,0), m_broadphase(0), m_dispatcher(0), diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp index 0fb3560e94c..36f675a6c0c 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -911,9 +911,9 @@ btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldI &hres.m_OutputVertices[0],0); for(int i=0;i<(int)hres.mNumFaces;++i) { - const int idx[]={ hres.m_Indices[i*3+0], - hres.m_Indices[i*3+1], - hres.m_Indices[i*3+2]}; + const int idx[]={ static_cast<int>(hres.m_Indices[i*3+0]), + static_cast<int>(hres.m_Indices[i*3+1]), + static_cast<int>(hres.m_Indices[i*3+2])}; if(idx[0]<idx[1]) psb->appendLink( idx[0],idx[1]); if(idx[1]<idx[2]) psb->appendLink( idx[1],idx[2]); if(idx[2]<idx[0]) psb->appendLink( idx[2],idx[0]); diff --git a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h index 180e3c218cc..bcf0c798253 100644 --- a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h +++ b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h @@ -69,6 +69,7 @@ struct btSparseSdf btScalar voxelsz; int puid; int ncells; + int m_clampCells; int nprobes; int nqueries; @@ -77,10 +78,13 @@ struct btSparseSdf // // - void Initialize(int hashsize=2383) + void Initialize(int hashsize=2383, int clampCells = 256*1024) { + //avoid a crash due to running out of memory, so clamp the maximum number of cells allocated + //if this limit is reached, the SDF is reset (at the cost of some performance during the reset) + m_clampCells = clampCells; cells.resize(hashsize,0); - Reset(); + Reset(); } // void Reset() @@ -181,6 +185,15 @@ struct btSparseSdf { ++nprobes; ++ncells; + int sz = sizeof(Cell); + if (ncells>m_clampCells) + { + static int numResets=0; + numResets++; +// printf("numResets=%d\n",numResets); + Reset(); + } + c=new Cell(); c->next=root;root=c; c->pclient=shape; diff --git a/extern/bullet2/src/LinearMath/btIDebugDraw.h b/extern/bullet2/src/LinearMath/btIDebugDraw.h index a00d7763a75..de97c3f87fd 100644 --- a/extern/bullet2/src/LinearMath/btIDebugDraw.h +++ b/extern/bullet2/src/LinearMath/btIDebugDraw.h @@ -62,29 +62,17 @@ class btIDebugDraw virtual void drawSphere(btScalar radius, const btTransform& transform, const btVector3& color) { - btVector3 start = transform.getOrigin(); - - const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0); - const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0); - const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius); - - // XY - drawLine(start-xoffs, start+yoffs, color); - drawLine(start+yoffs, start+xoffs, color); - drawLine(start+xoffs, start-yoffs, color); - drawLine(start-yoffs, start-xoffs, color); - - // XZ - drawLine(start-xoffs, start+zoffs, color); - drawLine(start+zoffs, start+xoffs, color); - drawLine(start+xoffs, start-zoffs, color); - drawLine(start-zoffs, start-xoffs, color); - - // YZ - drawLine(start-yoffs, start+zoffs, color); - drawLine(start+zoffs, start+yoffs, color); - drawLine(start+yoffs, start-zoffs, color); - drawLine(start-zoffs, start-yoffs, color); + + btVector3 center = transform.getOrigin(); + btVector3 up = transform.getBasis().getColumn(1); + btVector3 axis = transform.getBasis().getColumn(0); + btScalar minTh = -SIMD_HALF_PI; + btScalar maxTh = SIMD_HALF_PI; + btScalar minPs = -SIMD_HALF_PI; + btScalar maxPs = SIMD_HALF_PI; + btScalar stepDegrees = 30.f; + drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees ,false); + drawSpherePatch(center, up, -axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees,false ); } virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color) @@ -179,7 +167,7 @@ class btIDebugDraw } } virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius, - btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f)) + btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f),bool drawCenter = true) { btVector3 vA[74]; btVector3 vB[74]; @@ -261,18 +249,22 @@ class btIDebugDraw { drawLine(npole, pvB[j], color); } - if(isClosed) + + if (drawCenter) { - if(j == (n_vert-1)) + if(isClosed) { - drawLine(arcStart, pvB[j], color); + if(j == (n_vert-1)) + { + drawLine(arcStart, pvB[j], color); + } } - } - else - { - if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1)))) + else { - drawLine(center, pvB[j], color); + if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1)))) + { + drawLine(center, pvB[j], color); + } } } } @@ -314,6 +306,8 @@ class btIDebugDraw virtual void drawCapsule(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color) { + int stepDegrees = 30; + btVector3 capStart(0.f,0.f,0.f); capStart[upAxis] = -halfHeight; @@ -325,34 +319,47 @@ class btIDebugDraw btTransform childTransform = transform; childTransform.getOrigin() = transform * capStart; - drawSphere(radius, childTransform, color); + { + btVector3 center = childTransform.getOrigin(); + btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3); + btVector3 axis = -childTransform.getBasis().getColumn(upAxis); + btScalar minTh = -SIMD_HALF_PI; + btScalar maxTh = SIMD_HALF_PI; + btScalar minPs = -SIMD_HALF_PI; + btScalar maxPs = SIMD_HALF_PI; + + drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false); + } + + + } { btTransform childTransform = transform; childTransform.getOrigin() = transform * capEnd; - drawSphere(radius, childTransform, color); + { + btVector3 center = childTransform.getOrigin(); + btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3); + btVector3 axis = childTransform.getBasis().getColumn(upAxis); + btScalar minTh = -SIMD_HALF_PI; + btScalar maxTh = SIMD_HALF_PI; + btScalar minPs = -SIMD_HALF_PI; + btScalar maxPs = SIMD_HALF_PI; + drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false); + } } // Draw some additional lines btVector3 start = transform.getOrigin(); - capStart[(upAxis+1)%3] = radius; - capEnd[(upAxis+1)%3] = radius; - drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); - capStart[(upAxis+1)%3] = -radius; - capEnd[(upAxis+1)%3] = -radius; - drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); - - capStart[(upAxis+1)%3] = 0.f; - capEnd[(upAxis+1)%3] = 0.f; - - capStart[(upAxis+2)%3] = radius; - capEnd[(upAxis+2)%3] = radius; - drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); - capStart[(upAxis+2)%3] = -radius; - capEnd[(upAxis+2)%3] = -radius; - drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); + for (int i=0;i<360;i+=stepDegrees) + { + capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius; + capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius; + drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); + } + } virtual void drawCylinder(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color) @@ -360,11 +367,18 @@ class btIDebugDraw btVector3 start = transform.getOrigin(); btVector3 offsetHeight(0,0,0); offsetHeight[upAxis] = halfHeight; - btVector3 offsetRadius(0,0,0); - offsetRadius[(upAxis+1)%3] = radius; - drawLine(start+transform.getBasis() * (offsetHeight+offsetRadius),start+transform.getBasis() * (-offsetHeight+offsetRadius),color); - drawLine(start+transform.getBasis() * (offsetHeight-offsetRadius),start+transform.getBasis() * (-offsetHeight-offsetRadius),color); + int stepDegrees=30; + btVector3 capStart(0.f,0.f,0.f); + capStart[upAxis] = -halfHeight; + btVector3 capEnd(0.f,0.f,0.f); + capEnd[upAxis] = halfHeight; + for (int i=0;i<360;i+=stepDegrees) + { + capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius; + capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius; + drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); + } // Drawing top and bottom caps of the cylinder btVector3 yaxis(0,0,0); yaxis[upAxis] = btScalar(1.0); @@ -376,16 +390,28 @@ class btIDebugDraw virtual void drawCone(btScalar radius, btScalar height, int upAxis, const btTransform& transform, const btVector3& color) { - + int stepDegrees = 30; btVector3 start = transform.getOrigin(); btVector3 offsetHeight(0,0,0); - offsetHeight[upAxis] = height * btScalar(0.5); + btScalar halfHeight = height * btScalar(0.5); + offsetHeight[upAxis] = halfHeight; btVector3 offsetRadius(0,0,0); offsetRadius[(upAxis+1)%3] = radius; btVector3 offset2Radius(0,0,0); offset2Radius[(upAxis+2)%3] = radius; + + btVector3 capEnd(0.f,0.f,0.f); + capEnd[upAxis] = -halfHeight; + + for (int i=0;i<360;i+=stepDegrees) + { + capEnd[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius; + capEnd[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius; + drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * capEnd, color); + } + drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offsetRadius),color); drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offsetRadius),color); drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offset2Radius),color); diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet2/src/LinearMath/btMatrix3x3.h index d4f5c95aa64..14fe704f81a 100644 --- a/extern/bullet2/src/LinearMath/btMatrix3x3.h +++ b/extern/bullet2/src/LinearMath/btMatrix3x3.h @@ -22,10 +22,15 @@ subject to the following restrictions: #ifdef BT_USE_SSE //const __m128 ATTRIBUTE_ALIGNED16(v2220) = {2.0f, 2.0f, 2.0f, 0.0f}; -const __m128 ATTRIBUTE_ALIGNED16(vMPPP) = {-0.0f, +0.0f, +0.0f, +0.0f}; +//const __m128 ATTRIBUTE_ALIGNED16(vMPPP) = {-0.0f, +0.0f, +0.0f, +0.0f}; +#define vMPPP (_mm_set_ps (+0.0f, +0.0f, +0.0f, -0.0f)) #endif -#if defined(BT_USE_SSE) || defined(BT_USE_NEON) +#if defined(BT_USE_SSE) +#define v1000 (_mm_set_ps(0.0f,0.0f,0.0f,1.0f)) +#define v0100 (_mm_set_ps(0.0f,0.0f,1.0f,0.0f)) +#define v0010 (_mm_set_ps(0.0f,1.0f,0.0f,0.0f)) +#elif defined(BT_USE_NEON) const btSimdFloat4 ATTRIBUTE_ALIGNED16(v1000) = {1.0f, 0.0f, 0.0f, 0.0f}; const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0100) = {0.0f, 1.0f, 0.0f, 0.0f}; const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0010) = {0.0f, 0.0f, 1.0f, 0.0f}; @@ -207,7 +212,7 @@ public: btFullAssert(d != btScalar(0.0)); btScalar s = btScalar(2.0) / d; - #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) + #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) __m128 vs, Q = q.get128(); __m128i Qi = btCastfTo128i(Q); __m128 Y, Z; @@ -341,7 +346,7 @@ public: * @param m The array to be filled */ void getOpenGLSubMatrix(btScalar *m) const { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) __m128 v0 = m_el[0].mVec128; __m128 v1 = m_el[1].mVec128; __m128 v2 = m_el[2].mVec128; // x2 y2 z2 w2 @@ -362,7 +367,7 @@ public: vm[2] = v2; #elif defined(BT_USE_NEON) // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions. - static const uint32x2_t zMask = (const uint32x2_t) {-1, 0 }; + static const uint32x2_t zMask = (const uint32x2_t) {static_cast<uint32_t>(-1), 0 }; float32x4_t *vm = (float32x4_t *)m; float32x4x2_t top = vtrnq_f32( m_el[0].mVec128, m_el[1].mVec128 ); // {x0 x1 z0 z1}, {y0 y1 w0 w1} float32x2x2_t bl = vtrn_f32( vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f) ); // {x2 0 }, {y2 0} @@ -740,7 +745,7 @@ public: SIMD_FORCE_INLINE btMatrix3x3& btMatrix3x3::operator*=(const btMatrix3x3& m) { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) __m128 rv00, rv01, rv02; __m128 rv10, rv11, rv12; __m128 rv20, rv21, rv22; @@ -953,7 +958,7 @@ btMatrix3x3::determinant() const SIMD_FORCE_INLINE btMatrix3x3 btMatrix3x3::absolute() const { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) +#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) return btMatrix3x3( _mm_and_ps(m_el[0].mVec128, btvAbsfMask), _mm_and_ps(m_el[1].mVec128, btvAbsfMask), @@ -974,7 +979,7 @@ btMatrix3x3::absolute() const SIMD_FORCE_INLINE btMatrix3x3 btMatrix3x3::transpose() const { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) +#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) __m128 v0 = m_el[0].mVec128; __m128 v1 = m_el[1].mVec128; __m128 v2 = m_el[2].mVec128; // x2 y2 z2 w2 @@ -993,7 +998,7 @@ btMatrix3x3::transpose() const return btMatrix3x3( v0, v1, v2 ); #elif defined(BT_USE_NEON) // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions. - static const uint32x2_t zMask = (const uint32x2_t) {-1, 0 }; + static const uint32x2_t zMask = (const uint32x2_t) {static_cast<uint32_t>(-1), 0 }; float32x4x2_t top = vtrnq_f32( m_el[0].mVec128, m_el[1].mVec128 ); // {x0 x1 z0 z1}, {y0 y1 w0 w1} float32x2x2_t bl = vtrn_f32( vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f) ); // {x2 0 }, {y2 0} float32x4_t v0 = vcombine_f32( vget_low_f32(top.val[0]), bl.val[0] ); @@ -1031,7 +1036,7 @@ btMatrix3x3::inverse() const SIMD_FORCE_INLINE btMatrix3x3 btMatrix3x3::transposeTimes(const btMatrix3x3& m) const { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) +#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) // zeros w // static const __m128i xyzMask = (const __m128i){ -1ULL, 0xffffffffULL }; __m128 row = m_el[0].mVec128; @@ -1053,7 +1058,7 @@ btMatrix3x3::transposeTimes(const btMatrix3x3& m) const #elif defined BT_USE_NEON // zeros w - static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 }; + static const uint32x4_t xyzMask = (const uint32x4_t){ static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), 0 }; float32x4_t m0 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(0).mVec128, xyzMask ); float32x4_t m1 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(1).mVec128, xyzMask ); float32x4_t m2 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(2).mVec128, xyzMask ); @@ -1151,7 +1156,7 @@ operator*(const btMatrix3x3& m, const btVector3& v) SIMD_FORCE_INLINE btVector3 operator*(const btVector3& v, const btMatrix3x3& m) { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) +#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) const __m128 vv = v.mVec128; @@ -1191,7 +1196,7 @@ operator*(const btVector3& v, const btMatrix3x3& m) SIMD_FORCE_INLINE btMatrix3x3 operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) +#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) __m128 m10 = m1[0].mVec128; __m128 m11 = m1[1].mVec128; diff --git a/extern/bullet2/src/LinearMath/btMatrixX.h b/extern/bullet2/src/LinearMath/btMatrixX.h new file mode 100644 index 00000000000..1c29632c536 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btMatrixX.h @@ -0,0 +1,504 @@ +/* +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_MATRIX_X_H +#define BT_MATRIX_X_H + +#include "LinearMath/btQuickprof.h" +#include "LinearMath/btAlignedObjectArray.h" + +class btIntSortPredicate +{ + public: + bool operator() ( const int& a, const int& b ) const + { + return a < b; + } +}; + + +template <typename T> +struct btMatrixX +{ + int m_rows; + int m_cols; + int m_operations; + int m_resizeOperations; + int m_setElemOperations; + + btAlignedObjectArray<T> m_storage; + btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1; + btAlignedObjectArray< btAlignedObjectArray<int> > m_colNonZeroElements; + + T* getBufferPointerWritable() + { + return m_storage.size() ? &m_storage[0] : 0; + } + + const T* getBufferPointer() const + { + return m_storage.size() ? &m_storage[0] : 0; + } + btMatrixX() + :m_rows(0), + m_cols(0), + m_operations(0), + m_resizeOperations(0), + m_setElemOperations(0) + { + } + btMatrixX(int rows,int cols) + :m_rows(rows), + m_cols(cols), + m_operations(0), + m_resizeOperations(0), + m_setElemOperations(0) + { + resize(rows,cols); + } + void resize(int rows, int cols) + { + m_resizeOperations++; + m_rows = rows; + m_cols = cols; + { + BT_PROFILE("m_storage.resize"); + m_storage.resize(rows*cols); + } + clearSparseInfo(); + } + int cols() const + { + return m_cols; + } + int rows() const + { + return m_rows; + } + ///we don't want this read/write operator(), because we cannot keep track of non-zero elements, use setElem instead + /*T& operator() (int row,int col) + { + return m_storage[col*m_rows+row]; + } + */ + + void addElem(int row,int col, T val) + { + if (val) + { + if (m_storage[col+row*m_cols]==0.f) + { + setElem(row,col,val); + } else + { + m_storage[row*m_cols+col] += val; + } + } + } + + void copyLowerToUpperTriangle() + { + int count=0; + for (int row=0;row<m_rowNonZeroElements1.size();row++) + { + for (int j=0;j<m_rowNonZeroElements1[row].size();j++) + { + int col = m_rowNonZeroElements1[row][j]; + setElem(col,row, (*this)(row,col)); + count++; + + } + } + //printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows()); + } + void setElem(int row,int col, T val) + { + m_setElemOperations++; + if (val) + { + if (m_storage[col+row*m_cols]==0.f) + { + m_rowNonZeroElements1[row].push_back(col); + m_colNonZeroElements[col].push_back(row); + } + m_storage[row*m_cols+col] = val; + } + } + const T& operator() (int row,int col) const + { + return m_storage[col+row*m_cols]; + } + + void clearSparseInfo() + { + BT_PROFILE("clearSparseInfo=0"); + m_rowNonZeroElements1.resize(m_rows); + m_colNonZeroElements.resize(m_cols); + for (int i=0;i<m_rows;i++) + m_rowNonZeroElements1[i].resize(0); + for (int j=0;j<m_cols;j++) + m_colNonZeroElements[j].resize(0); + } + + void setZero() + { + { + BT_PROFILE("storage=0"); + btSetZero(&m_storage[0],m_storage.size()); + //memset(&m_storage[0],0,sizeof(T)*m_storage.size()); + //for (int i=0;i<m_storage.size();i++) + // m_storage[i]=0; + } + { + BT_PROFILE("clearSparseInfo=0"); + clearSparseInfo(); + } + } + + void printMatrix(const char* msg) + { + printf("%s ---------------------\n",msg); + for (int i=0;i<rows();i++) + { + printf("\n"); + for (int j=0;j<cols();j++) + { + printf("%2.1f\t",(*this)(i,j)); + } + } + printf("\n---------------------\n"); + + } + void printNumZeros(const char* msg) + { + printf("%s: ",msg); + int numZeros = 0; + for (int i=0;i<m_storage.size();i++) + if (m_storage[i]==0) + numZeros++; + int total = m_cols*m_rows; + int computedNonZero = total-numZeros; + int nonZero = 0; + for (int i=0;i<m_colNonZeroElements.size();i++) + nonZero += m_colNonZeroElements[i].size(); + btAssert(computedNonZero==nonZero); + if(computedNonZero!=nonZero) + { + printf("Error: computedNonZero=%d, but nonZero=%d\n",computedNonZero,nonZero); + } + //printf("%d numZeros out of %d (%f)\n",numZeros,m_cols*m_rows,numZeros/(m_cols*m_rows)); + printf("total %d, %d rows, %d cols, %d non-zeros (%f %)\n", total, rows(),cols(), nonZero,100.f*(T)nonZero/T(total)); + } + /* + void rowComputeNonZeroElements() + { + m_rowNonZeroElements1.resize(rows()); + for (int i=0;i<rows();i++) + { + m_rowNonZeroElements1[i].resize(0); + for (int j=0;j<cols();j++) + { + if ((*this)(i,j)!=0.f) + { + m_rowNonZeroElements1[i].push_back(j); + } + } + } + } + */ + btMatrixX transpose() const + { + //transpose is optimized for sparse matrices + btMatrixX tr(m_cols,m_rows); + tr.setZero(); +#if 0 + for (int i=0;i<m_cols;i++) + for (int j=0;j<m_rows;j++) + { + T v = (*this)(j,i); + if (v) + { + tr.setElem(i,j,v); + } + } +#else + for (int i=0;i<m_colNonZeroElements.size();i++) + for (int h=0;h<m_colNonZeroElements[i].size();h++) + { + int j = m_colNonZeroElements[i][h]; + T v = (*this)(j,i); + tr.setElem(i,j,v); + } +#endif + return tr; + } + + void sortRowIndexArrays() + { + for (int i=0;i<m_rowNonZeroElements1[i].size();i++) + { + m_rowNonZeroElements1[i].quickSort(btIntSortPredicate()); + } + } + + void sortColIndexArrays() + { + for (int i=0;i<m_colNonZeroElements[i].size();i++) + { + m_colNonZeroElements[i].quickSort(btIntSortPredicate()); + } + } + + btMatrixX operator*(const btMatrixX& other) + { + //btMatrixX*btMatrixX implementation, optimized for sparse matrices + btAssert(cols() == other.rows()); + + btMatrixX res(rows(),other.cols()); + res.setZero(); +// BT_PROFILE("btMatrixX mul"); + for (int j=0; j < res.cols(); ++j) + { + //int numZero=other.m_colNonZeroElements[j].size(); + //if (numZero) + { + for (int i=0; i < res.rows(); ++i) + //for (int g = 0;g<m_colNonZeroElements[j].size();g++) + { + T dotProd=0; + T dotProd2=0; + int waste=0,waste2=0; + + bool doubleWalk = false; + if (doubleWalk) + { + int numRows = m_rowNonZeroElements1[i].size(); + int numOtherCols = other.m_colNonZeroElements[j].size(); + for (int ii=0;ii<numRows;ii++) + { + int vThis=m_rowNonZeroElements1[i][ii]; + } + + for (int ii=0;ii<numOtherCols;ii++) + { + int vOther = other.m_colNonZeroElements[j][ii]; + } + + + int indexRow = 0; + int indexOtherCol = 0; + while (indexRow < numRows && indexOtherCol < numOtherCols) + { + int vThis=m_rowNonZeroElements1[i][indexRow]; + int vOther = other.m_colNonZeroElements[j][indexOtherCol]; + if (vOther==vThis) + { + dotProd += (*this)(i,vThis) * other(vThis,j); + } + if (vThis<vOther) + { + indexRow++; + } else + { + indexOtherCol++; + } + } + + } else + { + bool useOtherCol = true; + if (other.m_colNonZeroElements[j].size() <m_rowNonZeroElements1[i].size()) + { + useOtherCol=true; + } + if (!useOtherCol ) + { + for (int q=0;q<other.m_colNonZeroElements[j].size();q++) + { + int v = other.m_colNonZeroElements[j][q]; + T w = (*this)(i,v); + if (w!=0.f) + { + dotProd+=w*other(v,j); + } + + } + } + else + { + for (int q=0;q<m_rowNonZeroElements1[i].size();q++) + { + int v=m_rowNonZeroElements1[i][q]; + T w = (*this)(i,v); + if (other(v,j)!=0.f) + { + dotProd+=w*other(v,j); + } + + } + } + } + if (dotProd) + res.setElem(i,j,dotProd); + } + } + } + return res; + } + + // this assumes the 4th and 8th rows of B and C are zero. + void multiplyAdd2_p8r (const btScalar *B, const btScalar *C, int numRows, int numRowsOther ,int row, int col) + { + const btScalar *bb = B; + for ( int i = 0;i<numRows;i++) + { + const btScalar *cc = C; + for ( int j = 0;j<numRowsOther;j++) + { + btScalar sum; + sum = bb[0]*cc[0]; + sum += bb[1]*cc[1]; + sum += bb[2]*cc[2]; + sum += bb[4]*cc[4]; + sum += bb[5]*cc[5]; + sum += bb[6]*cc[6]; + addElem(row+i,col+j,sum); + cc += 8; + } + bb += 8; + } + } + + void multiply2_p8r (const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col) + { + btAssert (numRows>0 && numRowsOther>0 && B && C); + const btScalar *bb = B; + for ( int i = 0;i<numRows;i++) + { + const btScalar *cc = C; + for ( int j = 0;j<numRowsOther;j++) + { + btScalar sum; + sum = bb[0]*cc[0]; + sum += bb[1]*cc[1]; + sum += bb[2]*cc[2]; + sum += bb[4]*cc[4]; + sum += bb[5]*cc[5]; + sum += bb[6]*cc[6]; + setElem(row+i,col+j,sum); + cc += 8; + } + bb += 8; + } + } + +}; + +template <typename T> +struct btVectorX +{ + btAlignedObjectArray<T> m_storage; + + btVectorX() + { + } + btVectorX(int numRows) + { + m_storage.resize(numRows); + } + + void resize(int rows) + { + m_storage.resize(rows); + } + int cols() const + { + return 1; + } + int rows() const + { + return m_storage.size(); + } + int size() const + { + return rows(); + } + void setZero() + { + // for (int i=0;i<m_storage.size();i++) + // m_storage[i]=0; + //memset(&m_storage[0],0,sizeof(T)*m_storage.size()); + btSetZero(&m_storage[0],m_storage.size()); + } + const T& operator[] (int index) const + { + return m_storage[index]; + } + + T& operator[] (int index) + { + return m_storage[index]; + } + + T* getBufferPointerWritable() + { + return m_storage.size() ? &m_storage[0] : 0; + } + + const T* getBufferPointer() const + { + return m_storage.size() ? &m_storage[0] : 0; + } + +}; +/* +template <typename T> +void setElem(btMatrixX<T>& mat, int row, int col, T val) +{ + mat.setElem(row,col,val); +} +*/ + + +typedef btMatrixX<float> btMatrixXf; +typedef btVectorX<float> btVectorXf; + +typedef btMatrixX<double> btMatrixXd; +typedef btVectorX<double> btVectorXd; + + + +inline void setElem(btMatrixXd& mat, int row, int col, double val) +{ + mat.setElem(row,col,val); +} + +inline void setElem(btMatrixXf& mat, int row, int col, float val) +{ + mat.setElem(row,col,val); +} + +#ifdef BT_USE_DOUBLE_PRECISION + #define btVectorXu btVectorXd + #define btMatrixXu btMatrixXd +#else + #define btVectorXu btVectorXf + #define btMatrixXu btMatrixXf +#endif //BT_USE_DOUBLE_PRECISION + + + +#endif//BT_MATRIX_H_H diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet2/src/LinearMath/btQuaternion.h index 7d7f25fb4d3..665421de1e4 100644 --- a/extern/bullet2/src/LinearMath/btQuaternion.h +++ b/extern/bullet2/src/LinearMath/btQuaternion.h @@ -27,11 +27,17 @@ subject to the following restrictions: #ifdef BT_USE_SSE -const __m128 ATTRIBUTE_ALIGNED16(vOnes) = {1.0f, 1.0f, 1.0f, 1.0f}; +//const __m128 ATTRIBUTE_ALIGNED16(vOnes) = {1.0f, 1.0f, 1.0f, 1.0f}; +#define vOnes (_mm_set_ps(1.0f, 1.0f, 1.0f, 1.0f)) #endif -#if defined(BT_USE_SSE) || defined(BT_USE_NEON) +#if defined(BT_USE_SSE) + +#define vQInv (_mm_set_ps(+0.0f, -0.0f, -0.0f, -0.0f)) +#define vPPPM (_mm_set_ps(-0.0f, +0.0f, +0.0f, +0.0f)) + +#elif defined(BT_USE_NEON) const btSimdFloat4 ATTRIBUTE_ALIGNED16(vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f}; const btSimdFloat4 ATTRIBUTE_ALIGNED16(vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f}; @@ -285,7 +291,7 @@ public: * @param q The other quaternion */ btScalar dot(const btQuaternion& q) const { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) __m128 vd; vd = _mm_mul_ps(mVec128, q.mVec128); @@ -384,7 +390,7 @@ public: { return *this / length(); } - /**@brief Return the angle between this quaternion and the other + /**@brief Return the ***half*** angle between this quaternion and the other * @param q The other quaternion */ btScalar angle(const btQuaternion& q) const { @@ -392,6 +398,19 @@ public: btAssert(s != btScalar(0.0)); return btAcos(dot(q) / s); } + + /**@brief Return the angle between this quaternion and the other along the shortest path + * @param q The other quaternion */ + btScalar angleShortestPath(const btQuaternion& q) const + { + btScalar s = btSqrt(length2() * q.length2()); + btAssert(s != btScalar(0.0)); + if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp + return btAcos(dot(-q) / s) * btScalar(2.0); + else + return btAcos(dot(q) / s) * btScalar(2.0); + } + /**@brief Return the angle of rotation represented by this quaternion */ btScalar getAngle() const { @@ -399,6 +418,19 @@ public: return s; } + /**@brief Return the angle of rotation represented by this quaternion along the shortest path*/ + btScalar getAngleShortestPath() const + { + btScalar s; + if (dot(*this) < 0) + s = btScalar(2.) * btAcos(m_floats[3]); + else + s = btScalar(2.) * btAcos(-m_floats[3]); + + return s; + } + + /**@brief Return the axis of the rotation represented by this quaternion */ btVector3 getAxis() const { @@ -835,7 +867,7 @@ quatRotate(const btQuaternion& rotation, const btVector3& v) { btQuaternion q = rotation * v; q *= rotation.inverse(); -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) return btVector3(_mm_and_ps(q.get128(), btvFFF0fMask)); #elif defined(BT_USE_NEON) return btVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), btvFFF0Mask)); diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index aaa1d6de6b4..37c6dec191d 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 281 +#define BT_BULLET_VERSION 282 inline int btGetVersion() { @@ -180,7 +180,7 @@ inline int btGetVersion() #include <emmintrin.h> #endif #endif //BT_USE_SSE - #elif defined( __armv7__ ) + #elif defined( __ARM_NEON__ ) #ifdef __clang__ #define BT_USE_NEON 1 #define BT_USE_SIMD_VECTOR3 @@ -257,10 +257,12 @@ inline int btGetVersion() ///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision. #if defined(BT_USE_DOUBLE_PRECISION) + typedef double btScalar; //this number could be bigger in double precision #define BT_LARGE_FLOAT 1e30 #else + typedef float btScalar; //keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX #define BT_LARGE_FLOAT 1e18f @@ -284,6 +286,8 @@ static int btInfinityMask = 0x7F800000; #define BT_INFINITY (*(float*)&btInfinityMask) #endif +//use this, in case there are clashes (such as xnamath.h) +#ifndef BT_NO_SIMD_OPERATOR_OVERLOADS inline __m128 operator + (const __m128 A, const __m128 B) { return _mm_add_ps(A, B); @@ -298,6 +302,7 @@ inline __m128 operator * (const __m128 A, const __m128 B) { return _mm_mul_ps(A, B); } +#endif //BT_NO_SIMD_OPERATOR_OVERLOADS #define btCastfTo128i(a) (_mm_castps_si128(a)) #define btCastfTo128d(a) (_mm_castps_pd(a)) @@ -317,7 +322,24 @@ inline __m128 operator * (const __m128 A, const __m128 B) #define BT_INFINITY INFINITY #define BT_NAN NAN #endif//_WIN32 -#endif //BT_USE_SSE_IN_API +#else + +#ifdef BT_USE_NEON + #include <arm_neon.h> + + typedef float32x4_t btSimdFloat4; + #define BT_INFINITY INFINITY + #define BT_NAN NAN + #define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3} +#else//BT_USE_NEON + + #ifndef BT_INFINITY + static int btInfinityMask = 0x7F800000; + #define BT_INFINITY (*(float*)&btInfinityMask) + #endif +#endif//BT_USE_NEON + +#endif //BT_USE_SSE #ifdef BT_USE_NEON #include <arm_neon.h> @@ -409,15 +431,15 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); } #endif -#define SIMD_2_PI btScalar(6.283185307179586232) -#define SIMD_PI (SIMD_2_PI * btScalar(0.5)) -#define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25)) +#define SIMD_PI btScalar(3.1415926535897932384626433832795029) +#define SIMD_2_PI btScalar(2.0) * SIMD_PI +#define SIMD_HALF_PI (SIMD_PI * btScalar(0.5)) #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0)) #define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI) #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) #define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */ - +#define btRecip(x) (btScalar(1.0)/btScalar(x)) #ifdef BT_USE_DOUBLE_PRECISION #define SIMD_EPSILON DBL_EPSILON @@ -608,6 +630,46 @@ SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) return d; } +template<typename T> +SIMD_FORCE_INLINE void btSetZero(T* a, int n) +{ + T* acurr = a; + size_t ncurr = n; + while (ncurr > 0) + { + *(acurr++) = 0; + --ncurr; + } +} + + +SIMD_FORCE_INLINE btScalar btLargeDot(const btScalar *a, const btScalar *b, int n) +{ + btScalar p0,q0,m0,p1,q1,m1,sum; + sum = 0; + n -= 2; + while (n >= 0) { + p0 = a[0]; q0 = b[0]; + m0 = p0 * q0; + p1 = a[1]; q1 = b[1]; + m1 = p1 * q1; + sum += m0; + sum += m1; + a += 2; + b += 2; + n -= 2; + } + n += 2; + while (n > 0) { + sum += (*a) * (*b); + a++; + b++; + n--; + } + return sum; +} + + // returns normalized value in range [-SIMD_PI, SIMD_PI] SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) { @@ -626,6 +688,8 @@ SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) } } + + ///rudimentary class to provide type info struct btTypedObject { diff --git a/extern/bullet2/src/LinearMath/btSerializer.cpp b/extern/bullet2/src/LinearMath/btSerializer.cpp index d6b2b3a5a5c..ba344939530 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(63),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(69),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), @@ -58,238 +58,242 @@ char(102),char(111),char(114),char(109),char(0),char(42),char(109),char(95),char char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104), char(105),char(108),char(100),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104), char(97),char(112),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(104),char(105),char(108),char(100),char(83),char(104),char(97), -char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115), -char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86),char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101), -char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86), -char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108), -char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101),char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118), -char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65), -char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105), -char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0), -char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114),char(116),char(101),char(120),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(101),char(100),char(103),char(101),char(68),char(105),char(115),char(116),char(97),char(110),char(99),char(101),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(122),char(101),char(114),char(111),char(65),char(114),char(101),char(97),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(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95), -char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86), -char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(103),char(105), -char(109),char(112),char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99), -char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42), -char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98), -char(108),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80), -char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0), -char(42),char(109),char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104),char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0), -char(42),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(0),char(42),char(109), -char(95),char(114),char(111),char(111),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(0), -char(109),char(95),char(119),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(105), -char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97), +char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(117),char(112),char(73),char(110),char(100), +char(101),char(120),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86), +char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103), +char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42), +char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101), +char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80), +char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95), +char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110), +char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114), +char(116),char(101),char(120),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(101),char(100),char(103),char(101),char(68), +char(105),char(115),char(116),char(97),char(110),char(99),char(101),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(122), +char(101),char(114),char(111),char(65),char(114),char(101),char(97),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(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83), +char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117), +char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(103),char(105),char(109),char(112),char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121), +char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115), +char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100), +char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117), +char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97), +char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0),char(42),char(109),char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104), +char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0),char(42),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(0),char(42),char(109),char(95),char(114),char(111),char(111),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(0),char(109),char(95),char(119),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(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), -char(111),char(110),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105), -char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),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(105),char(115),char(111),char(116),char(114),char(111),char(112),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(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80), -char(114),char(111),char(99),char(101),char(115),char(115),char(105),char(110),char(103),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(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95), -char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(114),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(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110), -char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(99),char(100), -char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114),char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(109),char(95), -char(99),char(99),char(100),char(77),char(111),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(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),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(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),char(103),char(115), -char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84),char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112), -char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110), -char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112), -char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104), -char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118), -char(105),char(116),char(121),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(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101), -char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),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(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(70),char(97),char(99),char(116),char(111),char(114), -char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114), -char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101),char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109), -char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116), -char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114), -char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101),char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95), -char(108),char(105),char(110),char(101),char(97),char(114),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(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111), -char(110),char(97),char(108),char(68),char(97),char(109),char(112),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(97), -char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105), -char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100), -char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110), -char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105), -char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),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(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112), -char(105),char(110),char(103),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(97),char(110),char(103),char(117),char(108), -char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0), -char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0), -char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65),char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95), -char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),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(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),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(73),char(100),char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101), -char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117), -char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68),char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95), -char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116), -char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100),char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111), -char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109),char(83),char(111),char(108),char(118),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(98),char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112), -char(117),char(108),char(115),char(101),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(105),char(115),char(69),char(110), -char(97),char(98),char(108),char(101),char(100),char(0),char(109),char(95),char(116),char(121),char(112),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(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109),char(95), -char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101),char(0), -char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102),char(101), -char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97), -char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108),char(97), -char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),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(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(73), -char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0), -char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105),char(116), -char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116),char(111), -char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111),char(114), -char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110), -char(103),char(83),char(112),char(97),char(110),char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109), -char(95),char(100),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(85),char(112),char(112), -char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101), -char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101), -char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101), -char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101), -char(102),char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79), -char(102),char(102),char(115),char(101),char(116),char(70),char(111),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(114), -char(97),char(109),char(101),char(0),char(109),char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114), -char(105),char(110),char(103),char(69),char(110),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(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(84),char(89),char(80),char(69),char(76),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(97),char(112),char(115),char(117),char(108), +char(111),char(110),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(105), +char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101), +char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), +char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),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(105),char(115),char(111),char(116),char(114),char(111),char(112),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(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(114),char(111),char(99),char(101),char(115),char(115),char(105),char(110),char(103),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(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116), +char(105),char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109), +char(95),char(114),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(0),char(109),char(95),char(114), +char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99), +char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(99),char(100),char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114), +char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(99),char(100),char(77),char(111),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(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116), +char(114),char(111),char(112),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(99),char(111),char(108),char(108), +char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84), +char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95), +char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105), +char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67), +char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73), +char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),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(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110), +char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0), +char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),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(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(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70), +char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101), +char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105), +char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0), +char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101), +char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),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(68),char(97),char(109),char(112),char(105),char(110),char(103), +char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76), +char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), +char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103), +char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100), +char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117), +char(108),char(97),char(114),char(68),char(97),char(109),char(112),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(108), +char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),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(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103), +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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110), +char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116), +char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65), +char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101), +char(0),char(109),char(95),char(117),char(115),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(84),char(121),char(112), +char(101),char(0),char(109),char(95),char(117),char(115),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(73),char(100), +char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97), +char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68), +char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108), +char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100), +char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109), +char(83),char(111),char(108),char(118),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(98), +char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108),char(115),char(101),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(105),char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100), +char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112),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(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109), +char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101), +char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102), +char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), +char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108), +char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),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(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114), +char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116), +char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105), +char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116), +char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111), +char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119), +char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110), +char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(100),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(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109), +char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105), +char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105), +char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105), +char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110), +char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116), +char(70),char(111),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(114),char(97),char(109),char(101),char(0),char(109), +char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110), +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), @@ -305,155 +309,193 @@ char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),ch 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(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(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(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(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(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(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(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(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(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(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(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(0),char(0),char(84),char(0), -char(116),char(0),char(92),char(1),char(-36),char(0),char(-44),char(0),char(-4),char(0),char(92),char(1),char(-52),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(83),char(84),char(82),char(67), -char(65),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(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(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(77),char(0),char(0),char(0),char(37),char(0),char(43),char(0),char(4),char(0),char(4),char(0),char(78),char(0), -char(7),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(37),char(0),char(14),char(0),char(4),char(0),char(82),char(0), -char(4),char(0),char(83),char(0),char(43),char(0),char(84),char(0),char(4),char(0),char(85),char(0),char(7),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(4),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(0),char(0),char(37),char(0),char(44),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(95),char(0),char(45),char(0),char(5),char(0), -char(27),char(0),char(47),char(0),char(13),char(0),char(96),char(0),char(14),char(0),char(97),char(0),char(4),char(0),char(98),char(0),char(0),char(0),char(99),char(0), -char(46),char(0),char(25),char(0),char(9),char(0),char(100),char(0),char(9),char(0),char(101),char(0),char(25),char(0),char(102),char(0),char(0),char(0),char(35),char(0), -char(18),char(0),char(103),char(0),char(18),char(0),char(104),char(0),char(14),char(0),char(105),char(0),char(14),char(0),char(106),char(0),char(14),char(0),char(107),char(0), -char(8),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(4),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(0),char(0),char(37),char(0),char(47),char(0),char(25),char(0),char(9),char(0),char(100),char(0),char(9),char(0),char(101),char(0),char(25),char(0),char(102),char(0), -char(0),char(0),char(35),char(0),char(17),char(0),char(103),char(0),char(17),char(0),char(104),char(0),char(13),char(0),char(105),char(0),char(13),char(0),char(106),char(0), -char(13),char(0),char(107),char(0),char(7),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(4),char(0),char(116),char(0), +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(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(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(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(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(0),char(0),char(37),char(0),char(48),char(0),char(2),char(0),char(49),char(0),char(123),char(0),char(14),char(0),char(124),char(0), -char(50),char(0),char(2),char(0),char(51),char(0),char(123),char(0),char(13),char(0),char(124),char(0),char(52),char(0),char(21),char(0),char(47),char(0),char(125),char(0), -char(15),char(0),char(126),char(0),char(13),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(124),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(7),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(4),char(0),char(-112),char(0), -char(53),char(0),char(22),char(0),char(46),char(0),char(125),char(0),char(16),char(0),char(126),char(0),char(14),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(124),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(8),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(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(2),char(0),char(4),char(0),char(-111),char(0), -char(4),char(0),char(-110),char(0),char(55),char(0),char(13),char(0),char(56),char(0),char(-109),char(0),char(56),char(0),char(-108),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(-107),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(7),char(0),char(-103),char(0), -char(7),char(0),char(-102),char(0),char(4),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(7),char(0),char(-99),char(0),char(4),char(0),char(-98),char(0), -char(57),char(0),char(3),char(0),char(55),char(0),char(-97),char(0),char(13),char(0),char(-96),char(0),char(13),char(0),char(-95),char(0),char(58),char(0),char(3),char(0), -char(55),char(0),char(-97),char(0),char(14),char(0),char(-96),char(0),char(14),char(0),char(-95),char(0),char(59),char(0),char(13),char(0),char(55),char(0),char(-97),char(0), -char(18),char(0),char(-94),char(0),char(18),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(4),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0), -char(7),char(0),char(-89),char(0),char(7),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(60),char(0),char(13),char(0),char(55),char(0),char(-97),char(0),char(17),char(0),char(-94),char(0), -char(17),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(4),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(7),char(0),char(-89),char(0), -char(7),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(61),char(0),char(11),char(0),char(55),char(0),char(-97),char(0),char(17),char(0),char(-94),char(0),char(17),char(0),char(-93),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),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(-79),char(0),char(0),char(0),char(21),char(0),char(62),char(0),char(9),char(0),char(55),char(0),char(-97),char(0), -char(17),char(0),char(-94),char(0),char(17),char(0),char(-93),char(0),char(13),char(0),char(-78),char(0),char(13),char(0),char(-77),char(0),char(13),char(0),char(-76),char(0), -char(13),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),char(63),char(0),char(5),char(0),char(62),char(0),char(-72),char(0), -char(4),char(0),char(-71),char(0),char(7),char(0),char(-70),char(0),char(7),char(0),char(-69),char(0),char(7),char(0),char(-68),char(0),char(64),char(0),char(9),char(0), -char(55),char(0),char(-97),char(0),char(17),char(0),char(-94),char(0),char(17),char(0),char(-93),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0), -char(7),char(0),char(-76),char(0),char(7),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),char(49),char(0),char(22),char(0), -char(8),char(0),char(-67),char(0),char(8),char(0),char(-79),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(-66),char(0),char(8),char(0),char(112),char(0), -char(8),char(0),char(-65),char(0),char(8),char(0),char(-64),char(0),char(8),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(8),char(0),char(-61),char(0), -char(8),char(0),char(-60),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(4),char(0),char(-54),char(0),char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0), -char(4),char(0),char(-50),char(0),char(0),char(0),char(37),char(0),char(51),char(0),char(22),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-79),char(0), -char(7),char(0),char(110),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0), -char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),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(4),char(0),char(-54),char(0), -char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(0),char(0),char(37),char(0), -char(65),char(0),char(4),char(0),char(7),char(0),char(-49),char(0),char(7),char(0),char(-48),char(0),char(7),char(0),char(-47),char(0),char(4),char(0),char(78),char(0), -char(66),char(0),char(10),char(0),char(65),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(7),char(0),char(-121),char(0),char(7),char(0),char(-40),char(0),char(4),char(0),char(-39),char(0), -char(4),char(0),char(53),char(0),char(67),char(0),char(4),char(0),char(65),char(0),char(-46),char(0),char(4),char(0),char(-38),char(0),char(7),char(0),char(-37),char(0), -char(4),char(0),char(-36),char(0),char(68),char(0),char(4),char(0),char(13),char(0),char(-41),char(0),char(65),char(0),char(-46),char(0),char(4),char(0),char(-35),char(0), -char(7),char(0),char(-34),char(0),char(69),char(0),char(7),char(0),char(13),char(0),char(-33),char(0),char(65),char(0),char(-46),char(0),char(4),char(0),char(-32),char(0), -char(7),char(0),char(-31),char(0),char(7),char(0),char(-30),char(0),char(7),char(0),char(-29),char(0),char(4),char(0),char(53),char(0),char(70),char(0),char(6),char(0), -char(15),char(0),char(-28),char(0),char(13),char(0),char(-30),char(0),char(13),char(0),char(-27),char(0),char(56),char(0),char(-26),char(0),char(4),char(0),char(-25),char(0), -char(7),char(0),char(-29),char(0),char(71),char(0),char(26),char(0),char(4),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-79),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(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(4),char(0),char(-4),char(0),char(4),char(0),char(-3),char(0), -char(4),char(0),char(-2),char(0),char(4),char(0),char(-1),char(0),char(4),char(0),char(117),char(0),char(72),char(0),char(12),char(0),char(15),char(0),char(0),char(1), -char(15),char(0),char(1),char(1),char(15),char(0),char(2),char(1),char(13),char(0),char(3),char(1),char(13),char(0),char(4),char(1),char(7),char(0),char(5),char(1), -char(4),char(0),char(6),char(1),char(4),char(0),char(7),char(1),char(4),char(0),char(8),char(1),char(4),char(0),char(9),char(1),char(7),char(0),char(-31),char(0), -char(4),char(0),char(53),char(0),char(73),char(0),char(27),char(0),char(17),char(0),char(10),char(1),char(15),char(0),char(11),char(1),char(15),char(0),char(12),char(1), -char(13),char(0),char(3),char(1),char(13),char(0),char(13),char(1),char(13),char(0),char(14),char(1),char(13),char(0),char(15),char(1),char(13),char(0),char(16),char(1), -char(13),char(0),char(17),char(1),char(4),char(0),char(18),char(1),char(7),char(0),char(19),char(1),char(4),char(0),char(20),char(1),char(4),char(0),char(21),char(1), -char(4),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1),char(4),char(0),char(25),char(1),char(4),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(4),char(0),char(33),char(1),char(4),char(0),char(34),char(1),char(4),char(0),char(35),char(1),char(74),char(0),char(12),char(0), -char(9),char(0),char(36),char(1),char(9),char(0),char(37),char(1),char(13),char(0),char(38),char(1),char(7),char(0),char(39),char(1),char(7),char(0),char(-63),char(0), -char(7),char(0),char(40),char(1),char(4),char(0),char(41),char(1),char(13),char(0),char(42),char(1),char(4),char(0),char(43),char(1),char(4),char(0),char(44),char(1), -char(4),char(0),char(45),char(1),char(4),char(0),char(53),char(0),char(75),char(0),char(19),char(0),char(47),char(0),char(125),char(0),char(72),char(0),char(46),char(1), -char(65),char(0),char(47),char(1),char(66),char(0),char(48),char(1),char(67),char(0),char(49),char(1),char(68),char(0),char(50),char(1),char(69),char(0),char(51),char(1), -char(70),char(0),char(52),char(1),char(73),char(0),char(53),char(1),char(74),char(0),char(54),char(1),char(4),char(0),char(55),char(1),char(4),char(0),char(21),char(1), -char(4),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(4),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(71),char(0),char(62),char(1),}; +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(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(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(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(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(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(-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(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(-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(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(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), +}; int sBulletDNAlen= sizeof(sBulletDNAstr); + char sBulletDNAstr64[]= { -char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(63),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(69),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), @@ -512,238 +554,242 @@ char(102),char(111),char(114),char(109),char(0),char(42),char(109),char(95),char char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104), char(105),char(108),char(100),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104), char(97),char(112),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(104),char(105),char(108),char(100),char(83),char(104),char(97), -char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115), -char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86),char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101), -char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86), -char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108), -char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101),char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118), -char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65), -char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105), -char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0), -char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114),char(116),char(101),char(120),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(101),char(100),char(103),char(101),char(68),char(105),char(115),char(116),char(97),char(110),char(99),char(101),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(122),char(101),char(114),char(111),char(65),char(114),char(101),char(97),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(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95), -char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86), -char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(103),char(105), -char(109),char(112),char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99), -char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42), -char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98), -char(108),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80), -char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0), -char(42),char(109),char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104),char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0), -char(42),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(0),char(42),char(109), -char(95),char(114),char(111),char(111),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(0), -char(109),char(95),char(119),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(105), -char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97), +char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(117),char(112),char(73),char(110),char(100), +char(101),char(120),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86), +char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103), +char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42), +char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101), +char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80), +char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95), +char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110), +char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114), +char(116),char(101),char(120),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(101),char(100),char(103),char(101),char(68), +char(105),char(115),char(116),char(97),char(110),char(99),char(101),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(122), +char(101),char(114),char(111),char(65),char(114),char(101),char(97),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(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83), +char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117), +char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(103),char(105),char(109),char(112),char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121), +char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115), +char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100), +char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117), +char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97), +char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0),char(42),char(109),char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104), +char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0),char(42),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(0),char(42),char(109),char(95),char(114),char(111),char(111),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(0),char(109),char(95),char(119),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(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), -char(111),char(110),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105), -char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),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(105),char(115),char(111),char(116),char(114),char(111),char(112),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(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80), -char(114),char(111),char(99),char(101),char(115),char(115),char(105),char(110),char(103),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(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95), -char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(114),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(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110), -char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(99),char(100), -char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114),char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(109),char(95), -char(99),char(99),char(100),char(77),char(111),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(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),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(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),char(103),char(115), -char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84),char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112), -char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110), -char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112), -char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104), -char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118), -char(105),char(116),char(121),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(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101), -char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),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(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(70),char(97),char(99),char(116),char(111),char(114), -char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114), -char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101),char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109), -char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116), -char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114), -char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101),char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95), -char(108),char(105),char(110),char(101),char(97),char(114),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(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111), -char(110),char(97),char(108),char(68),char(97),char(109),char(112),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(97), -char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105), -char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100), -char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110), -char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105), -char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),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(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112), -char(105),char(110),char(103),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(97),char(110),char(103),char(117),char(108), -char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0), -char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0), -char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65),char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95), -char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),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(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),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(73),char(100),char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101), -char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117), -char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68),char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95), -char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116), -char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100),char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111), -char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109),char(83),char(111),char(108),char(118),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(98),char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112), -char(117),char(108),char(115),char(101),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(105),char(115),char(69),char(110), -char(97),char(98),char(108),char(101),char(100),char(0),char(109),char(95),char(116),char(121),char(112),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(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109),char(95), -char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101),char(0), -char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102),char(101), -char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97), -char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108),char(97), -char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),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(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(73), -char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0), -char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105),char(116), -char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116),char(111), -char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111),char(114), -char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110), -char(103),char(83),char(112),char(97),char(110),char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109), -char(95),char(100),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(85),char(112),char(112), -char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101), -char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101), -char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101), -char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101), -char(102),char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79), -char(102),char(102),char(115),char(101),char(116),char(70),char(111),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(114), -char(97),char(109),char(101),char(0),char(109),char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114), -char(105),char(110),char(103),char(69),char(110),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(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(84),char(89),char(80),char(69),char(76),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(97),char(112),char(115),char(117),char(108), +char(111),char(110),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(105), +char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101), +char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), +char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),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(105),char(115),char(111),char(116),char(114),char(111),char(112),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(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(114),char(111),char(99),char(101),char(115),char(115),char(105),char(110),char(103),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(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116), +char(105),char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109), +char(95),char(114),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(0),char(109),char(95),char(114), +char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99), +char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(99),char(100),char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114), +char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(99),char(100),char(77),char(111),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(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116), +char(114),char(111),char(112),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(99),char(111),char(108),char(108), +char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84), +char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95), +char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105), +char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67), +char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73), +char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),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(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110), +char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0), +char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),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(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(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70), +char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101), +char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105), +char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0), +char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101), +char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),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(68),char(97),char(109),char(112),char(105),char(110),char(103), +char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76), +char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), +char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103), +char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100), +char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117), +char(108),char(97),char(114),char(68),char(97),char(109),char(112),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(108), +char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),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(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103), +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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110), +char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116), +char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65), +char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101), +char(0),char(109),char(95),char(117),char(115),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(84),char(121),char(112), +char(101),char(0),char(109),char(95),char(117),char(115),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(73),char(100), +char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97), +char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68), +char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108), +char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100), +char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109), +char(83),char(111),char(108),char(118),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(98), +char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108),char(115),char(101),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(105),char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100), +char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112),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(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109), +char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101), +char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102), +char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), +char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108), +char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),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(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114), +char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116), +char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105), +char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116), +char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111), +char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119), +char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110), +char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(100),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(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109), +char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105), +char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105), +char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105), +char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110), +char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116), +char(70),char(111),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(114),char(97),char(109),char(101),char(0),char(109), +char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110), +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), @@ -759,150 +805,187 @@ char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),ch 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(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(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(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(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(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(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(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(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(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(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(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(0),char(0),char(96),char(0), -char(-128),char(0),char(104),char(1),char(-24),char(0),char(-32),char(0),char(8),char(1),char(104),char(1),char(-40),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(83),char(84),char(82),char(67), -char(65),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(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(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(77),char(0),char(0),char(0),char(37),char(0),char(43),char(0),char(4),char(0),char(4),char(0),char(78),char(0), -char(7),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(37),char(0),char(14),char(0),char(4),char(0),char(82),char(0), -char(4),char(0),char(83),char(0),char(43),char(0),char(84),char(0),char(4),char(0),char(85),char(0),char(7),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(4),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(0),char(0),char(37),char(0),char(44),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(95),char(0),char(45),char(0),char(5),char(0), -char(27),char(0),char(47),char(0),char(13),char(0),char(96),char(0),char(14),char(0),char(97),char(0),char(4),char(0),char(98),char(0),char(0),char(0),char(99),char(0), -char(46),char(0),char(25),char(0),char(9),char(0),char(100),char(0),char(9),char(0),char(101),char(0),char(25),char(0),char(102),char(0),char(0),char(0),char(35),char(0), -char(18),char(0),char(103),char(0),char(18),char(0),char(104),char(0),char(14),char(0),char(105),char(0),char(14),char(0),char(106),char(0),char(14),char(0),char(107),char(0), -char(8),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(4),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(0),char(0),char(37),char(0),char(47),char(0),char(25),char(0),char(9),char(0),char(100),char(0),char(9),char(0),char(101),char(0),char(25),char(0),char(102),char(0), -char(0),char(0),char(35),char(0),char(17),char(0),char(103),char(0),char(17),char(0),char(104),char(0),char(13),char(0),char(105),char(0),char(13),char(0),char(106),char(0), -char(13),char(0),char(107),char(0),char(7),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(4),char(0),char(116),char(0), +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(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(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(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(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(0),char(0),char(37),char(0),char(48),char(0),char(2),char(0),char(49),char(0),char(123),char(0),char(14),char(0),char(124),char(0), -char(50),char(0),char(2),char(0),char(51),char(0),char(123),char(0),char(13),char(0),char(124),char(0),char(52),char(0),char(21),char(0),char(47),char(0),char(125),char(0), -char(15),char(0),char(126),char(0),char(13),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(124),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(7),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(4),char(0),char(-112),char(0), -char(53),char(0),char(22),char(0),char(46),char(0),char(125),char(0),char(16),char(0),char(126),char(0),char(14),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(124),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(8),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(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(2),char(0),char(4),char(0),char(-111),char(0), -char(4),char(0),char(-110),char(0),char(55),char(0),char(13),char(0),char(56),char(0),char(-109),char(0),char(56),char(0),char(-108),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(-107),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(7),char(0),char(-103),char(0), -char(7),char(0),char(-102),char(0),char(4),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(7),char(0),char(-99),char(0),char(4),char(0),char(-98),char(0), -char(57),char(0),char(3),char(0),char(55),char(0),char(-97),char(0),char(13),char(0),char(-96),char(0),char(13),char(0),char(-95),char(0),char(58),char(0),char(3),char(0), -char(55),char(0),char(-97),char(0),char(14),char(0),char(-96),char(0),char(14),char(0),char(-95),char(0),char(59),char(0),char(13),char(0),char(55),char(0),char(-97),char(0), -char(18),char(0),char(-94),char(0),char(18),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(4),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0), -char(7),char(0),char(-89),char(0),char(7),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(60),char(0),char(13),char(0),char(55),char(0),char(-97),char(0),char(17),char(0),char(-94),char(0), -char(17),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(4),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(7),char(0),char(-89),char(0), -char(7),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(61),char(0),char(11),char(0),char(55),char(0),char(-97),char(0),char(17),char(0),char(-94),char(0),char(17),char(0),char(-93),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),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(-79),char(0),char(0),char(0),char(21),char(0),char(62),char(0),char(9),char(0),char(55),char(0),char(-97),char(0), -char(17),char(0),char(-94),char(0),char(17),char(0),char(-93),char(0),char(13),char(0),char(-78),char(0),char(13),char(0),char(-77),char(0),char(13),char(0),char(-76),char(0), -char(13),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),char(63),char(0),char(5),char(0),char(62),char(0),char(-72),char(0), -char(4),char(0),char(-71),char(0),char(7),char(0),char(-70),char(0),char(7),char(0),char(-69),char(0),char(7),char(0),char(-68),char(0),char(64),char(0),char(9),char(0), -char(55),char(0),char(-97),char(0),char(17),char(0),char(-94),char(0),char(17),char(0),char(-93),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0), -char(7),char(0),char(-76),char(0),char(7),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),char(49),char(0),char(22),char(0), -char(8),char(0),char(-67),char(0),char(8),char(0),char(-79),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(-66),char(0),char(8),char(0),char(112),char(0), -char(8),char(0),char(-65),char(0),char(8),char(0),char(-64),char(0),char(8),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(8),char(0),char(-61),char(0), -char(8),char(0),char(-60),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(4),char(0),char(-54),char(0),char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0), -char(4),char(0),char(-50),char(0),char(0),char(0),char(37),char(0),char(51),char(0),char(22),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-79),char(0), -char(7),char(0),char(110),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0), -char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),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(4),char(0),char(-54),char(0), -char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(0),char(0),char(37),char(0), -char(65),char(0),char(4),char(0),char(7),char(0),char(-49),char(0),char(7),char(0),char(-48),char(0),char(7),char(0),char(-47),char(0),char(4),char(0),char(78),char(0), -char(66),char(0),char(10),char(0),char(65),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(7),char(0),char(-121),char(0),char(7),char(0),char(-40),char(0),char(4),char(0),char(-39),char(0), -char(4),char(0),char(53),char(0),char(67),char(0),char(4),char(0),char(65),char(0),char(-46),char(0),char(4),char(0),char(-38),char(0),char(7),char(0),char(-37),char(0), -char(4),char(0),char(-36),char(0),char(68),char(0),char(4),char(0),char(13),char(0),char(-41),char(0),char(65),char(0),char(-46),char(0),char(4),char(0),char(-35),char(0), -char(7),char(0),char(-34),char(0),char(69),char(0),char(7),char(0),char(13),char(0),char(-33),char(0),char(65),char(0),char(-46),char(0),char(4),char(0),char(-32),char(0), -char(7),char(0),char(-31),char(0),char(7),char(0),char(-30),char(0),char(7),char(0),char(-29),char(0),char(4),char(0),char(53),char(0),char(70),char(0),char(6),char(0), -char(15),char(0),char(-28),char(0),char(13),char(0),char(-30),char(0),char(13),char(0),char(-27),char(0),char(56),char(0),char(-26),char(0),char(4),char(0),char(-25),char(0), -char(7),char(0),char(-29),char(0),char(71),char(0),char(26),char(0),char(4),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-79),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(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(4),char(0),char(-4),char(0),char(4),char(0),char(-3),char(0), -char(4),char(0),char(-2),char(0),char(4),char(0),char(-1),char(0),char(4),char(0),char(117),char(0),char(72),char(0),char(12),char(0),char(15),char(0),char(0),char(1), -char(15),char(0),char(1),char(1),char(15),char(0),char(2),char(1),char(13),char(0),char(3),char(1),char(13),char(0),char(4),char(1),char(7),char(0),char(5),char(1), -char(4),char(0),char(6),char(1),char(4),char(0),char(7),char(1),char(4),char(0),char(8),char(1),char(4),char(0),char(9),char(1),char(7),char(0),char(-31),char(0), -char(4),char(0),char(53),char(0),char(73),char(0),char(27),char(0),char(17),char(0),char(10),char(1),char(15),char(0),char(11),char(1),char(15),char(0),char(12),char(1), -char(13),char(0),char(3),char(1),char(13),char(0),char(13),char(1),char(13),char(0),char(14),char(1),char(13),char(0),char(15),char(1),char(13),char(0),char(16),char(1), -char(13),char(0),char(17),char(1),char(4),char(0),char(18),char(1),char(7),char(0),char(19),char(1),char(4),char(0),char(20),char(1),char(4),char(0),char(21),char(1), -char(4),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1),char(4),char(0),char(25),char(1),char(4),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(4),char(0),char(33),char(1),char(4),char(0),char(34),char(1),char(4),char(0),char(35),char(1),char(74),char(0),char(12),char(0), -char(9),char(0),char(36),char(1),char(9),char(0),char(37),char(1),char(13),char(0),char(38),char(1),char(7),char(0),char(39),char(1),char(7),char(0),char(-63),char(0), -char(7),char(0),char(40),char(1),char(4),char(0),char(41),char(1),char(13),char(0),char(42),char(1),char(4),char(0),char(43),char(1),char(4),char(0),char(44),char(1), -char(4),char(0),char(45),char(1),char(4),char(0),char(53),char(0),char(75),char(0),char(19),char(0),char(47),char(0),char(125),char(0),char(72),char(0),char(46),char(1), -char(65),char(0),char(47),char(1),char(66),char(0),char(48),char(1),char(67),char(0),char(49),char(1),char(68),char(0),char(50),char(1),char(69),char(0),char(51),char(1), -char(70),char(0),char(52),char(1),char(73),char(0),char(53),char(1),char(74),char(0),char(54),char(1),char(4),char(0),char(55),char(1),char(4),char(0),char(21),char(1), -char(4),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(4),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(71),char(0),char(62),char(1),}; +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(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(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(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(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(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(-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(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(-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(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(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), +}; int sBulletDNAlen64= sizeof(sBulletDNAstr64); diff --git a/extern/bullet2/src/LinearMath/btSerializer.h b/extern/bullet2/src/LinearMath/btSerializer.h index c5bc96b7839..ff1dc574c5d 100644 --- a/extern/bullet2/src/LinearMath/btSerializer.h +++ b/extern/bullet2/src/LinearMath/btSerializer.h @@ -17,7 +17,6 @@ subject to the following restrictions: #define BT_SERIALIZER_H #include "btScalar.h" // has definitions like SIMD_FORCE_INLINE -#include "btStackAlloc.h" #include "btHashMap.h" #if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) @@ -439,7 +438,7 @@ public: buffer[9] = '2'; buffer[10] = '8'; - buffer[11] = '1'; + buffer[11] = '2'; } diff --git a/extern/bullet2/src/LinearMath/btVector3.cpp b/extern/bullet2/src/LinearMath/btVector3.cpp index bcb18704b93..a84f7baee2e 100644 --- a/extern/bullet2/src/LinearMath/btVector3.cpp +++ b/extern/bullet2/src/LinearMath/btVector3.cpp @@ -51,7 +51,7 @@ long _maxdot_large( const float *vv, const float *vec, unsigned long count, floa long _maxdot_large( const float *vv, const float *vec, unsigned long count, float *dotResult ) { const float4 *vertices = (const float4*) vv; - static const unsigned char indexTable[16] = {-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 }; + static const unsigned char indexTable[16] = {(unsigned char)-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 }; float4 dotMax = btAssign128( -BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_INFINITY ); float4 vvec = _mm_loadu_ps( vec ); float4 vHi = btCastiTo128f(_mm_shuffle_epi32( btCastfTo128i( vvec), 0xaa )); /// zzzz @@ -436,7 +436,7 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa long _mindot_large( const float *vv, const float *vec, unsigned long count, float *dotResult ) { const float4 *vertices = (const float4*) vv; - static const unsigned char indexTable[16] = {-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 }; + static const unsigned char indexTable[16] = {(unsigned char)-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 }; float4 dotmin = btAssign128( BT_INFINITY, BT_INFINITY, BT_INFINITY, BT_INFINITY ); float4 vvec = _mm_loadu_ps( vec ); float4 vHi = btCastiTo128f(_mm_shuffle_epi32( btCastfTo128i( vvec), 0xaa )); /// zzzz @@ -823,7 +823,8 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa #elif defined BT_USE_NEON #define ARM_NEON_GCC_COMPATIBILITY 1 #include <arm_neon.h> - +#include <sys/types.h> +#include <sys/sysctl.h> //for sysctlbyname static long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult ); static long _maxdot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult ); @@ -835,11 +836,34 @@ static long _mindot_large_sel( const float *vv, const float *vec, unsigned long long (*_maxdot_large)( const float *vv, const float *vec, unsigned long count, float *dotResult ) = _maxdot_large_sel; long (*_mindot_large)( const float *vv, const float *vec, unsigned long count, float *dotResult ) = _mindot_large_sel; -extern "C" {int _get_cpu_capabilities( void );} + +static inline uint32_t btGetCpuCapabilities( void ) +{ + static uint32_t capabilities = 0; + static bool testedCapabilities = false; + + if( 0 == testedCapabilities) + { + 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 |= 0x2000; + + testedCapabilities = true; + } + + return capabilities; +} + + + static long _maxdot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult ) { - if( _get_cpu_capabilities() & 0x2000 ) + + if( btGetCpuCapabilities() & 0x2000 ) _maxdot_large = _maxdot_large_v1; else _maxdot_large = _maxdot_large_v0; @@ -849,7 +873,8 @@ static long _maxdot_large_sel( const float *vv, const float *vec, unsigned long static long _mindot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult ) { - if( _get_cpu_capabilities() & 0x2000 ) + + if( btGetCpuCapabilities() & 0x2000 ) _mindot_large = _mindot_large_v1; else _mindot_large = _mindot_large_v0; @@ -872,8 +897,8 @@ long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, f float32x2_t dotMaxHi = (float32x2_t) { -BT_INFINITY, -BT_INFINITY }; uint32x2_t indexLo = (uint32x2_t) {0, 1}; uint32x2_t indexHi = (uint32x2_t) {2, 3}; - uint32x2_t iLo = (uint32x2_t) {-1, -1}; - uint32x2_t iHi = (uint32x2_t) {-1, -1}; + uint32x2_t iLo = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)}; + uint32x2_t iHi = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)}; const uint32x2_t four = (uint32x2_t) {4,4}; for( ; i+8 <= count; i+= 8 ) @@ -1059,7 +1084,7 @@ long _maxdot_large_v1( const float *vv, const float *vec, unsigned long count, f float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0); const uint32x4_t four = (uint32x4_t){ 4, 4, 4, 4 }; uint32x4_t local_index = (uint32x4_t) {0, 1, 2, 3}; - uint32x4_t index = (uint32x4_t) { -1, -1, -1, -1 }; + uint32x4_t index = (uint32x4_t) { static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1) }; float32x4_t maxDot = (float32x4_t) { -BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_INFINITY }; unsigned long i = 0; @@ -1257,8 +1282,8 @@ long _mindot_large_v0( const float *vv, const float *vec, unsigned long count, f float32x2_t dotMinHi = (float32x2_t) { BT_INFINITY, BT_INFINITY }; uint32x2_t indexLo = (uint32x2_t) {0, 1}; uint32x2_t indexHi = (uint32x2_t) {2, 3}; - uint32x2_t iLo = (uint32x2_t) {-1, -1}; - uint32x2_t iHi = (uint32x2_t) {-1, -1}; + uint32x2_t iLo = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)}; + uint32x2_t iHi = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)}; const uint32x2_t four = (uint32x2_t) {4,4}; for( ; i+8 <= count; i+= 8 ) @@ -1442,7 +1467,7 @@ long _mindot_large_v1( const float *vv, const float *vec, unsigned long count, f float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0); const uint32x4_t four = (uint32x4_t){ 4, 4, 4, 4 }; uint32x4_t local_index = (uint32x4_t) {0, 1, 2, 3}; - uint32x4_t index = (uint32x4_t) { -1, -1, -1, -1 }; + uint32x4_t index = (uint32x4_t) { static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1) }; float32x4_t minDot = (float32x4_t) { BT_INFINITY, BT_INFINITY, BT_INFINITY, BT_INFINITY }; unsigned long i = 0; diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h index 1cf65358803..89685929288 100644 --- a/extern/bullet2/src/LinearMath/btVector3.h +++ b/extern/bullet2/src/LinearMath/btVector3.h @@ -53,19 +53,24 @@ subject to the following restrictions: #define btvxyzMaskf btvFFF0fMask #define btvAbsfMask btCastiTo128f(btvAbsMask) +//there is an issue with XCode 3.2 (LCx errors) +#define btvMzeroMask (_mm_set_ps(-0.0f, -0.0f, -0.0f, -0.0f)) +#define v1110 (_mm_set_ps(0.0f, 1.0f, 1.0f, 1.0f)) +#define vHalf (_mm_set_ps(0.5f, 0.5f, 0.5f, 0.5f)) +#define v1_5 (_mm_set_ps(1.5f, 1.5f, 1.5f, 1.5f)) - -const __m128 ATTRIBUTE_ALIGNED16(btvMzeroMask) = {-0.0f, -0.0f, -0.0f, -0.0f}; -const __m128 ATTRIBUTE_ALIGNED16(v1110) = {1.0f, 1.0f, 1.0f, 0.0f}; -const __m128 ATTRIBUTE_ALIGNED16(vHalf) = {0.5f, 0.5f, 0.5f, 0.5f}; -const __m128 ATTRIBUTE_ALIGNED16(v1_5) = {1.5f, 1.5f, 1.5f, 1.5f}; +//const __m128 ATTRIBUTE_ALIGNED16(btvMzeroMask) = {-0.0f, -0.0f, -0.0f, -0.0f}; +//const __m128 ATTRIBUTE_ALIGNED16(v1110) = {1.0f, 1.0f, 1.0f, 0.0f}; +//const __m128 ATTRIBUTE_ALIGNED16(vHalf) = {0.5f, 0.5f, 0.5f, 0.5f}; +//const __m128 ATTRIBUTE_ALIGNED16(v1_5) = {1.5f, 1.5f, 1.5f, 1.5f}; #endif #ifdef BT_USE_NEON const float32x4_t ATTRIBUTE_ALIGNED16(btvMzeroMask) = (float32x4_t){-0.0f, -0.0f, -0.0f, -0.0f}; -const int32x4_t ATTRIBUTE_ALIGNED16(btvFFF0Mask) = (int32x4_t){0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x0}; +const int32x4_t ATTRIBUTE_ALIGNED16(btvFFF0Mask) = (int32x4_t){static_cast<int32_t>(0xFFFFFFFF), + static_cast<int32_t>(0xFFFFFFFF), static_cast<int32_t>(0xFFFFFFFF), 0x0}; const int32x4_t ATTRIBUTE_ALIGNED16(btvAbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF}; const int32x4_t ATTRIBUTE_ALIGNED16(btv3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x0}; @@ -260,6 +265,12 @@ public: return btSqrt(length2()); } + /**@brief Return the norm (length) of the vector */ + SIMD_FORCE_INLINE btScalar norm() const + { + return length(); + } + /**@brief Return the distance squared between the ends of this and another vector * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const; @@ -285,6 +296,9 @@ public: * x^2 + y^2 + z^2 = 1 */ SIMD_FORCE_INLINE btVector3& normalize() { + + btAssert(length() != btScalar(0)); + #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) // dot product first __m128 vd = _mm_mul_ps(mVec128, mVec128); @@ -718,7 +732,7 @@ public: return btVector3(r); #elif defined(BT_USE_NEON) - static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 }; + static const uint32x4_t xyzMask = (const uint32x4_t){ static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), 0 }; float32x4_t a0 = vmulq_f32( v0.mVec128, this->mVec128); float32x4_t a1 = vmulq_f32( v1.mVec128, this->mVec128); float32x4_t a2 = vmulq_f32( v2.mVec128, this->mVec128); @@ -936,13 +950,9 @@ SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const SIMD_FORCE_INLINE btVector3 btVector3::normalized() const { -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) btVector3 norm = *this; return norm.normalize(); -#else - return *this / length(); -#endif } SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const diff --git a/extern/bullet2/src/SConscript b/extern/bullet2/src/SConscript index 3ffff9c80fc..ff2e86affb4 100644 --- a/extern/bullet2/src/SConscript +++ b/extern/bullet2/src/SConscript @@ -31,6 +31,8 @@ bullet2_src += env.Glob("BulletDynamics/Vehicle/*.cpp") bullet2_src += env.Glob("BulletDynamics/ConstraintSolver/*.cpp") bullet2_src += env.Glob("BulletDynamics/Dynamics/*.cpp") bullet2_src += env.Glob("BulletDynamics/Character/*.cpp") +bullet2_src += env.Glob("BulletDynamics/Featherstone/*.cpp") +bullet2_src += env.Glob("BulletDynamics/MLCPSolvers/*.cpp") bullet2_src += env.Glob("BulletSoftBody/*.cpp") incs = '. BulletCollision BulletDynamics LinearMath BulletSoftBody' diff --git a/extern/bullet2/src/btBulletDynamicsCommon.h b/extern/bullet2/src/btBulletDynamicsCommon.h index dbd175c3fe6..50282bf2105 100644 --- a/extern/bullet2/src/btBulletDynamicsCommon.h +++ b/extern/bullet2/src/btBulletDynamicsCommon.h @@ -33,6 +33,8 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btUniversalConstraint.h" #include "BulletDynamics/ConstraintSolver/btHinge2Constraint.h" #include "BulletDynamics/ConstraintSolver/btGearConstraint.h" +#include "BulletDynamics/ConstraintSolver/btFixedConstraint.h" + #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" |