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

dirty commit: experimenting with the 6DoF grabbing/p2p constraint

Browse Source
This commit is contained in:
kubas
2014-01-09 01:03:20 +01:00
parent c0530d31ec
commit 81447aa7c5
5 changed files with 296 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@@ -1,5 +1,6 @@
#include "btMultiBodyConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
:m_bodyA(bodyA),
@@ -296,16 +297,20 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
}
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
solverConstraint.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
#endif
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
if(multiBodyA->isMultiDof())
multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
else
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
#endif
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
if(multiBodyA->isMultiDof())
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);