AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

Merge pull request #2016 from GaborPuhr/Fix-for-#2000

Browse Source 
Fix for #2000
This commit is contained in:
erwincoumans 2018-12-04 12:14:57 -08:00 committed by GitHub
commit 4a66d6c80b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 20 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
View File

@ -820,8 +820,17 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
btScalar dt = BT_ONE / info->fps;
btScalar kd = limot->m_springDamping;
btScalar ks = limot->m_springStiffness;
btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
// btScalar erp = 0.1;
btScalar vel;
if (rotational)
{
vel = angVelA.dot(ax1) - angVelB.dot(ax1);
}
else
{
btVector3 tanVelA = angVelA.cross(m_calculatedTransformA.getOrigin() - transA.getOrigin());
btVector3 tanVelB = angVelB.cross(m_calculatedTransformB.getOrigin() - transB.getOrigin());
vel = (linVelA + tanVelA).dot(ax1) - (linVelB + tanVelB).dot(ax1);
}
btScalar cfm = BT_ZERO;
btScalar mA = BT_ONE / m_rbA.getInvMass();
btScalar mB = BT_ONE / m_rbB.getInvMass();
@ -832,7 +841,10 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();
if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();
}
btScalar m = mA > mB ? mB : mA;
btScalar m;
if (m_rbA.getInvMass() == 0) m = mB; else
if (m_rbB.getInvMass() == 0) m = mA; else
m = mA*mB / (mA + mB);
btScalar angularfreq = sqrt(ks / m);
//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
@ -856,15 +868,14 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
// however in practice any value is fine as long as it is greater then the "proper" velocity,
// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
// so it is much simpler (and more robust) just to simply use inf (with the proper sign)
// (Even with our best intent the "new" velocity is only an estimation. If we underestimate
// the "proper" velocity that will weaken the spring, however if we overestimate it, it doesn't
// matter, because the solver will limit it according the force limit)
// you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
// will we not request a velocity with the wrong direction ?
// and the answare is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
// and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
// so the sign of the force that is really matters
// BEWARE! This whole approach has shown osciliation issues that prevent proper damping.
if ( m_flags & BT_6DOF_FLAGS_USE_INFINITE_ERROR )
info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
else
info->m_constraintError[srow] = vel + f / m * (rotational ? -1 : 1);
info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
btScalar minf = f < fd ? f : fd;
btScalar maxf = f < fd ? fd : f;

1
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
View File

@ -265,7 +265,6 @@ enum bt6DofFlags2
BT_6DOF_FLAGS_ERP_STOP2 = 2,
BT_6DOF_FLAGS_CFM_MOTO2 = 4,
BT_6DOF_FLAGS_ERP_MOTO2 = 8,
BT_6DOF_FLAGS_USE_INFINITE_ERROR = (1<<16),
};
#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis