diff --git a/src/Base/Rotation.cpp b/src/Base/Rotation.cpp index 4e6e904a7e..3799367cc4 100644 --- a/src/Base/Rotation.cpp +++ b/src/Base/Rotation.cpp @@ -119,9 +119,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 = rfAngle; if (_angle >= D_PI) { @@ -146,7 +148,16 @@ void Rotation::setValue(const double q0, const double q1, const double q2, const void Rotation::getValue(Vector3d & axis, double & rfAngle) const { - rfAngle = _angle;//double(acos(this->quat[3])) * 2.0; + rfAngle = _angle; + axis.x = _axis.x; + axis.y = _axis.y; + axis.z = _axis.z; + axis.Normalize(); +} + +void Rotation::getRawValue(Vector3d & axis, double & rfAngle) const +{ + rfAngle = _angle; axis.x = _axis.x; axis.y = _axis.y; axis.z = _axis.z; @@ -242,13 +253,17 @@ void Rotation::setValue(const Vector3d & axis, const double fAngle) double l = norm.Length(); // Keep old axis in case the new axis is the null vector 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) @@ -642,3 +657,11 @@ void Rotation::getYawPitchRoll(double& y, double& p, double& r) const p = (p/D_PI)*180; r = (r/D_PI)*180; } + +bool Rotation::isNull() const +{ + return (this->quat[0] == 0 && + this->quat[1] == 0 && + this->quat[2] == 0 && + this->quat[3] == 0); +} diff --git a/src/Base/Rotation.h b/src/Base/Rotation.h index 122d78ae65..a7cd8fedc2 100644 --- a/src/Base/Rotation.h +++ b/src/Base/Rotation.h @@ -50,7 +50,10 @@ public: const double * getValue(void) const; 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); + /// If not a null quaternion then \a axis will be normalized void getValue(Vector3d & axis, double & rfAngle) const; + /// Does the same as the method above unless normalizing the axis. + void getRawValue(Vector3d & axis, double & rfAngle) const; void getValue(Matrix4D & matrix) const; void setValue(const double q[4]); void setValue(const Matrix4D& matrix); @@ -60,6 +63,7 @@ public: void setYawPitchRoll(double y, double p, double r); /// Euler angles in yaw,pitch,roll notation void getYawPitchRoll(double& y, double& p, double& r) const; + bool isNull() const; //@} /** Invert rotations. */