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

Add 'extractRotation' based on "A robust method to extract the rotational part of deformations"

Browse Source 
///See http://dl.acm.org/citation.cfm?doid=2994258.2994269
Rewrite 'diagonalize' to use 'extractRotation', should fix Issue 846
This commit is contained in:
Erwin Coumans
2017-02-25 16:57:18 -08:00
parent 88aa9e899e
commit 0131754173
2 changed files with 62 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

116
src/LinearMath/btMatrix3x3.h
View File

@@ -647,92 +647,48 @@ public:
return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
} }
///extractRotation is from "A robust method to extract the rotational part of deformations"
///See http://dl.acm.org/citation.cfm?doid=2994258.2994269
SIMD_FORCE_INLINE void extractRotation(btQuaternion &q,btScalar tolerance = 1.0e-9, int maxIter=100)
{
int iter =0;
btScalar w;
const btMatrix3x3& A=*this;
for(iter = 0; iter < maxIter; iter++)
{
btMatrix3x3 R(q);
btVector3 omega = (R.getColumn(0).cross(A.getColumn(0)) + R.getColumn(1).cross(A.getColumn(1))
+ R.getColumn(2).cross(A.getColumn(2))
) * (btScalar(1.0) / btFabs(R.getColumn(0).dot(A.getColumn(0)) + R.getColumn
(1).dot(A.getColumn(1)) + R.getColumn(2).dot(A.getColumn(2))) +
tolerance);
w = omega.norm();
if(w < tolerance)
break;
q = btQuaternion(btVector3((btScalar(1.0) / w) * omega),w) *
q;
q.normalize();
}
}
/**@brief diagonalizes this matrix by the Jacobi method.
/**@brief diagonalizes this matrix
* @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
* coordinate system, i.e., old_this = rot * new_this * rot^T. * coordinate system, i.e., old_this = rot * new_this * rot^T.
* @param threshold See iteration * @param threshold See iteration
* @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied * @param maxIter The iteration stops when we hit the given tolerance or when maxIter have been executed.
* by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
*
* Note that this matrix is assumed to be symmetric.
*/ */
void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) void diagonalize(btMatrix3x3& rot, btScalar tolerance = 1.0e-9, int maxIter=100)
{ {
rot.setIdentity(); btQuaternion r;
for (int step = maxSteps; step > 0; step--) extractRotation(r,tolerance,maxIter);
{ rot.setRotation(r);
// find off-diagonal element [p][q] with largest magnitude btMatrix3x3 rotInv = btMatrix3x3(r.inverse());
int p = 0; btMatrix3x3 old = *this;
int q = 1; setValue(old.tdotx( rotInv[0]), old.tdoty( rotInv[0]), old.tdotz( rotInv[0]),
int r = 2; old.tdotx( rotInv[1]), old.tdoty( rotInv[1]), old.tdotz( rotInv[1]),
btScalar max = btFabs(m_el[0][1]); old.tdotx( rotInv[2]), old.tdoty( rotInv[2]), old.tdotz( rotInv[2]));
btScalar v = btFabs(m_el[0][2]);
if (v > max)
{
q = 2;
r = 1;
max = v;
}
v = btFabs(m_el[1][2]);
if (v > max)
{
p = 1;
q = 2;
r = 0;
max = v;
}
btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
if (max <= t)
{
if (max <= SIMD_EPSILON * t)
{
return;
}
step = 1;
}
// compute Jacobi rotation J which leads to a zero for element [p][q]
btScalar mpq = m_el[p][q];
btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
btScalar theta2 = theta * theta;
btScalar cos;
btScalar sin;
if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
{
t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
: 1 / (theta - btSqrt(1 + theta2));
cos = 1 / btSqrt(1 + t * t);
sin = cos * t;
}
else
{
// approximation for large theta-value, i.e., a nearly diagonal matrix
t = 1 / (theta * (2 + btScalar(0.5) / theta2));
cos = 1 - btScalar(0.5) * t * t;
sin = cos * t;
}
// apply rotation to matrix (this = J^T * this * J)
m_el[p][q] = m_el[q][p] = 0;
m_el[p][p] -= t * mpq;
m_el[q][q] += t * mpq;
btScalar mrp = m_el[r][p];
btScalar mrq = m_el[r][q];
m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
// apply rotation to rot (rot = rot * J)
for (int i = 0; i < 3; i++)
{
btVector3& row = rot[i];
mrp = row[p];
mrq = row[q];
row[p] = cos * mrp - sin * mrq;
row[q] = cos * mrq + sin * mrp;
}
}
} }

30
src/LinearMath/btQuaternion.h
View File

@@ -141,11 +141,11 @@ public:
* @param yaw Angle around Z * @param yaw Angle around Z
* @param pitch Angle around Y * @param pitch Angle around Y
* @param roll Angle around X */ * @param roll Angle around X */
void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) void setEulerZYX(const btScalar& yawZ, const btScalar& pitchY, const btScalar& rollX)
{ {
btScalar halfYaw = btScalar(yaw) * btScalar(0.5); btScalar halfYaw = btScalar(yawZ) * btScalar(0.5);
btScalar halfPitch = btScalar(pitch) * btScalar(0.5); btScalar halfPitch = btScalar(pitchY) * btScalar(0.5);
btScalar halfRoll = btScalar(roll) * btScalar(0.5); btScalar halfRoll = btScalar(rollX) * btScalar(0.5);
btScalar cosYaw = btCos(halfYaw); btScalar cosYaw = btCos(halfYaw);
btScalar sinYaw = btSin(halfYaw); btScalar sinYaw = btSin(halfYaw);
btScalar cosPitch = btCos(halfPitch); btScalar cosPitch = btCos(halfPitch);
@@ -157,6 +157,28 @@ public:
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
} }
/**@brief Get the euler angles from this quaternion
* @param yaw Angle around Z
* @param pitch Angle around Y
* @param roll Angle around X */
void getEulerZYX(btScalar& yawZ, btScalar& pitchY, btScalar& rollX) const
{
btScalar squ;
btScalar sqx;
btScalar sqy;
btScalar sqz;
btScalar sarg;
sqx = m_floats[0] * m_floats[0];
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);
}
/**@brief Add two quaternions /**@brief Add two quaternions
* @param q The quaternion to add to this one */ * @param q The quaternion to add to this one */
SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q) SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q)