mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-12 12:50:08 +00:00
Add 'extractRotation' based on "A robust method to extract the rotational part of deformations"
///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
parent
88aa9e899e
commit
0131754173
@ -647,92 +647,48 @@ public:
|
||||
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
|
||||
* coordinate system, i.e., old_this = rot * new_this * rot^T.
|
||||
* @param threshold See iteration
|
||||
* @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
|
||||
* 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.
|
||||
* @param maxIter The iteration stops when we hit the given tolerance or when maxIter have been executed.
|
||||
*/
|
||||
void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
|
||||
void diagonalize(btMatrix3x3& rot, btScalar tolerance = 1.0e-9, int maxIter=100)
|
||||
{
|
||||
rot.setIdentity();
|
||||
for (int step = maxSteps; step > 0; step--)
|
||||
{
|
||||
// find off-diagonal element [p][q] with largest magnitude
|
||||
int p = 0;
|
||||
int q = 1;
|
||||
int r = 2;
|
||||
btScalar max = btFabs(m_el[0][1]);
|
||||
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;
|
||||
}
|
||||
}
|
||||
btQuaternion r;
|
||||
extractRotation(r,tolerance,maxIter);
|
||||
rot.setRotation(r);
|
||||
btMatrix3x3 rotInv = btMatrix3x3(r.inverse());
|
||||
btMatrix3x3 old = *this;
|
||||
setValue(old.tdotx( rotInv[0]), old.tdoty( rotInv[0]), old.tdotz( rotInv[0]),
|
||||
old.tdotx( rotInv[1]), old.tdoty( rotInv[1]), old.tdotz( rotInv[1]),
|
||||
old.tdotx( rotInv[2]), old.tdoty( rotInv[2]), old.tdotz( rotInv[2]));
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -141,11 +141,11 @@ public:
|
||||
* @param yaw Angle around Z
|
||||
* @param pitch Angle around Y
|
||||
* @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 halfPitch = btScalar(pitch) * btScalar(0.5);
|
||||
btScalar halfRoll = btScalar(roll) * btScalar(0.5);
|
||||
btScalar halfYaw = btScalar(yawZ) * btScalar(0.5);
|
||||
btScalar halfPitch = btScalar(pitchY) * btScalar(0.5);
|
||||
btScalar halfRoll = btScalar(rollX) * btScalar(0.5);
|
||||
btScalar cosYaw = btCos(halfYaw);
|
||||
btScalar sinYaw = btSin(halfYaw);
|
||||
btScalar cosPitch = btCos(halfPitch);
|
||||
@ -157,6 +157,28 @@ public:
|
||||
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
|
||||
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
|
||||
* @param q The quaternion to add to this one */
|
||||
SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user