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

197
BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp
View File

@@ -24,148 +24,113 @@ Generic6DofConstraint::Generic6DofConstraint()
{
}
Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB,
SimdVector3& axisInA,SimdVector3& axisInB)
:TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
m_axisInA(axisInA),
m_axisInB(axisInB)
{
}
Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA,const SimdVector3& pivotInA,SimdVector3& axisInA)
:TypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
m_axisInA(axisInA),
//fixed axis in worldspace
m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA)
Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB )
: TypedConstraint(rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB)
{
}
void Generic6DofConstraint::BuildJacobian()
void Generic6DofConstraint::BuildJacobian()
{
SimdVector3 normal(0,0,0);
const SimdVector3& pivotInA = m_frameInA.getOrigin();
const SimdVector3& pivotInB = m_frameInB.getOrigin();
//linear part
for (int i=0;i<3;i++)
{
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;
}
normal[i] = 1;
new (&m_jac[i]) JacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
m_rbB.getCenterOfMassTransform()*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
//these two jointAxis require equal angular velocities for both bodies
//this is unused for now, it's a todo
SimdVector3 axisWorldA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
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());
// angular part
for (int i=0;i<3;i++)
{
SimdVector3 axisInA = m_frameInA.getBasis().getColumn(i);
SimdVector3 axisInB = m_frameInA.getBasis().getColumn(i);
new (&m_jacAng[i]) JacobianEntry(axisInA, axisInB,
m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal());
}
}
void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
{
SimdScalar tau = 0.3f;
SimdScalar damping = 1.0f;
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
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);
SimdScalar tau = 0.3f;
SimdScalar damping = 1.f;
//linear part
{
for (int i=0;i<3;i++)
{
normal[i] = 1;
SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
// linear
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;
//velocity error (first order error)
SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);
//positional error (zeroth order error)
SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal);
SimdScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
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, rel_pos1);
m_rbB.applyImpulse(-impulse_vector, rel_pos2);
normal[i] = 0;
}
///solve angular part
// 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)
// angular
for (int i=0;i<3;i++)
{
SimdVector3 normal = velrel.normalized();
float denom = GetRigidBodyA().ComputeAngularImpulseDenominator(normal) +
GetRigidBodyB().ComputeAngularImpulseDenominator(normal);
// scale for mass and relaxation
velrel *= (1.f/denom) * 0.9;
SimdScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal();
//velocity error (first order error)
SimdScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
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)

19
BulletDynamics/ConstraintSolver/Generic6DofConstraint.h
View File

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

16
BulletDynamics/ConstraintSolver/JacobianEntry.h
View File

@@ -70,6 +70,22 @@ public:
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
JacobianEntry(
const SimdMatrix3x3& world2A,

4
Demos/ConstraintDemo/ConstraintDemo.cpp
View File

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

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

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