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

Merge remote-tracking branch 'upstream/master' into multibody_mlcp_solver_v2

Browse Source
This commit is contained in:
Jeongseok Lee
2018-08-06 10:34:43 -07:00
parent 809fb3ed49 6fe1b000e5
commit e8e97d39fe
23 changed files with 98 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -781,7 +781,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
// hhat is NOT stored for the base (but ahat is)
btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
// v_ptr += num_links * 2 + 2; // Disabled since v_ptr is not used in the rest of the code
v_ptr += num_links * 2 + 2;
//
// Y_i, invD_i
btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
@@ -967,9 +967,11 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
//
Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
- spatCoriolisAcc[i].dot(hDof)
;
- spatCoriolisAcc[i].dot(hDof);
}
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
{
btScalar *D_row = &D[dof * m_links[i].m_dofCount];
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
{

26
src/LinearMath/btQuaternion.h
View File

@@ -173,10 +173,28 @@ public:
sqy = m_floats[1] * m_floats[1];
sqz = m_floats[2] * m_floats[2];
squ = m_floats[3] * m_floats[3];
rollX = btAtan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
sarg = btScalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
pitchY = sarg <= btScalar(-1.0) ? btScalar(-0.5) * SIMD_PI: (sarg >= btScalar(1.0) ? btScalar(0.5) * SIMD_PI : btAsin(sarg));
yawZ = btAtan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
sarg = btScalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
// If the pitch angle is PI/2 or -PI/2, we can only compute
// the sum roll + yaw. However, any combination that gives
// the right sum will produce the correct orientation, so we
// set rollX = 0 and compute yawZ.
if (sarg <= -btScalar(0.99999))
{
pitchY = btScalar(-0.5)*SIMD_PI;
rollX = 0;
yawZ = btScalar(2) * btAtan2(m_floats[0],-m_floats[1]);
} else if (sarg >= btScalar(0.99999))
{
pitchY = btScalar(0.5)*SIMD_PI;
rollX = 0;
yawZ = btScalar(2) * btAtan2(-m_floats[0], m_floats[1]);
} else
{
pitchY = btAsin(sarg);
rollX = btAtan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
yawZ = btAtan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
}
}
/**@brief Add two quaternions

2
src/LinearMath/btQuickprof.cpp
View File

@@ -729,11 +729,9 @@ unsigned int btQuickprofGetCurrentThreadIndex2() {
void btEnterProfileZoneDefault(const char* name)
{
CProfileManager::Start_Profile( name );
}
void btLeaveProfileZoneDefault()
{
CProfileManager::Stop_Profile();
}

3
src/LinearMath/btQuickprof.h
View File

@@ -70,11 +70,12 @@ void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc);
//#define BT_NO_PROFILE 1
#endif //BT_NO_PROFILE
const unsigned int BT_QUICKPROF_MAX_THREAD_COUNT = 64;
#ifndef BT_NO_PROFILE
//btQuickprofGetCurrentThreadIndex will return -1 if thread index cannot be determined,
//otherwise returns thread index in range [0..maxThreads]
unsigned int btQuickprofGetCurrentThreadIndex2();
const unsigned int BT_QUICKPROF_MAX_THREAD_COUNT = 64;
#include <stdio.h>//@todo remove this, backwards compatibility