Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Management of Rotations (direction and angles) #1053

Closed
wants to merge 4 commits into from
Closed

Management of Rotations (direction and angles) #1053

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
1ba5ffc
Improvement of rotations
plgarcia Nov 19, 2017
a309796
Add a function to compare 2 double allowing approximation errors
plgarcia Nov 20, 2017
80218f0
Modification to store the non-normalized axis, what is more convenien…
plgarcia Dec 12, 2017
48a9664
Add ability to define rotation templates with either quaternion or ax…
plgarcia Dec 13, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
22 changes: 21 additions & 1 deletion src/App/PropertyGeo.cpp
View file
Open in desktop
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
134 changes: 104 additions & 30 deletions src/Base/Rotation.cpp
View file
Open in desktop
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,50 @@ 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
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) {
_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;
}

void Rotation::getValueNormalized(Vector3d & axis, double & rfAngle) const
{
getValue(axis, rfAngle);
axis.Normalize();
}

/**
Expand Down Expand Up @@ -162,6 +197,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,16 +229,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);
double l = norm.Length();
if (l>0.5) {
this->_axis = axis;
} else {
norm = _axis;
norm.Normalize();
}

double scale = (double)sin(theAngle/2.0);
this->quat[0] = norm.x * scale;
this->quat[1] = norm.y * scale;
this->quat[2] = norm.z * scale;
Expand Down Expand Up @@ -258,6 +306,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 +321,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 +352,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 +372,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 +413,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 +613,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
5 changes: 5 additions & 0 deletions src/Base/Rotation.h
View file
Open in desktop
Expand Up @@ -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);
Expand Down Expand Up @@ -103,9 +105,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
24 changes: 19 additions & 5 deletions src/Mod/Path/PathScripts/PathStock.py
View file
Open in desktop
Expand Up @@ -303,6 +303,10 @@ def TemplateAttributes(stock, includeExtent=True, includePlacement=True):
attrs['rotY'] = rot.Q[1]
attrs['rotZ'] = rot.Q[2]
attrs['rotW'] = rot.Q[3]
attrs['oX'] = rot.Axis.x
attrs['oY'] = rot.Axis.y
attrs['oZ'] = rot.Axis.z
attrs['angle'] = rot.Angle*180.0/math.pi

return attrs

Expand All @@ -318,13 +322,23 @@ def CreateFromTemplate(job, template):
rotY = template.get('rotY')
rotZ = template.get('rotZ')
rotW = template.get('rotW')
if posX is not None and posY is not None and posZ is not None and rotX is not None and rotY is not None and rotZ is not None and rotW is not None:
oX = template.get('oX')
oY = template.get('oY')
oZ = template.get('oZ')
angle = template.get('angle')
if posX is not None and posY is not None and posZ is not None :
pos = FreeCAD.Vector(float(posX), float(posY), float(posZ))
rot = FreeCAD.Rotation(float(rotX), float(rotY), float(rotZ), float(rotW))
placement = FreeCAD.Placement(pos, rot)
elif posX is not None or posY is not None or posZ is not None or rotX is not None or rotY is not None or rotZ is not None or rotW is not None:
if oX is not None and oY is not None and oZ is not None and angle is not None :
placement = FreeCAD.Placement(FreeCAD.Vector(posX, posY, posZ), FreeCAD.Vector(oX, oY, oZ), angle)
elif rotX is not None and rotY is not None and rotZ is not None and rotW is not None:
pos = FreeCAD.Vector(float(posX), float(posY), float(posZ))
rot = FreeCAD.Rotation(float(rotX), float(rotY), float(rotZ), float(rotW))
placement = FreeCAD.Placement(pos, rot)
else:
PathLog.warning(translate('PathStock', 'Corrupted or incomplete placement information in template - ignoring'))
else:
PathLog.warning(translate('PathStock', 'Corrupted or incomplete placement information in template - ignoring'))

if stockType == StockType.FromBase:
xneg = template.get('xneg')
xpos = template.get('xpos')
Expand Down