diff options
10 files changed, 106 insertions, 5 deletions
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp index 12e02f2837a..0e5c7cf5715 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp @@ -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()); diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp index 64b470f4391..b3e25e0fb45 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp @@ -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()); diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.cpp index d1605b7a030..61ea8ace7d6 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.cpp @@ -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)); diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.h b/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.h index 0bc18075d19..bc90814de44 100755 --- a/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.h @@ -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 diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index d5fd194dd99..2fee6d06963 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -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); + + + } + } diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h index 9de7053d271..73bd9f8c95e 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h @@ -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(); diff --git a/source/gameengine/Ketsji/KX_PyConstraintBinding.cpp b/source/gameengine/Ketsji/KX_PyConstraintBinding.cpp index 28138195a5e..6fbf2888bd9 100644 --- a/source/gameengine/Ketsji/KX_PyConstraintBinding.cpp +++ b/source/gameengine/Ketsji/KX_PyConstraintBinding.cpp @@ -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 } diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp index 6ff471c4bd5..2fee6d06963 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp @@ -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; diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h index 9de7053d271..73bd9f8c95e 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h @@ -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(); diff --git a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h index cdd67257de2..fe4370fa725 100644 --- a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h +++ b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h @@ -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; |