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

Improvement of rotations

Browse Source 
Rotation:
-	Add a private attribute Vector to store the direction of the rotation, and manage not to erase this direction when the angle id 0.
-	Add a private attribute to store the angle as defined (no modulo etc)
-	Keep the quaternion for calculations

PropertyGeo
-	Saves the rotation with angle and direction instead of saving the quaternion.
-	Attribute name chosen: Ox, Oy and Oz for the coordinates of the axis and A for the angle in radians. This has to be validated.
-	Backward compatibility with the saved files with quaternion (test presence of A to determine which of  the Quaternion (old way) or the direction and angle is stored (new way). New files can be opened by old FreeCAD and vice-versa.

The only side effect I can imagine is that it was possible to set a vector to 0, 0, 0 if the angle was not 0, what is somehow non sense. Now when setting to 0, 0 0 the last not null vector is kept. The vector can not be null any longer.
This commit is contained in:
plgarcia
2017-11-20 00:36:01 +01:00
committed by wmayer
parent e272f02e59
commit a0ea3ceec9
3 changed files with 120 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

129
src/Base/Rotation.cpp
View File

@@ -36,16 +36,21 @@ using namespace Base;
Rotation::Rotation()
{
quat[0]=quat[1]=quat[2]=0.0;quat[3]=1.0;
_axis.Set(0.0, 0.0, 1.0);
_angle = 0.0;
}
/** Construct a rotation by rotation axis and angle */
Rotation::Rotation(const Vector3d& axis, const double fAngle)
{
_axis.Set(0.0, 0.0, 1.0);
this->setValue(axis, fAngle);
}
Rotation::Rotation(const Matrix4D& matrix)
{
_axis.Set(0.0, 0.0, 1.0);
this->setValue(matrix);
}
@@ -55,6 +60,8 @@ Rotation::Rotation(const Matrix4D& matrix)
*/
Rotation::Rotation(const double q[4])
{
_axis.Set(0.0, 0.0, 1.0);
_angle = 0.0;
this->setValue(q);
}
@@ -64,11 +71,15 @@ Rotation::Rotation(const double q[4])
*/
Rotation::Rotation(const double q0, const double q1, const double q2, const double q3)
{
_axis.Set(0.0, 0.0, 1.0);
_angle = 0.0;
this->setValue(q0, q1, q2, q3);
}
Rotation::Rotation(const Vector3d & rotateFrom, const Vector3d & rotateTo)
{
_axis.Set(0.0, 0.0, 1.0);
_angle = 0.0;
this->setValue(rotateFrom, rotateTo);
}
@@ -78,6 +89,13 @@ Rotation::Rotation(const Rotation& rot)
this->quat[1] = rot.quat[1];
this->quat[2] = rot.quat[2];
this->quat[3] = rot.quat[3];
this->_axis[0] = rot._axis[0];
this->_axis[1] = rot._axis[1];
this->_axis[2] = rot._axis[2];
this->_angle = rot._angle;
}
const double * Rotation::getValue(void) const
@@ -93,6 +111,25 @@ void Rotation::getValue(double & q0, double & q1, double & q2, double & q3) cons
q3 = this->quat[3];
}
void Rotation::evaluateVector () {
if((this->quat[3] > -1.0) && (this->quat[3] < 1.0)) {
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;
_angle=double(acos(this->quat[3])) * 2.0;
if (_angle>=D_PI) {
_angle -= 2 * D_PI;
}
} else {
_angle = 0.0;
}
// the vector stays unchanged
}
void Rotation::setValue(const double q0, const double q1, const double q2, const double q3)
{
this->quat[0] = q0;
@@ -100,26 +137,16 @@ void Rotation::setValue(const double q0, const double q1, const double q2, const
this->quat[2] = q2;
this->quat[3] = q3;
this->normalize();
this->evaluateVector ();
}
void Rotation::getValue(Vector3d & axis, double & rfAngle) const
{
// Taken from <http://de.wikipedia.org/wiki/Quaternionen>
//
// Note: -1 < w < +1 (|w| == 1 not allowed, with w:=quat[3])
if((this->quat[3] > -1.0) && (this->quat[3] < 1.0)) {
rfAngle = double(acos(this->quat[3])) * 2.0;
double scale = (double)sin(rfAngle / 2.0);
// Get a normalized vector
axis.x = this->quat[0] / scale;
axis.y = this->quat[1] / scale;
axis.z = this->quat[2] / scale;
}
else {
// The quaternion doesn't describe a rotation, so we can setup any value we want
axis.Set(0.0, 0.0, 1.0);
rfAngle = 0.0;
}
rfAngle = _angle;//double(acos(this->quat[3])) * 2.0;
axis.x = _axis.x;
axis.y = _axis.y;
axis.z = _axis.z;
}
/**
@@ -162,6 +189,7 @@ void Rotation::setValue(const double q[4])
this->quat[2] = q[2];
this->quat[3] = q[3];
this->normalize();
this->evaluateVector ();
}
void Rotation::setValue(const Matrix4D & m)
@@ -193,19 +221,28 @@ void Rotation::setValue(const Matrix4D & m)
this->quat[j] = (double)((m[j][i] + m[i][j]) * s);
this->quat[k] = (double)((m[k][i] + m[i][k]) * s);
}
this->evaluateVector ();
}
void Rotation::setValue(const Vector3d & axis, const double fAngle)
{
// Taken from <http://de.wikipedia.org/wiki/Quaternionen>
//
this->quat[3] = (double)cos(fAngle/2.0);
// normalization of the angle to be in [0, 2pi[
_angle=fAngle;
double theAngle = fAngle - floor(fAngle / (2.0 * D_PI))*(2.0 * D_PI);
this->quat[3] = (double)cos(theAngle/2.0);
Vector3d norm = axis;
norm.Normalize();
double scale = (double)sin(fAngle/2.0);
this->quat[0] = norm.x * scale;
this->quat[1] = norm.y * scale;
this->quat[2] = norm.z * scale;
double l = norm.Length();
if (l>0.5) {
this->_axis = norm;
}
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;
}
void Rotation::setValue(const Vector3d & rotateFrom, const Vector3d & rotateTo)
@@ -258,6 +295,11 @@ Rotation & Rotation::invert(void)
this->quat[0] = -this->quat[0];
this->quat[1] = -this->quat[1];
this->quat[2] = -this->quat[2];
this->_axis.x = -this->_axis.x;
this->_axis.y = -this->_axis.y;
this->_axis.z = -this->_axis.z;
return *this;
}
@@ -268,6 +310,10 @@ Rotation Rotation::inverse(void) const
rot.quat[1] = -this->quat[1];
rot.quat[2] = -this->quat[2];
rot.quat[3] = this->quat[3];
rot._axis[0] = -this->_axis[0];
rot._axis[1] = -this->_axis[1];
rot._axis[2] = -this->_axis[2];
return rot;
}
@@ -295,10 +341,15 @@ Rotation Rotation::operator*(const Rotation & q) const
bool Rotation::operator==(const Rotation & q) const
{
if (this->quat[0] == q.quat[0] &&
this->quat[1] == q.quat[1] &&
this->quat[2] == q.quat[2] &&
this->quat[3] == q.quat[3])
if ((this->quat[0] == q.quat[0] &&
this->quat[1] == q.quat[1] &&
this->quat[2] == q.quat[2] &&
this->quat[3] == q.quat[3]) ||
(this->quat[0] == -q.quat[0] &&
this->quat[1] == -q.quat[1] &&
this->quat[2] == -q.quat[2] &&
this->quat[3] == -q.quat[3]))
return true;
return false;
}
@@ -310,10 +361,14 @@ bool Rotation::operator!=(const Rotation & q) const
bool Rotation::isSame(const Rotation& q) const
{
if ((this->quat[0] == q.quat[0] || this->quat[0] == -q.quat[0]) &&
(this->quat[1] == q.quat[1] || this->quat[1] == -q.quat[1]) &&
(this->quat[2] == q.quat[2] || this->quat[2] == -q.quat[2]) &&
(this->quat[3] == q.quat[3] || this->quat[3] == -q.quat[3]))
if ((this->quat[0] == q.quat[0] &&
this->quat[1] == q.quat[1] &&
this->quat[2] == q.quat[2] &&
this->quat[3] == q.quat[3]) ||
(this->quat[0] == -q.quat[0] &&
this->quat[1] == -q.quat[1] &&
this->quat[2] == -q.quat[2] &&
this->quat[3] == -q.quat[3]))
return true;
return false;
}
@@ -347,6 +402,7 @@ void Rotation::scaleAngle(const double scaleFactor)
Rotation Rotation::slerp(const Rotation & q0, const Rotation & q1, double t)
{
// Taken from <http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/>
// q = [q0*sin((1-t)*theta)+q1*sin(t*theta)]/sin(theta), 0<=t<=1
if (t<0.0) t=0.0;
@@ -546,10 +602,17 @@ void Rotation::setYawPitchRoll(double y, double p, double r)
double c3 = cos(r/2.0);
double s3 = sin(r/2.0);
quat[0] = c1*c2*s3 - s1*s2*c3;
quat[1] = c1*s2*c3 + s1*c2*s3;
quat[2] = s1*c2*c3 - c1*s2*s3;
quat[3] = c1*c2*c3 + s1*s2*s3;
// quat[0] = c1*c2*s3 - s1*s2*c3;
// quat[1] = c1*s2*c3 + s1*c2*s3;
// quat[2] = s1*c2*c3 - c1*s2*s3;
// quat[3] = c1*c2*c3 + s1*s2*s3;
this->setValue (
c1*c2*s3 - s1*s2*c3,
c1*s2*c3 + s1*c2*s3,
s1*c2*c3 - c1*s2*s3,
c1*c2*c3 + s1*s2*s3
);
}
void Rotation::getYawPitchRoll(double& y, double& p, double& r) const