archive/blender-archive
Archived
1
1
Fork 0
Code Pull Requests Packages Activity

debug lines / register the applied impulse for constraint (for breaking)

Browse Source
This commit is contained in:
Erwin Coumans
2006-07-03 05:58:23 +00:00
parent da83509dca
commit 3b1a592fb7
10 changed files with 106 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp vendored
View File

@@ -47,6 +47,8 @@ m_angularOnly(false)
void HingeConstraint::BuildJacobian()
{
m_appliedImpulse = 0.f;
SimdVector3 normal(0,0,0);
if (!m_angularOnly)
@@ -120,7 +122,7 @@ void HingeConstraint::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;
m_appliedImpulse += impulse;
SimdVector3 impulse_vector = normal * impulse;
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());

4
extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp vendored
View File

@@ -40,6 +40,8 @@ Point2PointConstraint::Point2PointConstraint(RigidBody& rbA,const SimdVector3& p
void Point2PointConstraint::BuildJacobian()
{
m_appliedImpulse = 0.f;
SimdVector3 normal(0,0,0);
for (int i=0;i<3;i++)
@@ -98,7 +100,7 @@ void Point2PointConstraint::SolveConstraint(SimdScalar timeStep)
SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
SimdScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
m_appliedImpulse+=impulse;
SimdVector3 impulse_vector = normal * impulse;
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());

9
extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.cpp vendored
View File

@@ -24,7 +24,8 @@ TypedConstraint::TypedConstraint()
: m_userConstraintType(-1),
m_userConstraintId(-1),
m_rbA(s_fixed),
m_rbB(s_fixed)
m_rbB(s_fixed),
m_appliedImpulse(0.f)
{
s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
}
@@ -32,7 +33,8 @@ TypedConstraint::TypedConstraint(RigidBody& rbA)
: m_userConstraintType(-1),
m_userConstraintId(-1),
m_rbA(rbA),
m_rbB(s_fixed)
m_rbB(s_fixed),
m_appliedImpulse(0.f)
{
s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
@@ -43,7 +45,8 @@ TypedConstraint::TypedConstraint(RigidBody& rbA,RigidBody& rbB)
: m_userConstraintType(-1),
m_userConstraintId(-1),
m_rbA(rbA),
m_rbB(rbB)
m_rbB(rbB),
m_appliedImpulse(0.f)
{
s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));

7
extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.h vendored
View File

@@ -24,10 +24,13 @@ class TypedConstraint
{
int m_userConstraintType;
int m_userConstraintId;
protected:
RigidBody& m_rbA;
RigidBody& m_rbB;
float m_appliedImpulse;
public:
@@ -78,6 +81,10 @@ public:
{
return m_userConstraintId;
}
float GetAppliedImpulse()
{
return m_appliedImpulse;
}
};
#endif //TYPED_CONSTRAINT_H

38
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp vendored
View File

@@ -1271,6 +1271,21 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
}
float CcdPhysicsEnvironment::getAppliedImpulse(int constraintid)
{
std::vector<TypedConstraint*>::iterator i;
for (i=m_constraints.begin();
!(i==m_constraints.end()); i++)
{
TypedConstraint* constraint = (*i);
if (constraint->GetUserConstraintId() == constraintid)
{
return constraint->GetAppliedImpulse();
}
}
return 0.f;
}
void CcdPhysicsEnvironment::removeConstraint(int constraintId)
{
std::vector<TypedConstraint*>::iterator i;
@@ -1337,6 +1352,13 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
SimdVector3 rayFrom(fromX,fromY,fromZ);
SimdVector3 rayTo(toX,toY,toZ);
if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
{
SimdVector3 color (1,0,0);
m_debugDrawer->DrawLine(rayFrom,rayTo,color);
}
SimdVector3 hitPointWorld,normalWorld;
//Either Ray Cast with or without filtering
@@ -1354,10 +1376,26 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
hitX = rayCallback.m_hitPointWorld.getX();
hitY = rayCallback.m_hitPointWorld.getY();
hitZ = rayCallback.m_hitPointWorld.getZ();
if (rayCallback.m_hitNormalWorld.length2() > SIMD_EPSILON)
{
rayCallback.m_hitNormalWorld.normalize();
}
normalX = rayCallback.m_hitNormalWorld.getX();
normalY = rayCallback.m_hitNormalWorld.getY();
normalZ = rayCallback.m_hitNormalWorld.getZ();
if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
{
SimdVector3 colorNormal(0,0,1);
m_debugDrawer->DrawLine(rayCallback.m_hitPointWorld,rayCallback.m_hitPointWorld+rayCallback.m_hitNormalWorld,colorNormal);
SimdVector3 color (0,1,0);
m_debugDrawer->DrawLine(rayFrom,rayCallback.m_hitPointWorld,color);
}
}

1
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h vendored
View File

@@ -113,6 +113,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
float pivotX,float pivotY,float pivotZ,
float axisX,float axisY,float axisZ);
virtual void removeConstraint(int constraintid);
virtual float getAppliedImpulse(int constraintid);
virtual void CallbackTriggers();

31
source/gameengine/Ketsji/KX_PyConstraintBinding.cpp
View File

@@ -73,6 +73,8 @@ static char gPySetSolverType__doc__[] = "setSolverType(int solverType) Very expe
static char gPyCreateConstraint__doc__[] = "createConstraint(ob1,ob2,float restLength,float restitution,float damping)";
static char gPyGetVehicleConstraint__doc__[] = "getVehicleConstraint(int constraintId)";
static char gPyRemoveConstraint__doc__[] = "removeConstraint(int constraintId)";
static char gPyGetAppliedImpulse__doc__[] = "getAppliedImpulse(int constraintId)";
@@ -401,6 +403,32 @@ static PyObject* gPyCreateConstraint(PyObject* self,
}
static PyObject* gPyGetAppliedImpulse(PyObject* self,
PyObject* args,
PyObject* kwds)
{
float appliedImpulse = 0.f;
#if defined(_WIN64)
__int64 constraintid;
if (PyArg_ParseTuple(args,"L",&constraintid))
#else
long constraintid;
if (PyArg_ParseTuple(args,"l",&constraintid))
#endif
{
if (PHY_GetActiveEnvironment())
{
appliedImpulse = PHY_GetActiveEnvironment()->getAppliedImpulse(constraintid);
}
}
return PyFloat_FromDouble(appliedImpulse);
}
static PyObject* gPyRemoveConstraint(PyObject* self,
PyObject* args,
PyObject* kwds)
@@ -470,6 +498,9 @@ static struct PyMethodDef physicsconstraints_methods[] = {
{"removeConstraint",(PyCFunction) gPyRemoveConstraint,
METH_VARARGS, gPyRemoveConstraint__doc__},
{"getAppliedImpulse",(PyCFunction) gPyGetAppliedImpulse,
METH_VARARGS, gPyGetAppliedImpulse__doc__},
//sentinel
{ NULL, (PyCFunction) NULL, 0, NULL }

15
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
View File

@@ -1271,6 +1271,21 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
}
float CcdPhysicsEnvironment::getAppliedImpulse(int constraintid)
{
std::vector<TypedConstraint*>::iterator i;
for (i=m_constraints.begin();
!(i==m_constraints.end()); i++)
{
TypedConstraint* constraint = (*i);
if (constraint->GetUserConstraintId() == constraintid)
{
return constraint->GetAppliedImpulse();
}
}
return 0.f;
}
void CcdPhysicsEnvironment::removeConstraint(int constraintId)
{
std::vector<TypedConstraint*>::iterator i;

1
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
View File

@@ -113,6 +113,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
float pivotX,float pivotY,float pivotZ,
float axisX,float axisY,float axisZ);
virtual void removeConstraint(int constraintid);
virtual float getAppliedImpulse(int constraintid);
virtual void CallbackTriggers();

1
source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
View File

@@ -88,6 +88,7 @@ class PHY_IPhysicsEnvironment
float pivotX,float pivotY,float pivotZ,
float axisX,float axisY,float axisZ)=0;
virtual void removeConstraint(int constraintid)=0;
virtual float getAppliedImpulse(int constraintid){ return 0.f;}
//complex constraint for vehicles
virtual PHY_IVehicle* getVehicleConstraint(int constraintId) =0;