Code: Select all
dispatcher = new btCollisionDispatcher(&config);
broadphase = new btDbvtBroadphase();
solver = new btMultiBodyConstraintSolver;
world = new btMultiBodyDynamicsWorld(dispatcher, broadphase, solver, &config);
m_world->dynamicWorldRef()->setInternalTickCallback(OgreIScene::onPreTickCallback, this, true);
I found this link
https://github.com/bulletphysics/bullet3/issues/335
and when I looked noticed btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish had a comment
//do a callback here?
which is ... interesting.
my code for the pre-tick callback might not be relevant, might be very wrong idk, but here it is
Code: Select all
void GroundReaction::addContactForce(btDispatcher* dispatcher,
const Eigen::Vector3d& com,
const std::vector<int>& linkToBodyIndex,
const rbd::MultiBody& mb,
rbd::MultiBodyConfig& mbc,
const btMultiBody* btmb,
btScalar timeStep)
{
auto log_size = []( const std::string& name, const Eigen::MatrixXd& m ) { std::cout << name << " (" << m.rows() << ", " << m.cols() << ")" << std::endl; };
auto to_Vector3d = []( const btVector3& v ) -> Eigen::Vector3d { return Eigen::Vector3d( v.x(), v.y(), v.z() ); };
auto skew_matrix = [](const Eigen::Vector3d &v) -> Eigen::Matrix3d
{
Eigen::Matrix3d m;
m << 0., -v[2], v[1],
v[2], 0., -v[0],
-v[1], v[0], 0.;
return m;
};
int nrDof = mb.nrDof();
Eigen::VectorXd wrench(6);
wrench.setZero();
Eigen::MatrixXd jacLambda(1, nrDof);
Eigen::MatrixXd fullJacLambda(1, nrDof);
fullJacLambda.setZero();
int numManifolds = dispatcher->getNumManifolds();
for (int i = 0; i < numManifolds; i++)
{
btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
int numContacts = manifold->getNumContacts();
if(numContacts > 0)
{
const btMultiBodyLinkCollider* obA = btMultiBodyLinkCollider::upcast( manifold->getBody0() );
if( obA && obA->m_multiBody == btmb )
{
int body = linkToBodyIndex[ obA->m_link + 1 ];
const Eigen::MatrixXd& jac = jacVec_[body].bodyJacobian(mb, mbc);
for (int j = 0; j < numContacts; j++)
{
btManifoldPoint& cp = manifold->getContactPoint(j);
Eigen::Vector3d pos = to_Vector3d( cp.m_positionWorldOnB );
Eigen::Vector3d force = to_Vector3d( cp.m_normalWorldOnB * cp.m_appliedImpulse/timeStep );
std::cout << "impulse " << cp.m_appliedImpulse << std::endl;
Eigen::Vector3d pt = (sva::PTransformd(pos) * mbc.bodyPosW[body].inv()).translation();
jacVec_[body].translateBodyJacobian(jac, mbc, pt, jacTrans_);
Eigen::Vector3d torque = skew_matrix(pos - com) * force;
wrench.segment(0, 3) = torque;
wrench.segment(3, 3) = force;
//std::cout << "impulse " << cp.m_appliedImpulse << std::endl << force.transpose() << std::endl << torque.transpose() << std::endl << std::endl;
jacLambda = wrench.transpose() * jacTrans_.block(0, 0, 6, jacVec_[body].dof());
jacVec_[body].addFullJacobian(mb,
jacLambda.block(0, 0, 1, jacVec_[body].dof()),
fullJacLambda);
}
}
}
}
std::cout << "fullJacLambda " << fullJacLambda << std::endl << std::endl;
int pos = mb.joint(0).dof();
for(std::size_t i = 1; i < mbc.jointTorque.size(); ++i)
{
for(double& d : mbc.jointTorque[i])
{
d -= fullJacLambda(pos);
++pos;
}
}
}