more physics work: fixed some deactivation bug, improved contact constraint stability and friction (lower the number of iterations).
This commit is contained in:
@@ -19,7 +19,8 @@ subject to the following restrictions:
|
||||
|
||||
void CollisionObject::SetActivationState(int newState)
|
||||
{
|
||||
m_activationState1 = newState;
|
||||
if (m_activationState1 != DISABLE_DEACTIVATION)
|
||||
m_activationState1 = newState;
|
||||
}
|
||||
|
||||
void CollisionObject::activate()
|
||||
|
||||
@@ -37,6 +37,7 @@ class ManifoldPoint
|
||||
m_normalWorldOnB( normal ),
|
||||
m_distance1( distance ),
|
||||
m_appliedImpulse(0.f),
|
||||
m_prevAppliedImpulse(0.f),
|
||||
m_accumulatedTangentImpulse0(0.f),
|
||||
m_accumulatedTangentImpulse1(0.f),
|
||||
m_jacDiagABInv(0.f),
|
||||
@@ -62,16 +63,21 @@ class ManifoldPoint
|
||||
float m_distance1;
|
||||
/// total applied impulse during most recent frame
|
||||
float m_appliedImpulse;
|
||||
float m_prevAppliedImpulse;
|
||||
float m_accumulatedTangentImpulse0;
|
||||
float m_accumulatedTangentImpulse1;
|
||||
|
||||
float m_jacDiagABInv;
|
||||
float m_jacDiagABInvTangent0;
|
||||
float m_jacDiagABInvTangent1;
|
||||
|
||||
|
||||
void CopyPersistentInformation(const ManifoldPoint& otherPoint)
|
||||
{
|
||||
m_appliedImpulse = otherPoint.m_appliedImpulse;
|
||||
m_accumulatedTangentImpulse0 = 0;//otherPoint.m_accumulatedTangentImpulse0;
|
||||
m_accumulatedTangentImpulse1 = 0;//otherPoint.m_accumulatedTangentImpulse1;
|
||||
m_accumulatedTangentImpulse0 = 0.f;//otherPoint.m_accumulatedTangentImpulse0;
|
||||
m_accumulatedTangentImpulse1 = 0.f;//otherPoint.m_accumulatedTangentImpulse1;
|
||||
m_prevAppliedImpulse = otherPoint.m_prevAppliedImpulse;
|
||||
m_lifeTime = otherPoint.m_lifeTime;
|
||||
|
||||
}
|
||||
|
||||
@@ -146,10 +146,14 @@ float resolveSingleCollision(
|
||||
|
||||
float Kcor = Kerp *Kfps;
|
||||
|
||||
float allowedPenetration = 0.001f;
|
||||
|
||||
SimdScalar positionalError = Kcor *-distance;
|
||||
float clipDist = distance + allowedPenetration;
|
||||
float dist = (clipDist > 0.f) ? 0.f : clipDist;
|
||||
|
||||
SimdScalar positionalError = Kcor *-dist*damping;
|
||||
//jacDiagABInv;
|
||||
SimdScalar velocityError = -(1.0f + restitution) * damping * rel_vel;
|
||||
SimdScalar velocityError = -(1.0f + restitution) * rel_vel;
|
||||
|
||||
SimdScalar penetrationImpulse = positionalError * contactPoint.m_jacDiagABInv;
|
||||
|
||||
@@ -161,6 +165,7 @@ float resolveSingleCollision(
|
||||
float oldNormalImpulse = contactPoint.m_appliedImpulse;
|
||||
float sum = oldNormalImpulse + normalImpulse;
|
||||
contactPoint.m_appliedImpulse = 0.f > sum ? 0.f: sum;
|
||||
|
||||
normalImpulse = contactPoint.m_appliedImpulse - oldNormalImpulse;
|
||||
|
||||
body1.applyImpulse(normal*(normalImpulse), rel_pos1);
|
||||
@@ -184,14 +189,14 @@ float resolveSingleFriction(
|
||||
|
||||
SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
float combinedFriction = calculateCombinedFriction(body1,body2);
|
||||
|
||||
//friction
|
||||
SimdScalar limit = contactPoint.m_appliedImpulse * combinedFriction;
|
||||
if (contactPoint.m_appliedImpulse>0.f)
|
||||
//friction
|
||||
{
|
||||
//apply friction in the 2 tangential directions
|
||||
|
||||
float combinedFriction = calculateCombinedFriction(body1,body2);
|
||||
SimdScalar limit = contactPoint.m_appliedImpulse * combinedFriction;
|
||||
SimdScalar relaxation = solverInfo.m_damping;
|
||||
{
|
||||
// 1st tangent
|
||||
@@ -200,12 +205,9 @@ float resolveSingleFriction(
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
|
||||
SimdScalar vrel = contactPoint.m_frictionWorldTangential0.dot(vel);
|
||||
float denom0 = body1.ComputeImpulseDenominator(pos1,contactPoint.m_frictionWorldTangential0);
|
||||
float denom1 = body2.ComputeImpulseDenominator(pos2,contactPoint.m_frictionWorldTangential0);
|
||||
float denom = relaxation/(denom0+denom1);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
SimdScalar j = -vrel * denom;//Scalar(contactPoint.pt.m_impulseScales[1]);
|
||||
SimdScalar j = -vrel * contactPoint.m_jacDiagABInvTangent1;
|
||||
float total = contactPoint.m_accumulatedTangentImpulse0 + j;
|
||||
GEN_set_min(total, limit);
|
||||
GEN_set_max(total, -limit);
|
||||
@@ -223,12 +225,9 @@ float resolveSingleFriction(
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
|
||||
SimdScalar vrel = contactPoint.m_frictionWorldTangential1.dot(vel);
|
||||
float denom0 = body1.ComputeImpulseDenominator(pos1,contactPoint.m_frictionWorldTangential1);
|
||||
float denom1 = body2.ComputeImpulseDenominator(pos2,contactPoint.m_frictionWorldTangential1);
|
||||
float denom = relaxation/(denom0+denom1);
|
||||
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
SimdScalar j = -vrel * denom;//Scalar(contactPoint.pt.m_impulseScales[1]);
|
||||
SimdScalar j = -vrel * contactPoint.m_jacDiagABInvTangent1;
|
||||
float total = contactPoint.m_accumulatedTangentImpulse1 + j;
|
||||
GEN_set_min(total, limit);
|
||||
GEN_set_max(total, -limit);
|
||||
|
||||
@@ -54,7 +54,7 @@ class BU_Joint;
|
||||
//see below
|
||||
|
||||
OdeConstraintSolver::OdeConstraintSolver():
|
||||
m_cfm(1e-5f),
|
||||
m_cfm(0.f),//1e-5f),
|
||||
m_erp(0.4f)
|
||||
{
|
||||
}
|
||||
|
||||
@@ -46,28 +46,29 @@ float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int n
|
||||
//should traverse the contacts random order...
|
||||
for (int i = 0;i<numiter;i++)
|
||||
{
|
||||
for (int j=0;j<numManifolds;j++)
|
||||
int j;
|
||||
for (j=0;j<numManifolds;j++)
|
||||
{
|
||||
int k=j;
|
||||
if (i % 2)
|
||||
k = numManifolds-1-j;
|
||||
if (i&&1)
|
||||
k=numManifolds-j-1;
|
||||
|
||||
Solve(manifoldPtr[k],info,i,debugDrawer);
|
||||
}
|
||||
for (j=0;j<numManifolds;j++)
|
||||
{
|
||||
int k = j;
|
||||
if (i&&1)
|
||||
k=numManifolds-j-1;
|
||||
SolveFriction(manifoldPtr[k],info,i,debugDrawer);
|
||||
}
|
||||
}
|
||||
|
||||
//now solve the friction
|
||||
for (int i = 0;i<numiter;i++)
|
||||
{
|
||||
for (int j=0;j<numManifolds;j++)
|
||||
{
|
||||
int k = numManifolds-1-j;
|
||||
if (i % 2)
|
||||
k=j;
|
||||
|
||||
|
||||
SolveFriction(manifoldPtr[k],info,i,debugDrawer);
|
||||
}
|
||||
int j;
|
||||
|
||||
}
|
||||
|
||||
return 0.f;
|
||||
@@ -109,18 +110,42 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta
|
||||
body1->getInvInertiaDiagLocal(),body1->getInvMass());
|
||||
|
||||
SimdScalar jacDiagAB = jac.getDiagonal();
|
||||
|
||||
cp.m_jacDiagABInv = 1.f / jacDiagAB;
|
||||
|
||||
float relaxation = 0.9f;
|
||||
//for friction
|
||||
cp.m_prevAppliedImpulse = cp.m_appliedImpulse;
|
||||
|
||||
float relaxation = info.m_damping;
|
||||
cp.m_appliedImpulse *= relaxation;
|
||||
|
||||
//apply previous frames impulse on both bodies
|
||||
body0->applyImpulse(cp.m_normalWorldOnB*(cp.m_appliedImpulse), rel_pos1);
|
||||
body1->applyImpulse(cp.m_normalWorldOnB*(-cp.m_appliedImpulse), rel_pos2);
|
||||
|
||||
|
||||
//re-calculate friction direction every frame, todo: check if this is really needed
|
||||
SimdPlaneSpace1(cp.m_normalWorldOnB,cp.m_frictionWorldTangential0,cp.m_frictionWorldTangential1);
|
||||
|
||||
cp.m_accumulatedTangentImpulse0 = 0.f;
|
||||
cp.m_accumulatedTangentImpulse1 = 0.f;
|
||||
|
||||
float denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential0);
|
||||
float denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential0);
|
||||
float denom = relaxation/(denom0+denom1);
|
||||
cp.m_jacDiagABInvTangent0 = denom;
|
||||
|
||||
|
||||
denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential1);
|
||||
denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential1);
|
||||
denom = relaxation/(denom0+denom1);
|
||||
cp.m_jacDiagABInvTangent1 = denom;
|
||||
|
||||
SimdVector3 totalImpulse =
|
||||
// cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+
|
||||
// cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+
|
||||
cp.m_normalWorldOnB*cp.m_appliedImpulse;
|
||||
|
||||
//apply previous frames impulse on both bodies
|
||||
body0->applyImpulse(totalImpulse, rel_pos1);
|
||||
body1->applyImpulse(-totalImpulse, rel_pos2);
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -148,10 +173,11 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta
|
||||
{
|
||||
|
||||
|
||||
float actualDist = cp.GetDistance();
|
||||
#define MAXPENETRATIONPERFRAME -0.2f
|
||||
float dist = actualDist< MAXPENETRATIONPERFRAME? MAXPENETRATIONPERFRAME:actualDist;
|
||||
//float actualDist = cp.GetDistance();
|
||||
//#define MAXPENETRATIONPERFRAME -0.2f
|
||||
//float dist = actualDist< MAXPENETRATIONPERFRAME? MAXPENETRATIONPERFRAME:actualDist;
|
||||
|
||||
float dist = cp.GetDistance();
|
||||
|
||||
float impulse = resolveSingleCollision(
|
||||
*body0,*body1,
|
||||
|
||||
@@ -96,6 +96,37 @@ void RigidBody::applyForces(SimdScalar step)
|
||||
|
||||
m_linearVelocity *= GEN_clamped((1.f - step * gLinearAirDamping * m_linearDamping), 0.0f, 1.0f);
|
||||
m_angularVelocity *= GEN_clamped((1.f - step * m_angularDamping), 0.0f, 1.0f);
|
||||
|
||||
//#define FORCE_VELOCITY_DAMPING 1
|
||||
#ifdef FORCE_VELOCITY_DAMPING
|
||||
float speed = m_linearVelocity.length();
|
||||
if (speed < m_linearDamping)
|
||||
{
|
||||
float dampVel = 0.005f;
|
||||
if (speed > dampVel)
|
||||
{
|
||||
SimdVector3 dir = m_linearVelocity.normalized();
|
||||
m_linearVelocity -= dir * dampVel;
|
||||
} else
|
||||
{
|
||||
m_linearVelocity.setValue(0.f,0.f,0.f);
|
||||
}
|
||||
}
|
||||
|
||||
float angSpeed = m_angularVelocity.length();
|
||||
if (angSpeed < m_angularDamping)
|
||||
{
|
||||
float angDampVel = 0.005f;
|
||||
if (angSpeed > angDampVel)
|
||||
{
|
||||
SimdVector3 dir = m_angularVelocity.normalized();
|
||||
m_angularVelocity -= dir * angDampVel;
|
||||
} else
|
||||
{
|
||||
m_angularVelocity.setValue(0.f,0.f,0.f);
|
||||
}
|
||||
}
|
||||
#endif //FORCE_VELOCITY_DAMPING
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -1,19 +1,3 @@
|
||||
/*
|
||||
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 "CcdPhysicsController.h"
|
||||
|
||||
#include "Dynamics/RigidBody.h"
|
||||
@@ -390,9 +374,8 @@ void CcdPhysicsController::setNewClientInfo(void* clientinfo)
|
||||
|
||||
void CcdPhysicsController::UpdateDeactivation(float timeStep)
|
||||
{
|
||||
if ( (m_body->GetActivationState() == 2))
|
||||
if ( (m_body->GetActivationState() == ISLAND_SLEEPING) || (m_body->GetActivationState() == DISABLE_DEACTIVATION))
|
||||
return;
|
||||
|
||||
|
||||
if ((m_body->getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) &&
|
||||
(m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold))
|
||||
@@ -409,6 +392,9 @@ void CcdPhysicsController::UpdateDeactivation(float timeStep)
|
||||
bool CcdPhysicsController::wantsSleeping()
|
||||
{
|
||||
|
||||
if (m_body->GetActivationState() == DISABLE_DEACTIVATION)
|
||||
return false;
|
||||
|
||||
//disable deactivation
|
||||
if (gDisableDeactivation || (gDeactivationTime == 0.f))
|
||||
return false;
|
||||
|
||||
@@ -1,17 +1,3 @@
|
||||
/*
|
||||
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 BULLET2_PHYSICSCONTROLLER_H
|
||||
#define BULLET2_PHYSICSCONTROLLER_H
|
||||
@@ -41,14 +27,14 @@ struct CcdConstructionInfo
|
||||
{
|
||||
CcdConstructionInfo()
|
||||
: m_gravity(0,0,0),
|
||||
m_scaling(1.f,1.f,1.f),
|
||||
m_mass(0.f),
|
||||
m_restitution(0.1f),
|
||||
m_linearDamping(0.1f),
|
||||
m_angularDamping(0.1f),
|
||||
m_MotionState(0),
|
||||
m_physicsEnv(0),
|
||||
m_inertiaFactor(1.f)
|
||||
m_inertiaFactor(1.f),
|
||||
m_scaling(1.f,1.f,1.f)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
@@ -1,18 +1,3 @@
|
||||
/*
|
||||
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 "CcdPhysicsEnvironment.h"
|
||||
#include "CcdPhysicsController.h"
|
||||
|
||||
@@ -21,6 +6,8 @@ subject to the following restrictions:
|
||||
#include "Dynamics/RigidBody.h"
|
||||
#include "BroadphaseCollision/BroadphaseInterface.h"
|
||||
#include "BroadphaseCollision/SimpleBroadphase.h"
|
||||
#include "BroadphaseCollision/AxisSweep3.h"
|
||||
|
||||
#include "CollisionDispatch/CollisionWorld.h"
|
||||
|
||||
#include "CollisionShapes/ConvexShape.h"
|
||||
@@ -62,9 +49,6 @@ RaycastVehicle::VehicleTuning gTuning;
|
||||
|
||||
#include "ConstraintSolver/ConstraintSolver.h"
|
||||
#include "ConstraintSolver/Point2PointConstraint.h"
|
||||
#include "ConstraintSolver/HingeConstraint.h"
|
||||
|
||||
|
||||
//#include "BroadphaseCollision/QueryDispatcher.h"
|
||||
//#include "BroadphaseCollision/QueryBox.h"
|
||||
//todo: change this to allow dynamic registration of types!
|
||||
@@ -133,6 +117,7 @@ public:
|
||||
{
|
||||
WheelInfo& info = m_vehicle->GetWheelInfo(i);
|
||||
PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ;
|
||||
m_vehicle->UpdateWheelTransform(i);
|
||||
SimdTransform trans = m_vehicle->GetWheelTransformWS(i);
|
||||
SimdQuaternion orn = trans.getRotation();
|
||||
const SimdVector3& pos = trans.getOrigin();
|
||||
@@ -308,10 +293,10 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV
|
||||
|
||||
|
||||
CcdPhysicsEnvironment::CcdPhysicsEnvironment(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase)
|
||||
:m_numIterations(5),
|
||||
:m_scalingPropagated(false),
|
||||
m_numIterations(10),
|
||||
m_ccdMode(0),
|
||||
m_solverType(-1),
|
||||
m_scalingPropagated(false)
|
||||
m_solverType(-1)
|
||||
{
|
||||
|
||||
if (!dispatcher)
|
||||
@@ -319,9 +304,18 @@ m_scalingPropagated(false)
|
||||
|
||||
|
||||
if(!broadphase)
|
||||
broadphase = new SimpleBroadphase();
|
||||
{
|
||||
|
||||
//todo: calculate/let user specify this world sizes
|
||||
SimdVector3 worldMin(-10000,-10000,-10000);
|
||||
SimdVector3 worldMax(10000,10000,10000);
|
||||
|
||||
broadphase = new AxisSweep3(worldMin,worldMax);
|
||||
|
||||
//broadphase = new SimpleBroadphase();
|
||||
}
|
||||
|
||||
setSolverType(1);
|
||||
setSolverType(0);
|
||||
|
||||
m_collisionWorld = new CollisionWorld(dispatcher,broadphase);
|
||||
|
||||
@@ -342,7 +336,7 @@ void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
|
||||
|
||||
assert(body->m_broadphaseHandle);
|
||||
|
||||
/*BroadphaseInterface* scene = */GetBroadphase();
|
||||
BroadphaseInterface* scene = GetBroadphase();
|
||||
|
||||
|
||||
CollisionShape* shapeinterface = ctrl->GetCollisionShape();
|
||||
@@ -398,12 +392,12 @@ void CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr
|
||||
//also remove constraint
|
||||
|
||||
{
|
||||
std::vector<TypedConstraint*>::iterator i;
|
||||
std::vector<Point2PointConstraint*>::iterator i;
|
||||
|
||||
for (i=m_constraints.begin();
|
||||
!(i==m_constraints.end()); i++)
|
||||
for (i=m_p2pConstraints.begin();
|
||||
!(i==m_p2pConstraints.end()); i++)
|
||||
{
|
||||
TypedConstraint* p2p = (*i);
|
||||
Point2PointConstraint* p2p = (*i);
|
||||
if ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() ||
|
||||
(&p2p->GetRigidBodyB() == ctrl->GetRigidBody())))
|
||||
{
|
||||
@@ -415,12 +409,12 @@ void CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr
|
||||
}
|
||||
|
||||
{
|
||||
std::vector<TypedConstraint*>::iterator i;
|
||||
std::vector<Point2PointConstraint*>::iterator i;
|
||||
|
||||
for (i=m_constraints.begin();
|
||||
!(i==m_constraints.end()); i++)
|
||||
for (i=m_p2pConstraints.begin();
|
||||
!(i==m_p2pConstraints.end()); i++)
|
||||
{
|
||||
TypedConstraint* p2p = (*i);
|
||||
Point2PointConstraint* p2p = (*i);
|
||||
if ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() ||
|
||||
(&p2p->GetRigidBodyB() == ctrl->GetRigidBody())))
|
||||
{
|
||||
@@ -594,12 +588,12 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
|
||||
|
||||
|
||||
int i;
|
||||
int numPoint2Point = m_constraints.size();
|
||||
int numPoint2Point = m_p2pConstraints.size();
|
||||
|
||||
//point to point constraints
|
||||
for (i=0;i< numPoint2Point ; i++ )
|
||||
{
|
||||
TypedConstraint* p2p = m_constraints[i];
|
||||
Point2PointConstraint* p2p = m_p2pConstraints[i];
|
||||
|
||||
p2p->BuildJacobian();
|
||||
p2p->SolveConstraint( timeStep );
|
||||
@@ -686,6 +680,10 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
|
||||
{
|
||||
break;
|
||||
}
|
||||
case DISABLE_DEACTIVATION:
|
||||
{
|
||||
color.setValue(1,0,1);
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
@@ -769,7 +767,8 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
|
||||
body->SetActivationState( WANTS_DEACTIVATION );
|
||||
} else
|
||||
{
|
||||
body->SetActivationState( ACTIVE_TAG );
|
||||
if (body->GetActivationState() != DISABLE_DEACTIVATION)
|
||||
body->SetActivationState( ACTIVE_TAG );
|
||||
}
|
||||
|
||||
if (useIslands)
|
||||
@@ -803,12 +802,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
|
||||
for (int i=0;i<numVehicles;i++)
|
||||
{
|
||||
WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
|
||||
|
||||
for (int j=0;j<wrapperVehicle->GetVehicle()->GetNumWheels();j++)
|
||||
{
|
||||
wrapperVehicle->GetVehicle()->UpdateWheelTransform(j);
|
||||
}
|
||||
|
||||
|
||||
wrapperVehicle->SyncWheels();
|
||||
}
|
||||
#endif //NEW_BULLET_VEHICLE_SUPPORT
|
||||
@@ -1033,13 +1027,8 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
|
||||
ASSERT(rb0);
|
||||
|
||||
SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
|
||||
SimdVector3 axisInA(axisX,axisY,axisZ);
|
||||
|
||||
SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
|
||||
SimdVector3 axisInB = rb1 ? rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis()*(axisInA)) : axisInA;
|
||||
|
||||
|
||||
|
||||
switch (type)
|
||||
{
|
||||
case PHY_POINT2POINT_CONSTRAINT:
|
||||
@@ -1057,34 +1046,12 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
|
||||
pivotInA);
|
||||
}
|
||||
|
||||
m_constraints.push_back(p2p);
|
||||
m_p2pConstraints.push_back(p2p);
|
||||
p2p->SetUserConstraintId(gConstraintUid++);
|
||||
p2p->SetUserConstraintType(type);
|
||||
//64 bit systems can't cast pointer to int. could use size_t instead.
|
||||
return p2p->GetUserConstraintId();
|
||||
|
||||
break;
|
||||
}
|
||||
case PHY_LINEHINGE_CONSTRAINT:
|
||||
{
|
||||
HingeConstraint* hinge= 0;
|
||||
|
||||
if (rb1)
|
||||
{
|
||||
hinge = new HingeConstraint(*rb0,
|
||||
*rb1,pivotInA,pivotInB,axisInA,axisInB);
|
||||
} else
|
||||
{
|
||||
hinge = new HingeConstraint(*rb0,
|
||||
pivotInA,axisInA);
|
||||
}
|
||||
|
||||
m_constraints.push_back(hinge);
|
||||
hinge->SetUserConstraintId(gConstraintUid++);
|
||||
hinge->SetUserConstraintType(type);
|
||||
//64 bit systems can't cast pointer to int. could use size_t instead.
|
||||
return hinge->GetUserConstraintId();
|
||||
|
||||
break;
|
||||
}
|
||||
#ifdef NEW_BULLET_VEHICLE_SUPPORT
|
||||
@@ -1118,16 +1085,19 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
|
||||
|
||||
void CcdPhysicsEnvironment::removeConstraint(int constraintId)
|
||||
{
|
||||
std::vector<TypedConstraint*>::iterator i;
|
||||
std::vector<Point2PointConstraint*>::iterator i;
|
||||
|
||||
for (i=m_constraints.begin();
|
||||
!(i==m_constraints.end()); i++)
|
||||
//std::find(m_p2pConstraints.begin(), m_p2pConstraints.end(),
|
||||
// (Point2PointConstraint *)p2p);
|
||||
|
||||
for (i=m_p2pConstraints.begin();
|
||||
!(i==m_p2pConstraints.end()); i++)
|
||||
{
|
||||
TypedConstraint* p2p = (*i);
|
||||
Point2PointConstraint* p2p = (*i);
|
||||
if (p2p->GetUserConstraintId() == constraintId)
|
||||
{
|
||||
std::swap(*i, m_constraints.back());
|
||||
m_constraints.pop_back();
|
||||
std::swap(*i, m_p2pConstraints.back());
|
||||
m_p2pConstraints.pop_back();
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -1350,3 +1320,4 @@ PHY_IVehicle* CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
|
||||
}
|
||||
|
||||
#endif //NEW_BULLET_VEHICLE_SUPPORT
|
||||
|
||||
|
||||
@@ -1,18 +1,3 @@
|
||||
/*
|
||||
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 CCDPHYSICSENVIRONMENT
|
||||
#define CCDPHYSICSENVIRONMENT
|
||||
|
||||
@@ -22,15 +7,14 @@ class CcdPhysicsController;
|
||||
#include "SimdVector3.h"
|
||||
|
||||
|
||||
class PHY_IVehicle;
|
||||
class TypedConstraint;
|
||||
|
||||
class Point2PointConstraint;
|
||||
class CollisionDispatcher;
|
||||
class Dispatcher;
|
||||
//#include "BroadphaseInterface.h"
|
||||
|
||||
//switch on/off new vehicle support
|
||||
//vehicles not available yet, under development in Blender CVS branch
|
||||
//#define NEW_BULLET_VEHICLE_SUPPORT 1
|
||||
#define NEW_BULLET_VEHICLE_SUPPORT 1
|
||||
|
||||
#include "ConstraintSolver/ContactSolverInfo.h"
|
||||
|
||||
@@ -160,7 +144,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
|
||||
|
||||
std::vector<CcdPhysicsController*> m_controllers;
|
||||
|
||||
std::vector<TypedConstraint*> m_constraints;
|
||||
std::vector<Point2PointConstraint*> m_p2pConstraints;
|
||||
|
||||
std::vector<WrapperVehicle*> m_wrapperVehicles;
|
||||
|
||||
|
||||
@@ -294,7 +294,7 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV
|
||||
|
||||
CcdPhysicsEnvironment::CcdPhysicsEnvironment(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase)
|
||||
:m_scalingPropagated(false),
|
||||
m_numIterations(30),
|
||||
m_numIterations(10),
|
||||
m_ccdMode(0),
|
||||
m_solverType(-1)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user