From 9d41401d937cab3182d966c79c8c9df8b05c10db Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 4 Apr 2006 22:26:11 +0000 Subject: [PATCH] improved some Bullet Dynamics, related to friction/contact constraints. --- .../BroadphaseCollision/SimpleBroadphase.cpp | 2 +- extern/bullet/Bullet/Bullet3_vc8.vcproj | 12 -- .../CollisionDispatch/CollisionObject.cpp | 5 + .../CollisionDispatch/CollisionObject.h | 2 + .../ConvexTriangleCallback.cpp | 2 +- .../NarrowPhaseCollision/ManifoldPoint.h | 4 +- .../BulletDynamics/BulletDynamics_vc8.vcproj | 8 -- .../ConstraintSolver/ContactConstraint.cpp | 11 +- .../ConstraintSolver/ContactSolverInfo.h | 2 +- .../Point2PointConstraint.cpp | 5 +- .../ConstraintSolver/Point2PointConstraint.h | 23 +++- .../SimpleConstraintSolver.cpp | 30 ++--- .../CcdPhysics/CcdPhysicsEnvironment.cpp | 107 +++++++++++------- .../CcdPhysics/CcdPhysicsEnvironment.h | 4 +- 14 files changed, 125 insertions(+), 92 deletions(-) diff --git a/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp b/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp index c415b7b997d..c42386af580 100644 --- a/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp +++ b/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: #include "SimdTransform.h" #include "SimdMatrix3x3.h" #include -#include "CollisionShapes/CollisionMargin.h" + SimpleBroadphase::SimpleBroadphase(int maxProxies,int maxOverlap) :m_firstFreeProxy(0), diff --git a/extern/bullet/Bullet/Bullet3_vc8.vcproj b/extern/bullet/Bullet/Bullet3_vc8.vcproj index c6a7f31f3a3..256cfd63d06 100644 --- a/extern/bullet/Bullet/Bullet3_vc8.vcproj +++ b/extern/bullet/Bullet/Bullet3_vc8.vcproj @@ -377,14 +377,6 @@ RelativePath=".\CollisionShapes\ConvexShape.h" > - - - - @@ -654,10 +646,6 @@ > - - diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp index 45b173d0842..191bd8e1c71 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp @@ -23,6 +23,11 @@ void CollisionObject::SetActivationState(int newState) m_activationState1 = newState; } +void CollisionObject::ForceActivationState(int newState) +{ + m_activationState1 = newState; +} + void CollisionObject::activate() { SetActivationState(1); diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionObject.h b/extern/bullet/Bullet/CollisionDispatch/CollisionObject.h index d412ffbf9d2..9a74954d36f 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionObject.h +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionObject.h @@ -76,6 +76,8 @@ struct CollisionObject void SetActivationState(int newState); + void ForceActivationState(int newState); + void activate(); diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp index 71a74f65bc2..35cad78fbb9 100644 --- a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp +++ b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp @@ -74,7 +74,7 @@ void ConvexTriangleCallback::ProcessTriangle(SimdVector3* triangle) input.m_maximumDistanceSquared = 1e30f;//? - gjkDetector.GetClosestPoints(input,output,0); + gjkDetector.GetClosestPoints(input,output); } diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h index 3ef93fc5de2..000dc4c1057 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h @@ -75,8 +75,8 @@ class ManifoldPoint void CopyPersistentInformation(const ManifoldPoint& otherPoint) { m_appliedImpulse = otherPoint.m_appliedImpulse; - m_accumulatedTangentImpulse0 = 0.f;//otherPoint.m_accumulatedTangentImpulse0; - m_accumulatedTangentImpulse1 = 0.f;//otherPoint.m_accumulatedTangentImpulse1; + m_accumulatedTangentImpulse0 = otherPoint.m_accumulatedTangentImpulse0; + m_accumulatedTangentImpulse1 = otherPoint.m_accumulatedTangentImpulse1; m_prevAppliedImpulse = otherPoint.m_prevAppliedImpulse; m_lifeTime = otherPoint.m_lifeTime; diff --git a/extern/bullet/BulletDynamics/BulletDynamics_vc8.vcproj b/extern/bullet/BulletDynamics/BulletDynamics_vc8.vcproj index 943479dc623..785945f2680 100644 --- a/extern/bullet/BulletDynamics/BulletDynamics_vc8.vcproj +++ b/extern/bullet/BulletDynamics/BulletDynamics_vc8.vcproj @@ -162,14 +162,6 @@ RelativePath=".\ConstraintSolver\ContactSolverInfo.h" > - - - - diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp index e8e8d37f6d6..ae1e6ce2221 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp @@ -42,7 +42,7 @@ SimdScalar calculateCombinedFriction(RigidBody& body0,RigidBody& body1) { SimdScalar friction = body0.getFriction() * body1.getFriction(); - const SimdScalar MAX_FRICTION = 1.f; + const SimdScalar MAX_FRICTION = 10.f; if (friction < -MAX_FRICTION) friction = -MAX_FRICTION; if (friction > MAX_FRICTION) @@ -102,7 +102,7 @@ void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1, #endif } -float allowedPenetration = 0.0f; + //velocity + friction @@ -153,10 +153,9 @@ float resolveSingleCollision( //printf("dist=%f\n",distance); - float clipDist = distance + allowedPenetration; - float dist = (clipDist > 0.f) ? 0.f : clipDist; + //distance = 0.f; - SimdScalar positionalError = Kcor *-dist; + SimdScalar positionalError = Kcor *-distance; //jacDiagABInv; SimdScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping; @@ -197,7 +196,7 @@ float resolveSingleFriction( float combinedFriction = calculateCombinedFriction(body1,body2); SimdScalar limit = contactPoint.m_appliedImpulse * combinedFriction; - if (contactPoint.m_appliedImpulse>0.f) + //if (contactPoint.m_appliedImpulse>0.f) //friction { //apply friction in the 2 tangential directions diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h b/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h index 6c6325df8a6..2ec9b7f6ca1 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h @@ -22,7 +22,7 @@ struct ContactSolverInfo inline ContactSolverInfo() { - m_tau = 0.4f; + m_tau = 0.6f; m_damping = 1.0f; m_friction = 0.3f; m_restitution = 0.f; diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp index 1b5f2d6139e..64b470f4391 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp @@ -67,8 +67,7 @@ void Point2PointConstraint::SolveConstraint(SimdScalar timeStep) SimdVector3 normal(0,0,0); - SimdScalar tau = 0.3f; - SimdScalar damping = 1.f; + // SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); // SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); @@ -98,7 +97,7 @@ void Point2PointConstraint::SolveConstraint(SimdScalar timeStep) //positional error (zeroth order error) SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - SimdScalar impulse = depth*tau/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; + SimdScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; SimdVector3 impulse_vector = normal * impulse; m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.h b/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.h index bb5e3ef8e80..bd28ac7b1e5 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.h @@ -23,6 +23,16 @@ subject to the following restrictions: class RigidBody; +struct ConstraintSetting +{ + ConstraintSetting() : + m_tau(0.3f), + m_damping(1.f) + { + } + float m_tau; + float m_damping; +}; /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space class Point2PointConstraint : public TypedConstraint @@ -33,8 +43,11 @@ class Point2PointConstraint : public TypedConstraint SimdVector3 m_pivotInB; + public: + ConstraintSetting m_setting; + Point2PointConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB); Point2PointConstraint(RigidBody& rbA,const SimdVector3& pivotInA); @@ -48,7 +61,15 @@ public: void UpdateRHS(SimdScalar timeStep); - + void SetPivotA(const SimdVector3& pivotA) + { + m_pivotInA = pivotA; + } + + void SetPivotB(const SimdVector3& pivotB) + { + m_pivotInB = pivotB; + } diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp index acfca631b72..d2d3636442e 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp @@ -43,25 +43,25 @@ float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int n for (j=0;jGetContactPoint(j); - if (cp.GetDistance() <= 0.f) + if (cp.GetDistance() <= 0.f) { if (iter == 0) @@ -204,9 +206,11 @@ float SimpleConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, con { int j=i; + //if (iter % 2) + // j = numpoints-1-i; ManifoldPoint& cp = manifoldPtr->GetContactPoint(j); - if (cp.GetDistance() <= 0.f) + if (cp.GetDistance() <= 0.f) { resolveSingleFriction( diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index 3039bba8565..f715387a189 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -452,9 +452,12 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) if (!SimdFuzzyZero(timeStep)) { - //Blender runs 30hertz, so subdivide so we get 60 hertz +#ifdef SPLIT_TIMESTEP proceedDeltaTimeOneStep(0.5f*timeStep); proceedDeltaTimeOneStep(0.5f*timeStep); +#else + proceedDeltaTimeOneStep(timeStep); +#endif } else { //todo: interpolate @@ -542,7 +545,48 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) //contacts - struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback + //solve the regular constraints (point 2 point, hinge, etc) + + for (int g=0;gBuildJacobian(); + p2p->SolveConstraint( timeStep ); + + } + + + + + } + + //solve the vehicles + + #ifdef NEW_BULLET_VEHICLE_SUPPORT + //vehicles + int numVehicles = m_wrapperVehicles.size(); + for (int i=0;iGetVehicle(); + vehicle->UpdateVehicle( timeStep); + } +#endif //NEW_BULLET_VEHICLE_SUPPORT + + + struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback { ContactSolverInfo& m_solverInfo; @@ -578,47 +622,10 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) m_solver, m_debugDrawer); + /// solve all the contact points and contact friction GetDispatcher()->BuildAndProcessIslands(numRigidBodies,&solverCallback); - for (int g=0;gBuildJacobian(); - p2p->SolveConstraint( timeStep ); - - } - - - - - } - - - #ifdef NEW_BULLET_VEHICLE_SUPPORT - //vehicles - int numVehicles = m_wrapperVehicles.size(); - for (int i=0;iGetVehicle(); - vehicle->UpdateVehicle( timeStep); - } -#endif //NEW_BULLET_VEHICLE_SUPPORT - - { @@ -943,13 +950,13 @@ void CcdPhysicsEnvironment::setGravity(float x,float y,float z) #ifdef NEW_BULLET_VEHICLE_SUPPORT -class BlenderVehicleRaycaster : public VehicleRaycaster +class DefaultVehicleRaycaster : public VehicleRaycaster { CcdPhysicsEnvironment* m_physEnv; PHY_IPhysicsController* m_chassis; public: - BlenderVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis): + DefaultVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis): m_physEnv(physEnv), m_chassis(chassis) { @@ -1061,7 +1068,7 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl { RaycastVehicle::VehicleTuning* tuning = new RaycastVehicle::VehicleTuning(); RigidBody* chassis = rb0; - BlenderVehicleRaycaster* raycaster = new BlenderVehicleRaycaster(this,ctrl0); + DefaultVehicleRaycaster* raycaster = new DefaultVehicleRaycaster(this,ctrl0); RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster); WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0); m_wrapperVehicles.push_back(wrapperVehicle); @@ -1300,6 +1307,20 @@ const PersistentManifold* CcdPhysicsEnvironment::GetManifold(int index) const return GetDispatcher()->GetManifoldByIndexInternal(index); } +Point2PointConstraint* CcdPhysicsEnvironment::getPoint2PointConstraint(int constraintId) +{ + int nump2p = m_p2pConstraints.size(); + int i; + for (i=0;iGetUserConstraintId()==constraintId) + { + return p2p; + } + } + return 0; +} #ifdef NEW_BULLET_VEHICLE_SUPPORT diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h index 712620e7165..37c78168ba6 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h @@ -14,7 +14,7 @@ class Dispatcher; //#include "BroadphaseInterface.h" //switch on/off new vehicle support -#define NEW_BULLET_VEHICLE_SUPPORT 1 +//#define NEW_BULLET_VEHICLE_SUPPORT 1 #include "ConstraintSolver/ContactSolverInfo.h" @@ -96,6 +96,8 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment } #endif //NEW_BULLET_VEHICLE_SUPPORT + Point2PointConstraint* getPoint2PointConstraint(int constraintId); + virtual PHY_IPhysicsController* rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ);