From 0131754173f38d63659c9d8c57ce2d1fb03cf51c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 25 Feb 2017 16:57:18 -0800 Subject: [PATCH] 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 --- src/LinearMath/btMatrix3x3.h | 116 +++++++++++----------------------- src/LinearMath/btQuaternion.h | 30 +++++++-- 2 files changed, 62 insertions(+), 84 deletions(-) diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h index 40cd1e086..96d0ba98f 100644 --- a/src/LinearMath/btMatrix3x3.h +++ b/src/LinearMath/btMatrix3x3.h @@ -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])); } diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index 32f0f85d2..a9b78022e 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -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)