From f9597df32d6ed03c1552e4049d16465b5abb675b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 26 Apr 2006 06:01:46 +0000 Subject: [PATCH] added Bullet rigidbodies that behave like Sumo 'dynamic', without rotations. Done using a special hinge constraint (no translational degrees of freedom removed) --- .../ConstraintSolver/HingeConstraint.cpp | 78 ++++++++++--------- .../ConstraintSolver/HingeConstraint.h | 6 ++ .../CcdPhysics/CcdPhysicsEnvironment.cpp | 7 +- .../Ketsji/KX_ConvertPhysicsObjects.cpp | 20 +++++ .../Physics/Bullet/CcdPhysicsEnvironment.cpp | 7 +- .../Physics/common/PHY_DynamicTypes.h | 1 + 6 files changed, 80 insertions(+), 39 deletions(-) diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp index b24ec54b75d..12e02f2837a 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp @@ -28,7 +28,8 @@ HingeConstraint::HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector SimdVector3& axisInA,SimdVector3& axisInB) :TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), m_axisInA(axisInA), -m_axisInB(axisInB) +m_axisInB(axisInB), +m_angularOnly(false) { } @@ -38,7 +39,8 @@ HingeConstraint::HingeConstraint(RigidBody& rbA,const SimdVector3& pivotInA,Simd :TypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), m_axisInA(axisInA), //fixed axis in worldspace -m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA) +m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA), +m_angularOnly(false) { } @@ -47,20 +49,23 @@ void HingeConstraint::BuildJacobian() { SimdVector3 normal(0,0,0); - for (int i=0;i<3;i++) + if (!m_angularOnly) { - normal[i] = 1; - new (&m_jac[i]) JacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), - m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), - normal, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - normal[i] = 0; + for (int i=0;i<3;i++) + { + normal[i] = 1; + new (&m_jac[i]) JacobianEntry( + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), + m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), + normal, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + normal[i] = 0; + } } //calculate two perpendicular jointAxis, orthogonal to hingeAxis @@ -97,28 +102,31 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep) SimdScalar tau = 0.3f; SimdScalar damping = 1.f; - for (int i=0;i<3;i++) - { - normal[i] = 1; - SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + if (!m_angularOnly) + { + for (int i=0;i<3;i++) + { + normal[i] = 1; + SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); - SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - SimdVector3 vel = vel1 - vel2; - SimdScalar rel_vel; - rel_vel = normal.dot(vel); - //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; + SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); + SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); + SimdVector3 vel = vel1 - vel2; + SimdScalar rel_vel; + rel_vel = normal.dot(vel); + //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; - SimdVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); - m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); - - normal[i] = 0; + SimdVector3 impulse_vector = normal * impulse; + m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); + m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); + + normal[i] = 0; + } } ///solve angular part diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.h b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.h index c9d2b5adf6a..a43463511b1 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.h @@ -36,6 +36,7 @@ class HingeConstraint : public TypedConstraint SimdVector3 m_axisInA; SimdVector3 m_axisInB; + bool m_angularOnly; public: @@ -60,6 +61,11 @@ public: return m_rbB; } + void setAngularOnly(bool angularOnly) + { + m_angularOnly = angularOnly; + } + }; diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index fd4bca40e7f..5cb2859046c 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -508,7 +508,6 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) - if (!SimdFuzzyZero(timeStep)) { @@ -1153,6 +1152,8 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : rb0->getCenterOfMassTransform().getBasis() * -axisInA; + bool hingeAngularOnly = false; + switch (type) { case PHY_POINT2POINT_CONSTRAINT: @@ -1179,6 +1180,8 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl break; } + case PHY_ANGULAR_CONSTRAINT: + hingeAngularOnly = true; case PHY_LINEHINGE_CONSTRAINT: { HingeConstraint* hinge = 0; @@ -1196,7 +1199,7 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl pivotInA,axisInA); } - + hinge->setAngularOnly( hingeAngularOnly ); m_constraints.push_back(hinge); hinge->SetUserConstraintId(gConstraintUid++); hinge->SetUserConstraintType(type); diff --git a/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp b/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp index 876a2ff5e81..ddf6c5bb321 100644 --- a/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp +++ b/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp @@ -1062,6 +1062,7 @@ void KX_ConvertBulletObject( class KX_GameObject* gameobj, ci.m_collisionShape = bm; + ci.m_friction = smmaterial->m_friction;//tweak the friction a bit, so the default 0.5 works nice @@ -1088,6 +1089,25 @@ void KX_ConvertBulletObject( class KX_GameObject* gameobj, if (objprop->m_disableSleeping) physicscontroller->GetRigidBody()->SetActivationState(DISABLE_DEACTIVATION); + if (!objprop->m_angular_rigidbody) + { + /* + //setting the inertia could achieve similar results to constraint the up + //but it is prone to instability, so use special 'Angular' constraint + SimdVector3 inertia = physicscontroller->GetRigidBody()->getInvInertiaDiagLocal(); + inertia.setX(0.f); + inertia.setZ(0.f); + + physicscontroller->GetRigidBody()->setInvInertiaDiagLocal(inertia); + physicscontroller->GetRigidBody()->updateInertiaTensor(); + */ + + env->createConstraint(physicscontroller,0,PHY_ANGULAR_CONSTRAINT,0,0,0,0,0,1); + + + + } + bool isActor = objprop->m_isactor; gameobj->getClientInfo()->m_type = (isActor ? KX_ClientObjectInfo::ACTOR : KX_ClientObjectInfo::STATIC); // store materialname in auxinfo, needed for touchsensors diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp index fd4bca40e7f..5cb2859046c 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp @@ -508,7 +508,6 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) - if (!SimdFuzzyZero(timeStep)) { @@ -1153,6 +1152,8 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : rb0->getCenterOfMassTransform().getBasis() * -axisInA; + bool hingeAngularOnly = false; + switch (type) { case PHY_POINT2POINT_CONSTRAINT: @@ -1179,6 +1180,8 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl break; } + case PHY_ANGULAR_CONSTRAINT: + hingeAngularOnly = true; case PHY_LINEHINGE_CONSTRAINT: { HingeConstraint* hinge = 0; @@ -1196,7 +1199,7 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl pivotInA,axisInA); } - + hinge->setAngularOnly( hingeAngularOnly ); m_constraints.push_back(hinge); hinge->SetUserConstraintId(gConstraintUid++); hinge->SetUserConstraintType(type); diff --git a/source/gameengine/Physics/common/PHY_DynamicTypes.h b/source/gameengine/Physics/common/PHY_DynamicTypes.h index 54aaf240660..c9ca3221355 100644 --- a/source/gameengine/Physics/common/PHY_DynamicTypes.h +++ b/source/gameengine/Physics/common/PHY_DynamicTypes.h @@ -99,6 +99,7 @@ typedef enum PHY_PhysicsType { typedef enum PHY_ConstraintType { PHY_POINT2POINT_CONSTRAINT=1, PHY_LINEHINGE_CONSTRAINT=2, + PHY_ANGULAR_CONSTRAINT = 3,//hinge without ball socket PHY_VEHICLE_CONSTRAINT=11,//complex 'constraint' that turns a rigidbody into a vehicle } PHY_ConstraintType;