Skip to content

Commit

Permalink
Improvement of rotations
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@plgarcia
plgarcia committed Nov 19, 2017
1 parent b3631e1 commit 5a71ae6
Show file tree
Hide file tree
Showing 3 changed files with 120 additions and 34 deletions.
22 changes: 21 additions & 1 deletion src/App/PropertyGeo.cpp
View file
Expand Up @@ -744,10 +744,18 @@ void PropertyPlacement::Save (Base::Writer &writer) const
writer.Stream() << " Px=\"" << _cPos.getPosition().x
<< "\" Py=\"" << _cPos.getPosition().y
<< "\" Pz=\"" << _cPos.getPosition().z << "\"";

writer.Stream() << " Q0=\"" << _cPos.getRotation()[0]
<< "\" Q1=\"" << _cPos.getRotation()[1]
<< "\" Q2=\"" << _cPos.getRotation()[2]
<< "\" Q3=\"" << _cPos.getRotation()[3] << "\"";
Vector3d axis;
double rfAngle;
_cPos.getRotation().getValue(axis, rfAngle);
writer.Stream() << " A=\"" << rfAngle
<< "\" Ox=\"" << axis.x
<< "\" Oy=\"" << axis.y
<< "\" Oz=\"" << axis.z << "\"";
writer.Stream() <<"/>" << endl;
}

Expand All @@ -757,13 +765,25 @@ void PropertyPlacement::Restore(Base::XMLReader &reader)
reader.readElement("PropertyPlacement");
// get the value of my Attribute
aboutToSetValue();
_cPos = Base::Placement(Vector3d(reader.getAttributeAsFloat("Px"),
if (reader.hasAttribute("A")) {
_cPos = Base::Placement(Vector3d(reader.getAttributeAsFloat("Px"),
reader.getAttributeAsFloat("Py"),
reader.getAttributeAsFloat("Pz")),
Rotation(
Vector3d(reader.getAttributeAsFloat("Ox"),
reader.getAttributeAsFloat("Oy"),
reader.getAttributeAsFloat("Oz")),
reader.getAttributeAsFloat("A")));
} else {
_cPos = Base::Placement(Vector3d(reader.getAttributeAsFloat("Px"),
reader.getAttributeAsFloat("Py"),
reader.getAttributeAsFloat("Pz")),
Rotation(reader.getAttributeAsFloat("Q0"),
reader.getAttributeAsFloat("Q1"),
reader.getAttributeAsFloat("Q2"),
reader.getAttributeAsFloat("Q3")));

}
hasSetValue();
}

Expand Down
129 changes: 96 additions & 33 deletions src/Base/Rotation.cpp
View file
Expand Up @@ -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);
}

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

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

Expand All @@ -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
Expand All @@ -93,33 +111,42 @@ 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;
this->quat[1] = q1;
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;
}

/**
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
}

Expand All @@ -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;
}

Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
}
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down
3 changes: 3 additions & 0 deletions src/Base/Rotation.h
View file
Expand Up @@ -103,9 +103,12 @@ class BaseExport Rotation
static Rotation makeRotationByAxes(Vector3d xdir, Vector3d ydir, Vector3d zdir, const char* priorityOrder = "ZXY");


void evaluateVector ();
private:
void normalize();
double quat[4];
Vector3d _axis; // the axis kept not to lose direction when angle is 0
double _angle; // this angle to keep the angle chozen by the user
};

}
Expand Down

0 comments on commit 5a71ae6

Please sign in to comment.