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

preserve length of rotation axis

Browse Source
This commit is contained in:
wmayer
2017-12-13 16:25:37 +01:00
parent 3692861136
commit ab908bf22d
2 changed files with 35 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
src/Base/Rotation.cpp
View File

@@ -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);
}

4
src/Base/Rotation.h
View File

@@ -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. */