diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h | 106 |
1 files changed, 59 insertions, 47 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 9fa317330b3..3b32b46e911 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -28,8 +28,8 @@ struct btSolverInfo; struct btMultiBodyJacobianData { btAlignedObjectArray<btScalar> m_jacobians; - btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse; - btAlignedObjectArray<btScalar> m_deltaVelocities; + 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; @@ -48,16 +48,17 @@ protected: int m_linkA; int m_linkB; - int m_num_rows; - int m_jac_size_A; - int m_jac_size_both; - int m_pos_offset; + 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) @@ -66,40 +67,37 @@ protected: void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); - void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar position, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow, - btMultiBodyJacobianData& data, - btScalar* jacOrgA,btScalar* jacOrgB, - const btContactSolverInfo& infoGlobal, - btScalar desiredVelocity, - btScalar lowerLimit, - btScalar upperLimit); + btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + btScalar* jacOrgA, btScalar* jacOrgB, + const btVector3& contactNormalOnB, + const btVector3& posAworld, const btVector3& posBworld, + btScalar posError, + const btContactSolverInfo& infoGlobal, + btScalar lowerLimit, btScalar upperLimit, + btScalar relaxation = 1.f, + bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0); public: btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); virtual ~btMultiBodyConstraint(); + void updateJacobianSizes(); + void allocateJacobiansMultiDof(); + virtual void finalizeMultiDof()=0; virtual int getIslandIdA() const =0; virtual int getIslandIdB() const =0; - + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)=0; int getNumRows() const { - return m_num_rows; + return m_numRows; } btMultiBody* getMultiBodyA() @@ -111,20 +109,33 @@ public: return m_bodyB; } + void internalSetAppliedImpulse(int dof, btScalar appliedImpulse) + { + btAssert(dof>=0); + btAssert(dof < getNumRows()); + m_data[dof] = appliedImpulse; + } + + btScalar getAppliedImpulse(int dof) + { + 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 - { - return m_data[m_pos_offset + row]; + btScalar getPosition(int row) const + { + return m_data[m_posOffset + row]; } - void setPosition(int row, btScalar pos) - { - m_data[m_pos_offset + row] = pos; + void setPosition(int row, btScalar pos) + { + m_data[m_posOffset + row] = pos; } - + bool isUnilateral() const { return m_isUnilateral; @@ -133,21 +144,21 @@ public: // 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) - { - return &m_data[m_num_rows + row * m_jac_size_both]; + btScalar* jacobianA(int row) + { + return &m_data[m_numRows + row * m_jacSizeBoth]; } - const btScalar* jacobianA(int row) const - { - return &m_data[m_num_rows + (row * m_jac_size_both)]; + const btScalar* jacobianA(int row) const + { + return &m_data[m_numRows + (row * m_jacSizeBoth)]; } - btScalar* jacobianB(int row) - { - return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; + btScalar* jacobianB(int row) + { + return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; } - const btScalar* jacobianB(int row) const - { - return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; + const btScalar* jacobianB(int row) const + { + return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; } btScalar getMaxAppliedImpulse() const @@ -158,7 +169,8 @@ public: { m_maxAppliedImpulse = maxImp; } - + + virtual void debugDraw(class btIDebugDraw* drawer)=0; }; |