diff --git a/src/Base/Rotation.cpp b/src/Base/Rotation.cpp index f1b671d339cc1..7d7968c52f1ca 100644 --- a/src/Base/Rotation.cpp +++ b/src/Base/Rotation.cpp @@ -116,9 +116,11 @@ void Rotation::evaluateVector () { double rfAngle = double(acos(this->quat[3])) * 2.0; double scale = (double)sin(rfAngle / 2.0); // Get a normalized vector - this->_axis.x = this->quat[0] / scale; - this->_axis.y = this->quat[1] / scale; - this->_axis.z = this->quat[2] / scale; + double l = this->_axis.Length(); + if (l < Base::Vector3d::epsilon()) l = 1; + this->_axis.x = this->quat[0] * l / scale; + this->_axis.y = this->quat[1] * l / scale; + this->_axis.z = this->quat[2] * l / scale; _angle=double(acos(this->quat[3])) * 2.0; if (_angle>=D_PI) { @@ -149,6 +151,12 @@ void Rotation::getValue(Vector3d & axis, double & rfAngle) const axis.z = _axis.z; } +void Rotation::getValueNormalized(Vector3d & axis, double & rfAngle) const +{ + getValue(axis, rfAngle); + axis.Normalize(); +} + /** * Returns this rotation in form of a matrix. */ @@ -236,13 +244,16 @@ void Rotation::setValue(const Vector3d & axis, const double fAngle) norm.Normalize(); double l = norm.Length(); if (l>0.5) { - this->_axis = norm; + this->_axis = axis; + } else { + norm = _axis; + norm.Normalize(); } double scale = (double)sin(theAngle/2.0); - this->quat[0] = this->_axis.x * scale; - this->quat[1] = this->_axis.y * scale; - this->quat[2] = this->_axis.z * scale; + this->quat[0] = norm.x * scale; + this->quat[1] = norm.y * scale; + this->quat[2] = norm.z * scale; } void Rotation::setValue(const Vector3d & rotateFrom, const Vector3d & rotateTo) diff --git a/src/Base/Rotation.h b/src/Base/Rotation.h index 765d1ec3375ea..c7a9c4d089a7a 100644 --- a/src/Base/Rotation.h +++ b/src/Base/Rotation.h @@ -51,6 +51,8 @@ class BaseExport Rotation void getValue(double & q0, double & q1, double & q2, double & q3) const; void setValue(const double q0, const double q1, const double q2, const double q3); void getValue(Vector3d & axis, double & rfAngle) const; + void getValueNormalized(Vector3d & axis, double & rfAngle) const; + void getValue(Matrix4D & matrix) const; void setValue(const double q[4]); void setValue(const Matrix4D& matrix);