diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 61ee33dbe..e6ff2130a 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -82,7 +82,9 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) //test for Bullet 2.74: use a relative contact breaking threshold without clamping against 'gContactBreakingThreshold' //btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold())); btScalar contactBreakingThreshold = btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()); - + + btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold()); + void* mem = 0; if (m_persistentManifoldPoolAllocator->getFreeCount()) @@ -93,7 +95,7 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) mem = btAlignedAlloc(sizeof(btPersistentManifold),16); } - btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold); + btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold); manifold->m_index1a = m_manifoldsPtr.size(); m_manifoldsPtr.push_back(manifold); diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index eebd0c99f..9b39dd7c1 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -17,7 +17,10 @@ subject to the following restrictions: #include "btCollisionObject.h" btCollisionObject::btCollisionObject() - : m_broadphaseHandle(0), + : m_anisotropicFriction(1.f,1.f,1.f), + m_hasAnisotropicFriction(false), + m_contactProcessingThreshold(1e30f), + m_broadphaseHandle(0), m_collisionShape(0), m_rootCollisionShape(0), m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT), diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 4fe361f97..0d5b78864 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -52,6 +52,11 @@ protected: //without destroying the continuous interpolated motion (which uses this interpolation velocities) btVector3 m_interpolationLinearVelocity; btVector3 m_interpolationAngularVelocity; + + btVector3 m_anisotropicFriction; + bool m_hasAnisotropicFriction; + btScalar m_contactProcessingThreshold; + btBroadphaseProxy* m_broadphaseHandle; btCollisionShape* m_collisionShape; @@ -127,6 +132,30 @@ public: return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0); } + const btVector3& getAnisotropicFriction() const + { + return m_anisotropicFriction; + } + void setAnisotropicFriction(const btVector3& anisotropicFriction) + { + m_anisotropicFriction = anisotropicFriction; + m_hasAnisotropicFriction = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f); + } + bool hasAnisotropicFriction() const + { + return m_hasAnisotropicFriction; + } + + ///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default. + ///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges + void setContactProcessingThreshold( btScalar contactProcessingThreshold) + { + m_contactProcessingThreshold = contactProcessingThreshold; + } + btScalar getContactProcessingThreshold() const + { + return m_contactProcessingThreshold; + } SIMD_FORCE_INLINE bool isStaticObject() const { return (m_collisionFlags & CF_STATIC_OBJECT) != 0; diff --git a/src/BulletCollision/CollisionShapes/btSphereShape.h b/src/BulletCollision/CollisionShapes/btSphereShape.h index b5b9015d6..bf163d3b5 100644 --- a/src/BulletCollision/CollisionShapes/btSphereShape.h +++ b/src/BulletCollision/CollisionShapes/btSphereShape.h @@ -47,6 +47,12 @@ public: btScalar getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX();} + void setUnscaledRadius(btScalar radius) + { + m_implicitShapeDimensions.setX(radius); + btConvexInternalShape::setMargin(radius); + } + //debugging virtual const char* getName()const {return "SPHERE";} diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 70969fa9f..0b3c734d1 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -55,6 +55,7 @@ ATTRIBUTE_ALIGNED16( class) btPersistentManifold int m_cachedPoints; btScalar m_contactBreakingThreshold; + btScalar m_contactProcessingThreshold; /// sort cached points so most isolated points come first @@ -70,9 +71,10 @@ public: btPersistentManifold(); - btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold) + btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold) : m_body0(body0),m_body1(body1),m_cachedPoints(0), - m_contactBreakingThreshold(contactBreakingThreshold) + m_contactBreakingThreshold(contactBreakingThreshold), + m_contactProcessingThreshold(contactProcessingThreshold) { } @@ -111,6 +113,11 @@ public: ///@todo: get this margin from the current physics / collision environment btScalar getContactBreakingThreshold() const; + + btScalar getContactProcessingThreshold() const + { + return m_contactProcessingThreshold; + } int getCacheEntry(const btManifoldPoint& newPoint) const; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index fea58363a..6d32ee70e 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -246,6 +246,21 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, +void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection); +void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection) +{ + if (colObj && colObj->hasAnisotropicFriction()) + { + // transform to local coordinates + btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis(); + const btVector3& friction_scaling = colObj->getAnisotropicFriction(); + //apply anisotropic friction + loc_lateral *= friction_scaling; + // ... and transform it back to global coordinates + frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; + } +} + btSolverConstraint& btSequentialImpulseConstraintSolver::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) @@ -379,6 +394,10 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m solverBodyIdB = getOrInitSolverBody(*colObj1); } + ///avoid collision response between two static objects + if (!solverBodyIdA && !solverBodyIdB) + return; + btVector3 rel_pos1; btVector3 rel_pos2; btScalar relaxation; @@ -388,9 +407,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m btManifoldPoint& cp = manifold->getContactPoint(j); - ///this is a bad test and results in jitter -> always solve for those zero-distanc contacts! - ///-> if (cp.getDistance() <= btScalar(0.)) - //if (cp.getDistance() <= manifold->getContactBreakingThreshold()) + if (cp.getDistance() <= manifold->getContactProcessingThreshold()) { const btVector3& pos1 = cp.getPositionWorldOnA(); @@ -526,11 +543,15 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) { cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); 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); cp.m_lateralFrictionDir2.normalize();//?? + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } cp.m_lateralFrictionInitialized = true; @@ -538,9 +559,14 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m { //re-calculate friction direction every frame, todo: check if this is really needed btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); + 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); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } cp.m_lateralFrictionInitialized = true; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 467e37bb9..90e7fc835 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -24,10 +24,7 @@ class btIDebugDraw; -///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses -///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com -///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) -///Applies impulses for combined restitution and penetration recovery and to simulate friction +///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. class btSequentialImpulseConstraintSolver : public btConstraintSolver { protected: diff --git a/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/src/BulletDynamics/Vehicle/btRaycastVehicle.h index 568470cfd..7a8f69e12 100644 --- a/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -1,218 +1,218 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies. - * Erwin Coumans makes no representations about the suitability - * of this software for any purpose. - * It is provided "as is" without express or implied warranty. -*/ -#ifndef RAYCASTVEHICLE_H -#define RAYCASTVEHICLE_H - -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "btVehicleRaycaster.h" -class btDynamicsWorld; -#include "LinearMath/btAlignedObjectArray.h" -#include "btWheelInfo.h" - -class btVehicleTuning; - -///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle. -class btRaycastVehicle : public btTypedConstraint -{ - - btAlignedObjectArray m_forwardWS; - btAlignedObjectArray m_axle; - btAlignedObjectArray m_forwardImpulse; - btAlignedObjectArray m_sideImpulse; - -public: - class btVehicleTuning - { - public: - - btVehicleTuning() - :m_suspensionStiffness(btScalar(5.88)), - m_suspensionCompression(btScalar(0.83)), - m_suspensionDamping(btScalar(0.88)), - m_maxSuspensionTravelCm(btScalar(500.)), - m_frictionSlip(btScalar(10.5)) - { - } - btScalar m_suspensionStiffness; - btScalar m_suspensionCompression; - btScalar m_suspensionDamping; - btScalar m_maxSuspensionTravelCm; - btScalar m_frictionSlip; - - }; -private: - - btScalar m_tau; - btScalar m_damping; - btVehicleRaycaster* m_vehicleRaycaster; - btScalar m_pitchControl; - btScalar m_steeringValue; - btScalar m_currentVehicleSpeedKmHour; - - btRigidBody* m_chassisBody; - - int m_indexRightAxis; - int m_indexUpAxis; - int m_indexForwardAxis; - - void defaultInit(const btVehicleTuning& tuning); - -public: - - //constructor to create a car from an existing rigidbody - btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ); - - virtual ~btRaycastVehicle() ; - - - const btTransform& getChassisWorldTransform() const; - - btScalar rayCast(btWheelInfo& wheel); - - virtual void updateVehicle(btScalar step); - - void resetSuspension(); - - btScalar getSteeringValue(int wheel) const; - - void setSteeringValue(btScalar steering,int wheel); - - - void applyEngineForce(btScalar force, int wheel); - - const btTransform& getWheelTransformWS( int wheelIndex ) const; - - void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true ); - - void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth); - - btWheelInfo& addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel); - - inline int getNumWheels() const { - return int (m_wheelInfo.size()); - } - - btAlignedObjectArray m_wheelInfo; - - - const btWheelInfo& getWheelInfo(int index) const; - - btWheelInfo& getWheelInfo(int index); - - void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true); - - - void setBrake(btScalar brake,int wheelIndex); - - void setPitchControl(btScalar pitch) - { - m_pitchControl = pitch; - } - - void updateSuspension(btScalar deltaTime); - - virtual void updateFriction(btScalar timeStep); - - - - inline btRigidBody* getRigidBody() - { - return m_chassisBody; - } - - const btRigidBody* getRigidBody() const - { - return m_chassisBody; - } - - inline int getRightAxis() const - { - return m_indexRightAxis; - } - inline int getUpAxis() const - { - return m_indexUpAxis; - } - - inline int getForwardAxis() const - { - return m_indexForwardAxis; - } - - - ///Worldspace forward vector - btVector3 getForwardVector() const - { - const btTransform& chassisTrans = getChassisWorldTransform(); - - btVector3 forwardW ( - chassisTrans.getBasis()[0][m_indexForwardAxis], - chassisTrans.getBasis()[1][m_indexForwardAxis], - chassisTrans.getBasis()[2][m_indexForwardAxis]); - - return forwardW; - } - - ///Velocity of vehicle (positive if velocity vector has same direction as foward vector) - btScalar getCurrentSpeedKmHour() const - { - return m_currentVehicleSpeedKmHour; - } - - virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex) - { - m_indexRightAxis = rightIndex; - m_indexUpAxis = upIndex; - m_indexForwardAxis = forwardIndex; - } - - virtual void buildJacobian() - { - //not yet - } - +/* + * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * Permission to use, copy, modify, distribute and sell this software + * and its documentation for any purpose is hereby granted without fee, + * provided that the above copyright notice appear in all copies. + * Erwin Coumans makes no representations about the suitability + * of this software for any purpose. + * It is provided "as is" without express or implied warranty. +*/ +#ifndef RAYCASTVEHICLE_H +#define RAYCASTVEHICLE_H + +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "btVehicleRaycaster.h" +class btDynamicsWorld; +#include "LinearMath/btAlignedObjectArray.h" +#include "btWheelInfo.h" + +class btVehicleTuning; + +///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle. +class btRaycastVehicle : public btTypedConstraint +{ + + btAlignedObjectArray m_forwardWS; + btAlignedObjectArray m_axle; + btAlignedObjectArray m_forwardImpulse; + btAlignedObjectArray m_sideImpulse; + +public: + class btVehicleTuning + { + public: + + btVehicleTuning() + :m_suspensionStiffness(btScalar(5.88)), + m_suspensionCompression(btScalar(0.83)), + m_suspensionDamping(btScalar(0.88)), + m_maxSuspensionTravelCm(btScalar(500.)), + m_frictionSlip(btScalar(10.5)) + { + } + btScalar m_suspensionStiffness; + btScalar m_suspensionCompression; + btScalar m_suspensionDamping; + btScalar m_maxSuspensionTravelCm; + btScalar m_frictionSlip; + + }; +private: + + btScalar m_tau; + btScalar m_damping; + btVehicleRaycaster* m_vehicleRaycaster; + btScalar m_pitchControl; + btScalar m_steeringValue; + btScalar m_currentVehicleSpeedKmHour; + + btRigidBody* m_chassisBody; + + int m_indexRightAxis; + int m_indexUpAxis; + int m_indexForwardAxis; + + void defaultInit(const btVehicleTuning& tuning); + +public: + + //constructor to create a car from an existing rigidbody + btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ); + + virtual ~btRaycastVehicle() ; + + + const btTransform& getChassisWorldTransform() const; + + btScalar rayCast(btWheelInfo& wheel); + + virtual void updateVehicle(btScalar step); + + void resetSuspension(); + + btScalar getSteeringValue(int wheel) const; + + void setSteeringValue(btScalar steering,int wheel); + + + void applyEngineForce(btScalar force, int wheel); + + const btTransform& getWheelTransformWS( int wheelIndex ) const; + + void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true ); + + void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth); + + btWheelInfo& addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel); + + inline int getNumWheels() const { + return int (m_wheelInfo.size()); + } + + btAlignedObjectArray m_wheelInfo; + + + const btWheelInfo& getWheelInfo(int index) const; + + btWheelInfo& getWheelInfo(int index); + + void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true); + + + void setBrake(btScalar brake,int wheelIndex); + + void setPitchControl(btScalar pitch) + { + m_pitchControl = pitch; + } + + void updateSuspension(btScalar deltaTime); + + virtual void updateFriction(btScalar timeStep); + + + + inline btRigidBody* getRigidBody() + { + return m_chassisBody; + } + + const btRigidBody* getRigidBody() const + { + return m_chassisBody; + } + + inline int getRightAxis() const + { + return m_indexRightAxis; + } + inline int getUpAxis() const + { + return m_indexUpAxis; + } + + inline int getForwardAxis() const + { + return m_indexForwardAxis; + } + + + ///Worldspace forward vector + btVector3 getForwardVector() const + { + const btTransform& chassisTrans = getChassisWorldTransform(); + + btVector3 forwardW ( + chassisTrans.getBasis()[0][m_indexForwardAxis], + chassisTrans.getBasis()[1][m_indexForwardAxis], + chassisTrans.getBasis()[2][m_indexForwardAxis]); + + return forwardW; + } + + ///Velocity of vehicle (positive if velocity vector has same direction as foward vector) + btScalar getCurrentSpeedKmHour() const + { + return m_currentVehicleSpeedKmHour; + } + + virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex) + { + m_indexRightAxis = rightIndex; + m_indexUpAxis = upIndex; + m_indexForwardAxis = forwardIndex; + } + + virtual void buildJacobian() + { + //not yet + } + virtual void getInfo1 (btConstraintInfo1* info) { info->m_numConstraintRows = 0; info->nub = 0; } - virtual void getInfo2 (btConstraintInfo2* info) - { - btAssert(0); - } - - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) - { - (void)timeStep; - //not yet - } - - -}; - -class btDefaultVehicleRaycaster : public btVehicleRaycaster -{ - btDynamicsWorld* m_dynamicsWorld; -public: - btDefaultVehicleRaycaster(btDynamicsWorld* world) - :m_dynamicsWorld(world) - { - } - - virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result); - -}; - - -#endif //RAYCASTVEHICLE_H - + virtual void getInfo2 (btConstraintInfo2* info) + { + btAssert(0); + } + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) + { + (void)timeStep; + //not yet + } + + +}; + +class btDefaultVehicleRaycaster : public btVehicleRaycaster +{ + btDynamicsWorld* m_dynamicsWorld; +public: + btDefaultVehicleRaycaster(btDynamicsWorld* world) + :m_dynamicsWorld(world) + { + } + + virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result); + +}; + + +#endif //RAYCASTVEHICLE_H + diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index a9c21ed8f..ee810c540 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -87,7 +87,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btV } updateBounds(); - + m_initialWorldTransform.setIdentity(); } // @@ -306,8 +306,16 @@ void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat) } // -void btSoftBody::appendAnchor(int node,btRigidBody* body) +void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies) { + if (disableCollisionBetweenLinkedBodies) + { + if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size()) + { + m_collisionDisabledObjects.push_back(body); + } + } + Anchor a; a.m_node = &m_nodes[node]; a.m_body = body; @@ -501,6 +509,7 @@ void btSoftBody::transform(const btTransform& trs) updateNormals(); updateBounds(); updateConstants(); + m_initialWorldTransform = trs; } // @@ -2662,22 +2671,26 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) break; case fCollision::VF_SS: { - btSoftColliders::CollideVF_SS docollide; - /* common */ - docollide.mrg= getCollisionShape()->getMargin()+ - psb->getCollisionShape()->getMargin(); - /* psb0 nodes vs psb1 faces */ - docollide.psb[0]=this; - docollide.psb[1]=psb; - docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - /* psb1 nodes vs psb0 faces */ - docollide.psb[0]=psb; - docollide.psb[1]=this; - docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); + //only self-collision for Cluster, not Vertex-Face yet + if (this!=psb) + { + btSoftColliders::CollideVF_SS docollide; + /* common */ + docollide.mrg= getCollisionShape()->getMargin()+ + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + docollide.psb[0]=this; + docollide.psb[1]=psb; + docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + /* psb1 nodes vs psb0 faces */ + docollide.psb[0]=psb; + docollide.psb[1]=this; + docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } } break; } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 9fcc4ec3b..a62c21883 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -49,6 +49,8 @@ struct btSoftBodyWorldInfo class btSoftBody : public btCollisionObject { public: + btAlignedObjectArray m_collisionDisabledObjects; + // // Enumerations // @@ -604,8 +606,11 @@ public: btDbvt m_fdbvt; // Faces tree btDbvt m_cdbvt; // Clusters tree tClusterArray m_clusters; // Clusters + btAlignedObjectArraym_clusterConnectivity;//cluster connectivity, for self-collision + btTransform m_initialWorldTransform; + // // Api // @@ -678,7 +683,7 @@ public: Material* mat=0); /* Append anchor */ void appendAnchor( int node, - btRigidBody* body); + btRigidBody* body, bool disableCollisionBetweenLinkedBodies=false); /* Append linear joint */ void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1); void appendLinearJoint(const LJoint::Specs& specs,Body body=Body()); diff --git a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp index b29b35fe6..11ad9e7da 100644 --- a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp +++ b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp @@ -58,8 +58,11 @@ void btSoftRigidCollisionAlgorithm::processCollision (btCollisionObject* body0,b btSoftBody* softBody = m_isSwapped? (btSoftBody*)body1 : (btSoftBody*)body0; btCollisionObject* rigidCollisionObject = m_isSwapped? body0 : body1; - - softBody->defaultCollisionHandler(rigidCollisionObject); + + if (softBody->m_collisionDisabledObjects.findLinearSearch(rigidCollisionObject)==softBody->m_collisionDisabledObjects.size()) + { + softBody->defaultCollisionHandler(rigidCollisionObject); + } }