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++)
{
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,
m_rbA.getInvInertiaDiagLocal(),
@@ -73,7 +73,7 @@ void Generic6DofConstraint::BuildJacobian()
void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
{
SimdScalar tau = 0.3f;
SimdScalar tau = 0.0f;
SimdScalar damping = 1.0f;
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_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();
//#define JACOBIAN_STYLE
#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);
// linear
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;
SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
@@ -112,6 +121,11 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
// angular
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();
//velocity error (first order error)
@@ -128,8 +142,8 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
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);
m_rbA.applyTorqueImpulse( impulse_vector);
m_rbB.applyTorqueImpulse(-impulse_vector);
}
}

2
BulletDynamics/ConstraintSolver/JacobianEntry.h
View File

@@ -77,7 +77,7 @@ public:
const SimdVector3& inertiaInvB)
: m_linearJointAxis(SimdVector3(0.f,0.f,0.f))
, m_aJ(axisInA)
, m_bJ(axisInB)
, m_bJ(-axisInB)
{
m_0MinvJt = inertiaInvA * m_aJ;
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 axisInA(axisX,axisY,axisZ);
SimdVector3 axisInB = rb1 ?
(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) :
rb0->getCenterOfMassTransform().getBasis() * -axisInA;
(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) :
rb0->getCenterOfMassTransform().getBasis() * axisInA;
bool angularOnly = false;
@@ -1195,9 +1195,19 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
{
SimdTransform frameInA;
SimdTransform frameInB;
SimdVector3 axis1, axis2;
SimdPlaneSpace1( axisInA, axis1, axis2 );
frameInA.setIdentity();
frameInB.setIdentity();
frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
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 );
frameInB.setOrigin( pivotInB );