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

add btHingeAccumulatedAngleConstraint derived from btHingeConstraint,

Browse Source 
that exposes a new method getAccumulatedHingeAngle
See also https://github.com/bulletphysics/bullet3/issues/42
This commit is contained in:
Erwin Coumans
2014-07-30 16:13:34 -07:00
parent 97fc144134
commit 1d00d91707
2 changed files with 91 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
View File

@@ -285,8 +285,59 @@ void btHingeConstraint::buildJacobian()
#endif //__SPU__
static inline btScalar btNormalizeAnglePositive(btScalar angle)
{
return btFmod(btFmod(angle, 2.0*SIMD_PI) + 2.0*SIMD_PI, 2.0*SIMD_PI);
}
static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
{
btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
btNormalizeAnglePositive(accAngle)));
return result;
}
static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
{
btScalar tol(0.3);
btScalar result = btShortestAngularDistance(accAngle, curAngle);
if (btFabs(result) > tol)
return curAngle;
else
return accAngle + result;
return curAngle;
}
btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
{
btScalar hingeAngle = getHingeAngle();
m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle);
return m_accumulatedAngle;
}
void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
{
m_accumulatedAngle = accAngle;
}
void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
{
//update m_accumulatedAngle
btScalar curHingeAngle = getHingeAngle();
m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle);
btHingeConstraint::getInfo1(info);
}
void btHingeConstraint::getInfo1(btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
@@ -606,6 +657,8 @@ void btHingeConstraint::updateRHS(btScalar timeStep)
}
btScalar btHingeConstraint::getHingeAngle()
{
return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());

38
src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
View File

@@ -236,6 +236,7 @@ public:
}
///The getHingeAngle gives the hinge angle in range [-PI,PI]
btScalar getHingeAngle();
btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
@@ -326,6 +327,43 @@ struct btHingeConstraintDoubleData
};
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account
ATTRIBUTE_ALIGNED16(class) btHingeAccumulatedAngleConstraint : public btHingeConstraint
{
protected:
btScalar m_accumulatedAngle;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,rbB,pivotInA,pivotInB, axisInA,axisInB, useReferenceFrameA )
{
m_accumulatedAngle=getHingeAngle();
}
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA)
{
m_accumulatedAngle=getHingeAngle();
}
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA )
{
m_accumulatedAngle=getHingeAngle();
}
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,rbAFrame, useReferenceFrameA )
{
m_accumulatedAngle=getHingeAngle();
}
btScalar getAccumulatedHingeAngle();
void setAccumulatedHingeAngle(btScalar accAngle);
virtual void getInfo1 (btConstraintInfo1* info);
};
struct btHingeConstraintFloatData
{