diff options
author | Erwin Coumans <blender@erwincoumans.com> | 2006-07-03 09:58:23 +0400 |
---|---|---|
committer | Erwin Coumans <blender@erwincoumans.com> | 2006-07-03 09:58:23 +0400 |
commit | 3b1a592fb7a4ce3fb0582bcdf0472eef43b9778c (patch) | |
tree | e750f88902b9cc23c461c8589dfe9bc5b9707d8b /extern | |
parent | da83509dca485b893aa76511d03a7e3aa8ff8a1f (diff) |
debug lines / register the applied impulse for constraint (for breaking)
Diffstat (limited to 'extern')
6 files changed, 58 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(); |