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

dgregorius: changes to generic constraint

Browse Source
This commit is contained in:
dondickied
2006-06-15 13:37:49 +00:00
parent 43f5e2af0f
commit 6dfe8389a1
3 changed files with 35 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp
View File

@@ -63,7 +63,7 @@ void Generic6DofConstraint::BuildJacobian()
for (int i=0;i<3;i++) for (int i=0;i<3;i++)
{ {
SimdVector3 axisInA = m_frameInA.getBasis().getColumn(i); SimdVector3 axisInA = m_frameInA.getBasis().getColumn(i);
SimdVector3 axisInB = m_frameInA.getBasis().getColumn(i); SimdVector3 axisInB = m_frameInB.getBasis().getColumn(i);
new (&m_jacAng[i]) JacobianEntry(axisInA, axisInB, new (&m_jacAng[i]) JacobianEntry(axisInA, axisInB,
m_rbA.getInvInertiaDiagLocal(), m_rbA.getInvInertiaDiagLocal(),
@@ -73,7 +73,7 @@ void Generic6DofConstraint::BuildJacobian()
void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep) void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
{ {
SimdScalar tau = 0.3f; SimdScalar tau = 0.0f;
SimdScalar damping = 1.0f; SimdScalar damping = 1.0f;
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
@@ -82,14 +82,23 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); //#define JACOBIAN_STYLE
SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
#ifdef JACOBIAN_STYLE
SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
#endif
SimdVector3 normal(0,0,0); SimdVector3 normal(0,0,0);
// linear // linear
for (int i=0;i<3;i++) for (int i=0;i<3;i++)
{ {
#ifndef JACOBIAN_STYLE
SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
#endif
normal[i] = 1; normal[i] = 1;
SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
@@ -112,6 +121,11 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
// angular // angular
for (int i=0;i<3;i++) for (int i=0;i<3;i++)
{ {
#ifndef JACOBIAN_STYLE
SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
#endif
SimdScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal(); SimdScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal();
//velocity error (first order error) //velocity error (first order error)
@@ -128,8 +142,8 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
SimdScalar impulse = (tau*rel_pos/timeStep - damping*rel_vel) * jacDiagABInv; SimdScalar impulse = (tau*rel_pos/timeStep - damping*rel_vel) * jacDiagABInv;
SimdVector3 impulse_vector = axisA * impulse; SimdVector3 impulse_vector = axisA * impulse;
//m_rbA.applyTorqueImpulse( impulse_vector); m_rbA.applyTorqueImpulse( impulse_vector);
//m_rbB.applyTorqueImpulse(-impulse_vector); m_rbB.applyTorqueImpulse(-impulse_vector);
} }
} }

2
BulletDynamics/ConstraintSolver/JacobianEntry.h
View File

@@ -77,7 +77,7 @@ public:
const SimdVector3& inertiaInvB) const SimdVector3& inertiaInvB)
: m_linearJointAxis(SimdVector3(0.f,0.f,0.f)) : m_linearJointAxis(SimdVector3(0.f,0.f,0.f))
, m_aJ(axisInA) , m_aJ(axisInA)
, m_bJ(axisInB) , m_bJ(-axisInB)
{ {
m_0MinvJt = inertiaInvA * m_aJ; m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ; m_1MinvJt = inertiaInvB * m_bJ;

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

@@ -1156,8 +1156,8 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA; SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
SimdVector3 axisInA(axisX,axisY,axisZ); SimdVector3 axisInA(axisX,axisY,axisZ);
SimdVector3 axisInB = rb1 ? SimdVector3 axisInB = rb1 ?
(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) :
rb0->getCenterOfMassTransform().getBasis() * -axisInA; rb0->getCenterOfMassTransform().getBasis() * axisInA;
bool angularOnly = false; bool angularOnly = false;
@@ -1195,9 +1195,19 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
{ {
SimdTransform frameInA; SimdTransform frameInA;
SimdTransform frameInB; SimdTransform frameInB;
SimdVector3 axis1, axis2;
SimdPlaneSpace1( axisInA, axis1, axis2 );
frameInA.setIdentity(); frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
frameInB.setIdentity(); axisInA.y(), axis1.y(), axis2.y(),
axisInA.z(), axis1.z(), axis2.z() );
SimdPlaneSpace1( axisInB, axis1, axis2 );
frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
axisInB.y(), axis1.y(), axis2.y(),
axisInB.z(), axis1.z(), axis2.z() );
frameInA.setOrigin( pivotInA ); frameInA.setOrigin( pivotInA );
frameInB.setOrigin( pivotInB ); frameInB.setOrigin( pivotInB );