summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h')
-rw-r--r--thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h167
1 files changed, 79 insertions, 88 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h
index a2ae571273..5c15f3e851 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h
@@ -27,66 +27,62 @@ struct btSolverInfo;
struct btMultiBodyJacobianData
{
- btAlignedObjectArray<btScalar> m_jacobians;
- btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
- btAlignedObjectArray<btScalar> m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
- btAlignedObjectArray<btScalar> scratch_r;
- btAlignedObjectArray<btVector3> scratch_v;
- btAlignedObjectArray<btMatrix3x3> scratch_m;
- btAlignedObjectArray<btSolverBody>* m_solverBodyPool;
- int m_fixedBodyId;
-
+ btAlignedObjectArray<btScalar> m_jacobians;
+ btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
+ btAlignedObjectArray<btScalar> m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
+ btAlignedObjectArray<btScalar> scratch_r;
+ btAlignedObjectArray<btVector3> scratch_v;
+ btAlignedObjectArray<btMatrix3x3> scratch_m;
+ btAlignedObjectArray<btSolverBody>* m_solverBodyPool;
+ int m_fixedBodyId;
};
-
-ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraint
+ATTRIBUTE_ALIGNED16(class)
+btMultiBodyConstraint
{
protected:
-
- btMultiBody* m_bodyA;
- btMultiBody* m_bodyB;
- int m_linkA;
- int m_linkB;
-
- int m_numRows;
- int m_jacSizeA;
- int m_jacSizeBoth;
- int m_posOffset;
-
- bool m_isUnilateral;
- int m_numDofsFinalized;
- btScalar m_maxAppliedImpulse;
-
-
- // warning: the data block lay out is not consistent for all constraints
- // data block laid out as follows:
- // cached impulses. (one per row.)
- // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
- // positions. (one per row.)
- btAlignedObjectArray<btScalar> m_data;
-
- void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
-
- 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);
+ btMultiBody* m_bodyA;
+ btMultiBody* m_bodyB;
+ int m_linkA;
+ int m_linkB;
+
+ int m_numRows;
+ int m_jacSizeA;
+ int m_jacSizeBoth;
+ int m_posOffset;
+
+ bool m_isUnilateral;
+ int m_numDofsFinalized;
+ btScalar m_maxAppliedImpulse;
+
+ // warning: the data block lay out is not consistent for all constraints
+ // data block laid out as follows:
+ // cached impulses. (one per row.)
+ // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
+ // positions. (one per row.)
+ btAlignedObjectArray<btScalar> m_data;
+
+ void applyDeltaVee(btMultiBodyJacobianData & data, btScalar * delta_vee, btScalar impulse, int velocityIndex, int ndof);
+
+ 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);
public:
-
BT_DECLARE_ALIGNED_ALLOCATOR();
- btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
+ btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral);
virtual ~btMultiBodyConstraint();
void updateJacobianSizes();
@@ -94,27 +90,27 @@ public:
//many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later.
virtual void setFrameInB(const btMatrix3x3& frameInB) {}
- virtual void setPivotInB(const btVector3& pivotInB){}
+ virtual void setPivotInB(const btVector3& pivotInB) {}
- virtual void finalizeMultiDof()=0;
+ virtual void finalizeMultiDof() = 0;
- virtual int getIslandIdA() const =0;
- virtual int getIslandIdB() const =0;
+ virtual int getIslandIdA() const = 0;
+ virtual int getIslandIdB() const = 0;
- virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
- btMultiBodyJacobianData& data,
- const btContactSolverInfo& infoGlobal)=0;
+ virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows,
+ btMultiBodyJacobianData & data,
+ const btContactSolverInfo& infoGlobal) = 0;
- int getNumRows() const
+ int getNumRows() const
{
return m_numRows;
}
- btMultiBody* getMultiBodyA()
+ btMultiBody* getMultiBodyA()
{
return m_bodyA;
}
- btMultiBody* getMultiBodyB()
+ btMultiBody* getMultiBodyB()
{
return m_bodyB;
}
@@ -127,77 +123,72 @@ public:
{
return m_linkB;
}
- void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
+ void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
{
- btAssert(dof>=0);
+ btAssert(dof >= 0);
btAssert(dof < getNumRows());
m_data[dof] = appliedImpulse;
-
}
-
- btScalar getAppliedImpulse(int dof)
+
+ btScalar getAppliedImpulse(int dof)
{
- btAssert(dof>=0);
+ btAssert(dof >= 0);
btAssert(dof < getNumRows());
return m_data[dof];
}
// current constraint position
- // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
- // NOTE: ignored position for friction rows.
- btScalar getPosition(int row) const
+ // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
+ // NOTE: ignored position for friction rows.
+ btScalar getPosition(int row) const
{
return m_data[m_posOffset + row];
}
- void setPosition(int row, btScalar pos)
+ void setPosition(int row, btScalar pos)
{
m_data[m_posOffset + row] = pos;
}
-
bool isUnilateral() const
{
return m_isUnilateral;
}
// jacobian blocks.
- // each of size 6 + num_links. (jacobian2 is null if no body2.)
- // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
- btScalar* jacobianA(int row)
+ // each of size 6 + num_links. (jacobian2 is null if no body2.)
+ // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
+ btScalar* jacobianA(int row)
{
return &m_data[m_numRows + row * m_jacSizeBoth];
}
- const btScalar* jacobianA(int row) const
+ const btScalar* jacobianA(int row) const
{
return &m_data[m_numRows + (row * m_jacSizeBoth)];
}
- btScalar* jacobianB(int row)
+ btScalar* jacobianB(int row)
{
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
}
- const btScalar* jacobianB(int row) const
+ const btScalar* jacobianB(int row) const
{
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
}
- btScalar getMaxAppliedImpulse() const
+ btScalar getMaxAppliedImpulse() const
{
return m_maxAppliedImpulse;
}
- void setMaxAppliedImpulse(btScalar maxImp)
+ void setMaxAppliedImpulse(btScalar maxImp)
{
m_maxAppliedImpulse = maxImp;
}
- virtual void debugDraw(class btIDebugDraw* drawer)=0;
+ virtual void debugDraw(class btIDebugDraw * drawer) = 0;
virtual void setGearRatio(btScalar ratio) {}
virtual void setGearAuxLink(int gearAuxLink) {}
- virtual void setRelativePositionTarget(btScalar relPosTarget){}
- virtual void setErp(btScalar erp){}
-
-
+ virtual void setRelativePositionTarget(btScalar relPosTarget) {}
+ virtual void setErp(btScalar erp) {}
};
-#endif //BT_MULTIBODY_CONSTRAINT_H
-
+#endif //BT_MULTIBODY_CONSTRAINT_H
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-30 16:55:50 +0000