kindred/create
1
1
Fork 0
Code Issues 37 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

fix implementation of Rotation::isSame(const Rotation& q, double tol) and extend Python bindings

Browse Source
This commit is contained in:
wmayer
2019-09-10 12:49:18 +02:00
parent f1b569621a
commit 3726006942
3 changed files with 17 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/Base/Rotation.cpp
View File

@@ -398,11 +398,19 @@ bool Rotation::isSame(const Rotation& q) const
bool Rotation::isSame(const Rotation& q, double tol) const
{
Vector3d v(0,0,1);
return std::fabs(multVec(v).GetAngle(q.multVec(v))) < tol;
// This follows the implementation of Coin3d where the norm
// (x1-y1)**2 + ... + (x4-y4)**2 is computed.
// This term can be simplified to
// 2 - 2*(x1*y1 + ... + x4*y4) so that for the equality we have to check
// 1 - tol/2 <= x1*y1 + ... + x4*y4
// Because a quaternion (x1,x2,x3,x4) is equal to (-x1,-x2,-x3,-x4) we use the
// absolute value of the scalar product
double dot = q.quat[0]*quat[0]+q.quat[1]*quat[1]+q.quat[2]*quat[2]+q.quat[3]*quat[3];
return fabs(dot) >= 1.0 - tol/2;
}
Vector3d Rotation::multVec(const Vector3d & src) const {
Vector3d Rotation::multVec(const Vector3d & src) const
{
Vector3d dst;
multVec(src,dst);
return dst;