2017-08-01 12:30:58 +00:00
/*
Bullet Continuous Collision Detection and Physics Library
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 ,
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 .
2. Altered source versions must be plainly marked as such , and must not be misrepresented as being the original software .
3. This notice may not be removed or altered from any source distribution .
*/
# ifndef BT_MULTIBODY_CONSTRAINT_H
# define BT_MULTIBODY_CONSTRAINT_H
# include "LinearMath/btScalar.h"
# include "LinearMath/btAlignedObjectArray.h"
# include "btMultiBody.h"
2021-09-29 13:47:08 +00:00
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
enum btTypedMultiBodyConstraintType
{
MULTIBODY_CONSTRAINT_LIMIT = 3 ,
MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR ,
MULTIBODY_CONSTRAINT_GEAR ,
MULTIBODY_CONSTRAINT_POINT_TO_POINT ,
MULTIBODY_CONSTRAINT_SLIDER ,
MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR ,
MULTIBODY_CONSTRAINT_FIXED ,
2022-05-16 22:02:51 +00:00
MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT ,
2021-09-29 13:47:08 +00:00
MAX_MULTIBODY_CONSTRAINT_TYPE ,
} ;
2017-08-01 12:30:58 +00:00
class btMultiBody ;
struct btSolverInfo ;
# include "btMultiBodySolverConstraint.h"
struct btMultiBodyJacobianData
{
2019-01-03 13:26:51 +00:00
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 ;
2017-08-01 12:30:58 +00:00
} ;
2019-01-03 13:26:51 +00:00
ATTRIBUTE_ALIGNED16 ( class )
btMultiBodyConstraint
2017-08-01 12:30:58 +00:00
{
protected :
2019-01-03 13:26:51 +00:00
btMultiBody * m_bodyA ;
btMultiBody * m_bodyB ;
int m_linkA ;
int m_linkB ;
2021-09-29 13:47:08 +00:00
int m_type ; //btTypedMultiBodyConstraintType
2019-01-03 13:26:51 +00:00
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 ,
2021-09-29 13:47:08 +00:00
bool isFriction = false , btScalar desiredVelocity = 0 , btScalar cfmSlip = 0 , btScalar damping = 1.0 ) ;
2017-08-01 12:30:58 +00:00
public :
BT_DECLARE_ALIGNED_ALLOCATOR ( ) ;
2021-09-29 13:47:08 +00:00
btMultiBodyConstraint ( btMultiBody * bodyA , btMultiBody * bodyB , int linkA , int linkB , int numRows , bool isUnilateral , int type ) ;
2017-08-01 12:30:58 +00:00
virtual ~ btMultiBodyConstraint ( ) ;
void updateJacobianSizes ( ) ;
void allocateJacobiansMultiDof ( ) ;
2021-09-29 13:47:08 +00:00
int getConstraintType ( ) const
{
return m_type ;
}
2017-08-01 12:30:58 +00:00
//many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later.
virtual void setFrameInB ( const btMatrix3x3 & frameInB ) { }
2019-01-03 13:26:51 +00:00
virtual void setPivotInB ( const btVector3 & pivotInB ) { }
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
virtual void finalizeMultiDof ( ) = 0 ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
virtual int getIslandIdA ( ) const = 0 ;
virtual int getIslandIdB ( ) const = 0 ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
virtual void createConstraintRows ( btMultiBodyConstraintArray & constraintRows ,
btMultiBodyJacobianData & data ,
const btContactSolverInfo & infoGlobal ) = 0 ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
int getNumRows ( ) const
2017-08-01 12:30:58 +00:00
{
return m_numRows ;
}
2019-01-03 13:26:51 +00:00
btMultiBody * getMultiBodyA ( )
2017-08-01 12:30:58 +00:00
{
return m_bodyA ;
}
2019-01-03 13:26:51 +00:00
btMultiBody * getMultiBodyB ( )
2017-08-01 12:30:58 +00:00
{
return m_bodyB ;
}
2018-09-07 14:11:04 +00:00
int getLinkA ( ) const
{
return m_linkA ;
}
int getLinkB ( ) const
{
return m_linkB ;
}
2019-01-03 13:26:51 +00:00
void internalSetAppliedImpulse ( int dof , btScalar appliedImpulse )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
btAssert ( dof > = 0 ) ;
2017-08-01 12:30:58 +00:00
btAssert ( dof < getNumRows ( ) ) ;
m_data [ dof ] = appliedImpulse ;
}
2019-01-03 13:26:51 +00:00
btScalar getAppliedImpulse ( int dof )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
btAssert ( dof > = 0 ) ;
2017-08-01 12:30:58 +00:00
btAssert ( dof < getNumRows ( ) ) ;
return m_data [ dof ] ;
}
// current constraint position
2019-01-03 13:26:51 +00:00
// constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
// NOTE: ignored position for friction rows.
btScalar getPosition ( int row ) const
2017-08-01 12:30:58 +00:00
{
return m_data [ m_posOffset + row ] ;
}
2019-01-03 13:26:51 +00:00
void setPosition ( int row , btScalar pos )
2017-08-01 12:30:58 +00:00
{
m_data [ m_posOffset + row ] = pos ;
}
bool isUnilateral ( ) const
{
return m_isUnilateral ;
}
// jacobian blocks.
2019-01-03 13:26:51 +00:00
// 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 )
2017-08-01 12:30:58 +00:00
{
return & m_data [ m_numRows + row * m_jacSizeBoth ] ;
}
2019-01-03 13:26:51 +00:00
const btScalar * jacobianA ( int row ) const
2017-08-01 12:30:58 +00:00
{
return & m_data [ m_numRows + ( row * m_jacSizeBoth ) ] ;
}
2019-01-03 13:26:51 +00:00
btScalar * jacobianB ( int row )
2017-08-01 12:30:58 +00:00
{
return & m_data [ m_numRows + ( row * m_jacSizeBoth ) + m_jacSizeA ] ;
}
2019-01-03 13:26:51 +00:00
const btScalar * jacobianB ( int row ) const
2017-08-01 12:30:58 +00:00
{
return & m_data [ m_numRows + ( row * m_jacSizeBoth ) + m_jacSizeA ] ;
}
2019-01-03 13:26:51 +00:00
btScalar getMaxAppliedImpulse ( ) const
2017-08-01 12:30:58 +00:00
{
return m_maxAppliedImpulse ;
}
2019-01-03 13:26:51 +00:00
void setMaxAppliedImpulse ( btScalar maxImp )
2017-08-01 12:30:58 +00:00
{
m_maxAppliedImpulse = maxImp ;
}
2019-01-03 13:26:51 +00:00
virtual void debugDraw ( class btIDebugDraw * drawer ) = 0 ;
2017-08-01 12:30:58 +00:00
virtual void setGearRatio ( btScalar ratio ) { }
virtual void setGearAuxLink ( int gearAuxLink ) { }
2019-01-03 13:26:51 +00:00
virtual void setRelativePositionTarget ( btScalar relPosTarget ) { }
virtual void setErp ( btScalar erp ) { }
2017-08-01 12:30:58 +00:00
} ;
2019-01-03 13:26:51 +00:00
# endif //BT_MULTIBODY_CONSTRAINT_H