23 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
24 #define BTMBP2PCONSTRAINT_DIM 3
26 #define BTMBP2PCONSTRAINT_DIM 6
62 return m_rigidBodyA->getIslandTag();
70 return col->getIslandTag();
74 if (m_bodyA->getLink(
m_linkA).m_collider)
75 return m_bodyA->getLink(
m_linkA).m_collider->getIslandTag();
91 return col->getIslandTag();
108 for (
int i = 0; i < numDim; i++)
112 constraintRow.m_orgConstraint =
this;
113 constraintRow.m_orgDofIndex = i;
114 constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
115 constraintRow.m_contactNormal1.setValue(0, 0, 0);
116 constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
117 constraintRow.m_contactNormal2.setValue(0, 0, 0);
118 constraintRow.m_angularComponentA.setValue(0, 0, 0);
119 constraintRow.m_angularComponentB.setValue(0, 0, 0);
121 constraintRow.m_solverBodyIdA =
data.m_fixedBodyId;
122 constraintRow.m_solverBodyIdB =
data.m_fixedBodyId;
125 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
126 contactNormalOnB[i] = -1;
128 contactNormalOnB[i % 3] = -1;
135 constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
136 pivotAworld = m_rigidBodyA->getCenterOfMassTransform() *
m_pivotInA;
146 constraintRow.m_solverBodyIdB =
m_rigidBodyB->getCompanionId();
155 btScalar posError = i < 3 ? (pivotAworld - pivotBworld).
dot(contactNormalOnB) : 0;
157 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
160 contactNormalOnB, pivotAworld, pivotBworld,
172 const btVector3& normalAng = i >= 3 ? contactNormalOnB : dummy;
173 const btVector3& normalLin = i < 3 ? contactNormalOnB : dummy;
175 m_bodyA->filConstraintJacobianMultiDof(
m_linkA, pivotAworld, normalAng, normalLin, jac1,
data.scratch_r,
data.scratch_v,
data.scratch_m);
200 tr.setOrigin(pivotAworld);
213 tr.setOrigin(pivotBworld);
void debugDraw(btIDebugDraw *debugDrawer)
btActionInterface interface
@ MULTIBODY_CONSTRAINT_POINT_TO_POINT
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)=0
btAlignedObjectArray< btScalar > m_data
virtual int getIslandIdA() const =0
virtual int getIslandIdB() const =0
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btVector3 &constraintNormalAng, const btVector3 &constraintNormalLin, const btVector3 &posAworld, const btVector3 &posBworld, btScalar posError, const btContactSolverInfo &infoGlobal, btScalar lowerLimit, btScalar upperLimit, bool angConstraint=false, btScalar relaxation=1.f, bool isFriction=false, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btScalar m_maxAppliedImpulse
btScalar * jacobianA(int row)
#define BTMBP2PCONSTRAINT_DIM
This file was written by Erwin Coumans.
btMultiBodyPoint2Point(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB)
virtual ~btMultiBodyPoint2Point()
btRigidBody * m_rigidBodyB
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE T & expandNonInitializing()
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
const btTransform & getCenterOfMassTransform() const
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)