+ fix == operator of Rotation class, + add method isSame()

FreeCAD · May 18, 2016 · 885050d · 885050d
1 parent 205cf94
commit 885050d
Show file tree

Hide file tree

Showing 4 changed files with 56 additions and 9 deletions.
diff --git a/src/Base/Rotation.cpp b/src/Base/Rotation.cpp
@@ -294,17 +294,29 @@ Rotation Rotation::operator*(const Rotation & q) const
 
 bool Rotation::operator==(const Rotation & q) const
 {
-    bool equal = true;
-    for (int i=0; i<4;i++)
-        equal &= (fabs(this->quat[i] - q.quat[i]) < 0.005 );
-    return equal;
+    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])
+        return true;
+    return false;
 }
 
 bool Rotation::operator!=(const Rotation & q) const
 {
     return !(*this == q);
 }
 
+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]))
+        return true;
+    return false;
+}
+
 void Rotation::multVec(const Vector3d & src, Vector3d & dst) const
 {
     double x = this->quat[0];

diff --git a/src/Base/Rotation.h b/src/Base/Rotation.h
@@ -79,6 +79,7 @@ class BaseExport Rotation
 
     void multVec(const Vector3d & src, Vector3d & dst) const;
     void scaleAngle(const double scaleFactor);
+    bool isSame(const Rotation&) const;
     //@}
 
     static Rotation slerp(const Rotation & rot0, const Rotation & rot1, double t);

diff --git a/src/Base/RotationPy.xml b/src/Base/RotationPy.xml
@@ -30,12 +30,28 @@
 		<Methode Name="invert">
 			<Documentation>
 				<UserDocu>
-					move(Vector)
-					Move the matrix along the vector
+                    invert() -> None
+                    Sets the rotation to its inverse
 				</UserDocu>
 			</Documentation>
 		</Methode>
-                <Methode Name="multiply" Const="true">
+        <Methode Name="inverted">
+            <Documentation>
+                <UserDocu>
+                    inverted() -> Rotation
+                    Returns the inverse of the rotation
+                </UserDocu>
+            </Documentation>
+        </Methode>
+        <Methode Name="isSame">
+            <Documentation>
+                <UserDocu>
+                    isSame(Rotation)
+                    Checks if the two quaternions perform the same rotation
+                </UserDocu>
+            </Documentation>
+        </Methode>
+        <Methode Name="multiply" Const="true">
 			<Documentation>
 				<UserDocu>
 					multiply(Rotation)

diff --git a/src/Base/RotationPyImp.cpp b/src/Base/RotationPyImp.cpp
@@ -203,6 +203,14 @@ PyObject* RotationPy::invert(PyObject * args)
     Py_Return;
 }
 
+PyObject* RotationPy::inverted(PyObject * args)
+{
+    if (!PyArg_ParseTuple(args, ""))
+        return 0;
+    Rotation mult = this->getRotationPtr()->inverse();
+    return new RotationPy(new Rotation(mult));
+}
+
 PyObject* RotationPy::multiply(PyObject * args)
 {
     PyObject *rot;
@@ -236,14 +244,24 @@ PyObject* RotationPy::toEuler(PyObject * args)
     return Py::new_reference_to(tuple);
 }
 
+PyObject* RotationPy::isSame(PyObject *args)
+{
+    PyObject *rot;
+    if (!PyArg_ParseTuple(args, "O!", &(RotationPy::Type), &rot))
+        return NULL;
+    Base::Rotation rot1 = * getRotationPtr();
+    Base::Rotation rot2 = * static_cast<RotationPy*>(rot)->getRotationPtr();
+    bool same = rot1.isSame(rot2);
+    return Py_BuildValue("O", (same ? Py_True : Py_False));
+}
+
 PyObject* RotationPy::isNull(PyObject *args)
 {
     if (!PyArg_ParseTuple(args, ""))
         return NULL;
     Base::Rotation rot = * getRotationPtr();
     Base::Rotation nullrot(0,0,0,1);
-    Base::Rotation nullrotinv(0,0,0,-1);
-    bool null = (rot == nullrot) | (rot == nullrotinv);
+    bool null = rot.isSame(nullrot);
     return Py_BuildValue("O", (null ? Py_True : Py_False));
 }