diff --git a/src/App/PropertyGeo.cpp b/src/App/PropertyGeo.cpp index cfd1b85a51096..ecda50e972343 100644 --- a/src/App/PropertyGeo.cpp +++ b/src/App/PropertyGeo.cpp @@ -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; } @@ -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(); } diff --git a/src/Base/Rotation.cpp b/src/Base/Rotation.cpp index c87f45a696cfa..f1b671d339cc1 100644 --- a/src/Base/Rotation.cpp +++ b/src/Base/Rotation.cpp @@ -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 - // - // 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 // - 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 // 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 diff --git a/src/Base/Rotation.h b/src/Base/Rotation.h index e38f239fe4c93..765d1ec3375ea 100644 --- a/src/Base/Rotation.h +++ b/src/Base/Rotation.h @@ -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 }; }