forked from bartvdbraak/blender
added Bullet rigidbodies that behave like Sumo 'dynamic', without rotations. Done using a special hinge constraint (no translational degrees of freedom removed)
This commit is contained in:
parent
45d0123a59
commit
f9597df32d
@ -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
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
@ -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);
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user