Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Browse Source
This commit is contained in:
dondickied
2006-06-13 21:48:15 +00:00
parent a88cee3a20
commit 63e05649ee
5 changed files with 120 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

189
BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp
View File

@@ -24,148 +24,113 @@ Generic6DofConstraint::Generic6DofConstraint()
{ {
} }
Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB, Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB )
SimdVector3& axisInA,SimdVector3& axisInB) : TypedConstraint(rbA, rbB)
:TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), , m_frameInA(frameInA)
m_axisInA(axisInA), , m_frameInB(frameInB)
m_axisInB(axisInB)
{ {
} }
Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA,const SimdVector3& pivotInA,SimdVector3& axisInA) void Generic6DofConstraint::BuildJacobian()
:TypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
m_axisInA(axisInA),
//fixed axis in worldspace
m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA)
{
}
void Generic6DofConstraint::BuildJacobian()
{ {
SimdVector3 normal(0,0,0); SimdVector3 normal(0,0,0);
const SimdVector3& pivotInA = m_frameInA.getOrigin();
const SimdVector3& pivotInB = m_frameInB.getOrigin();
//linear part //linear part
for (int i=0;i<3;i++)
{ {
for (int i=0;i<3;i++) normal[i] = 1;
{
normal[i] = 1; new (&m_jac[i]) JacobianEntry(
new (&m_jac[i]) JacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(), m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(),
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), normal,
normal, m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvInertiaDiagLocal(), m_rbA.getInvMass(),
m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass());
m_rbB.getInvMass());
normal[i] = 0; normal[i] = 0;
}
} }
//calculate two perpendicular jointAxis, orthogonal to hingeAxis // angular part
//these two jointAxis require equal angular velocities for both bodies for (int i=0;i<3;i++)
{
//this is unused for now, it's a todo SimdVector3 axisInA = m_frameInA.getBasis().getColumn(i);
SimdVector3 axisWorldA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; SimdVector3 axisInB = m_frameInA.getBasis().getColumn(i);
SimdVector3 jointAxis0;
SimdVector3 jointAxis1;
SimdPlaneSpace1(axisWorldA,jointAxis0,jointAxis1);
new (&m_jacAng[0]) JacobianEntry(jointAxis0,
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal());
new (&m_jacAng[1]) JacobianEntry(jointAxis1,
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal());
new (&m_jacAng[i]) JacobianEntry(axisInA, axisInB,
m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal());
}
} }
void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep) void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
{ {
SimdScalar tau = 0.3f;
SimdScalar damping = 1.0f;
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
SimdVector3 normal(0,0,0); SimdVector3 normal(0,0,0);
SimdScalar tau = 0.3f;
SimdScalar damping = 1.f;
//linear part // linear
for (int i=0;i<3;i++)
{ {
for (int i=0;i<3;i++) normal[i] = 1;
{ SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
normal[i] = 1;
SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); //velocity error (first order error)
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);
SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); //positional error (zeroth order error)
SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal);
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; SimdScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
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, rel_pos1);
m_rbB.applyImpulse(-impulse_vector, rel_pos2);
normal[i] = 0;
} }
///solve angular part // angular
for (int i=0;i<3;i++)
// get axes in world space
SimdVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
SimdVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
const SimdVector3& angVelA = GetRigidBodyA().getAngularVelocity();
const SimdVector3& angVelB = GetRigidBodyB().getAngularVelocity();
SimdVector3 angA = angVelA - axisA * axisA.dot(angVelA);
SimdVector3 angB = angVelB - axisB * axisB.dot(angVelB);
SimdVector3 velrel = angA-angB;
//solve angular velocity correction
float relaxation = 1.f;
float len = velrel.length();
if (len > 0.00001f)
{ {
SimdVector3 normal = velrel.normalized(); SimdScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal();
float denom = GetRigidBodyA().ComputeAngularImpulseDenominator(normal) +
GetRigidBodyB().ComputeAngularImpulseDenominator(normal); //velocity error (first order error)
// scale for mass and relaxation SimdScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
velrel *= (1.f/denom) * 0.9; m_rbB.getLinearVelocity(),angvelB);
//positional error (zeroth order error)
SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(i);
SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(i);
SimdScalar rel_pos = 0.0f * axisB.dot(axisA);
//impulse
SimdScalar impulse = (tau*rel_pos/timeStep - damping*rel_vel) * jacDiagABInv;
SimdVector3 impulse_vector = axisA * impulse;
//m_rbA.applyTorqueImpulse( impulse_vector);
//m_rbB.applyTorqueImpulse(-impulse_vector);
} }
//solve angular positional correction
SimdVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
float len2 = angularError.length();
if (len2>0.00001f)
{
SimdVector3 normal2 = angularError.normalized();
float denom2 = GetRigidBodyA().ComputeAngularImpulseDenominator(normal2) +
GetRigidBodyB().ComputeAngularImpulseDenominator(normal2);
angularError *= (1.f/denom2) * relaxation;
}
m_rbA.applyTorqueImpulse(-velrel+angularError);
m_rbB.applyTorqueImpulse(velrel-angularError);
} }
void Generic6DofConstraint::UpdateRHS(SimdScalar timeStep) void Generic6DofConstraint::UpdateRHS(SimdScalar timeStep)

19
BulletDynamics/ConstraintSolver/Generic6DofConstraint.h
View File

@@ -30,20 +30,19 @@ class RigidBody;
/// Work in progress (is still a Hinge actually) /// Work in progress (is still a Hinge actually)
class Generic6DofConstraint : public TypedConstraint class Generic6DofConstraint : public TypedConstraint
{ {
JacobianEntry m_jac[3]; //3 orthogonal linear constraints JacobianEntry m_jac[3]; // 3 orthogonal linear constraints
JacobianEntry m_jacAng[3]; //3 orthogonal angular constraints JacobianEntry m_jacAng[3]; // 3 orthogonal angular constraints
SimdVector3 m_pivotInA;
SimdVector3 m_pivotInB; SimdTransform m_frameInA; // the constraint space w.r.t body A
SimdVector3 m_axisInA; SimdTransform m_frameInB; // the constraint space w.r.t body B
SimdVector3 m_axisInB;
SimdScalar m_lowerLimit[6]; // the constraint lower limits
SimdScalar m_upperLimit[6]; // the constraint upper limits
public: public:
Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB );
Generic6DofConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB,SimdVector3& axisInA,SimdVector3& axisInB);
Generic6DofConstraint(RigidBody& rbA,const SimdVector3& pivotInA,SimdVector3& axisInA);
Generic6DofConstraint(); Generic6DofConstraint();

16
BulletDynamics/ConstraintSolver/JacobianEntry.h
View File

@@ -70,6 +70,22 @@ public:
assert(m_Adiag > 0.0f); assert(m_Adiag > 0.0f);
} }
//angular constraint between two different rigidbodies
JacobianEntry(const SimdVector3& axisInA,
const SimdVector3& axisInB,
const SimdVector3& inertiaInvA,
const SimdVector3& inertiaInvB)
: m_jointAxis(m_jointAxis)
, m_aJ(axisInA)
, m_bJ(axisInB)
{
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
assert(m_Adiag > 0.0f);
}
//constraint on one rigidbody //constraint on one rigidbody
JacobianEntry( JacobianEntry(
const SimdMatrix3x3& world2A, const SimdMatrix3x3& world2A,

4
Demos/ConstraintDemo/ConstraintDemo.cpp
View File

@@ -198,8 +198,8 @@ int main(int argc,char** argv)
//0, //0,
physObjects[2], physObjects[2],
//PHY_POINT2POINT_CONSTRAINT, //PHY_POINT2POINT_CONSTRAINT,
//PHY_GENERIC_6DOF_CONSTRAINT,//can leave any of the 6 degree of freedom 'free' or 'locked' PHY_GENERIC_6DOF_CONSTRAINT,//can leave any of the 6 degree of freedom 'free' or 'locked'
PHY_LINEHINGE_CONSTRAINT, //PHY_LINEHINGE_CONSTRAINT,
pivotX,pivotY,pivotZ, pivotX,pivotY,pivotZ,
axisX,axisY,axisZ axisX,axisY,axisZ
); );

16
Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
View File

@@ -1193,15 +1193,23 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
if (rb1) if (rb1)
{ {
SimdTransform frameInA;
SimdTransform frameInB;
frameInA.setIdentity();
frameInB.setIdentity();
frameInA.setOrigin( pivotInA );
frameInB.setOrigin( pivotInB );
genericConstraint = new Generic6DofConstraint( genericConstraint = new Generic6DofConstraint(
*rb0, *rb0,*rb1,
*rb1,pivotInA,pivotInB,axisInA,axisInB); frameInA,frameInB);
} else } else
{ {
genericConstraint = new Generic6DofConstraint(*rb0, // TODO: Implement single body case...
pivotInA,axisInA);
} }