From 885050d33e60b40ac9cf463129f8ab9b0097b0f4 Mon Sep 17 00:00:00 2001
From: wmayer <wmayer@users.sourceforge.net>
Date: Wed, 18 May 2016 18:08:40 +0200
Subject: [PATCH] + fix == operator of Rotation class, + add method isSame()

---
 src/Base/Rotation.cpp      | 20 ++++++++++++++++----
 src/Base/Rotation.h        |  1 +
 src/Base/RotationPy.xml    | 22 +++++++++++++++++++---
 src/Base/RotationPyImp.cpp | 22 ++++++++++++++++++++--
 4 files changed, 56 insertions(+), 9 deletions(-)

diff --git a/src/Base/Rotation.cpp b/src/Base/Rotation.cpp
index bbefa4b6e..db71026ea 100644
--- a/src/Base/Rotation.cpp
+++ b/src/Base/Rotation.cpp
@@ -294,10 +294,12 @@ 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
@@ -305,6 +307,16 @@ 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
index a0ecf21dc..7ded2c723 100644
--- a/src/Base/Rotation.h
+++ b/src/Base/Rotation.h
@@ -79,6 +79,7 @@ public:
 
     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
index f7ab91cea..42a64d142 100644
--- 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
index 7f9a1fe1c..65cd3849a 100644
--- 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));
 }