diff --git a/modules/geom/pymod/export_quat.cc b/modules/geom/pymod/export_quat.cc
index 4e136ea8ee9c7c7fdf4c1a1ee793bb6e1977a74a..7e233f69c363e9a5c568545c302f8c0a6df3fa1d 100644
--- a/modules/geom/pymod/export_quat.cc
+++ b/modules/geom/pymod/export_quat.cc
@@ -59,7 +59,6 @@ void export_Quat()
;
def("Conjugate",&Conjugate);
def("Slerp",&Slerp);
- def("Grassman",&Grassmann);
def("Normalize",normalize);
}
diff --git a/modules/geom/src/composite3.cc b/modules/geom/src/composite3.cc
index 751901b303d3b25f2b863c6767a7b21ff3343948..347629d2c059cd3be0ecdef523ab65363e20adb3 100644
--- a/modules/geom/src/composite3.cc
+++ b/modules/geom/src/composite3.cc
@@ -266,10 +266,7 @@ void Rotation3::SetRotationMatrix(const Mat3& rot)
}
Vec3 Rotation3::Apply(const Vec3& v) const
{
- // We can use Conjugate instead of Invert because q is guaranteed to
- // be unit Quat
- return origin_+(Grassmann(Grassmann(q_,Quat(0,v-origin_)),
- Conjugate(q_))).GetAxis();
+ return origin_+q_.Rotate(v-origin_);
}
bool Rotation3::operator==(const Rotation3& rhs) const
diff --git a/modules/geom/src/quat.cc b/modules/geom/src/quat.cc
index f9b852cae7799478c8f0ca3bfb599c3b80821421..44e39ee2bb71d9c308d07cfb8a43ffa8403e6f8f 100644
--- a/modules/geom/src/quat.cc
+++ b/modules/geom/src/quat.cc
@@ -455,30 +455,12 @@ Quat Log(const Quat& q)
Vec3 Quat::Rotate(const Vec3& vec) const {
Quat tmp(0.0, vec[0], vec[1], vec[2]);
+ // We use Conjugate instead of Invert here because we assume *this to be normalized
Quat conj=Conjugate(*this);
Quat res=*this*tmp*conj;
return Vec3(res.x, res.y, res.z);
}
-Quat Grassmann(const Quat& lhs, const Quat& rhs)
-{
- return Quat(lhs.GetAngle()*rhs.GetAngle()-
- lhs.GetAxis().x*rhs.GetAxis().x-
- lhs.GetAxis().y*rhs.GetAxis().y-
- lhs.GetAxis().z*rhs.GetAxis().z,
- lhs.GetAngle()*rhs.GetAxis().x+
- lhs.GetAxis().x*rhs.GetAngle()+
- lhs.GetAxis().y*rhs.GetAxis().z-
- lhs.GetAxis().z*rhs.GetAxis().y,
- lhs.GetAngle()*rhs.GetAxis().y-
- lhs.GetAxis().x*rhs.GetAxis().z+
- lhs.GetAxis().y*rhs.GetAngle()+
- lhs.GetAxis().z*rhs.GetAxis().x,
- lhs.GetAngle()*rhs.GetAxis().z+
- lhs.GetAxis().x*rhs.GetAxis().y-
- lhs.GetAxis().y*rhs.GetAxis().x+
- lhs.GetAxis().z*rhs.GetAngle());
-}
std::ostream& operator<<(std::ostream& str, const Quat& q)
{
diff --git a/modules/geom/src/quat.hh b/modules/geom/src/quat.hh
index dc206e4e5b9a30fd8b031bf539c61fa9ea83cdf1..881f69b5bdd8a8112eefe3ffc642ceed525efe42 100644
--- a/modules/geom/src/quat.hh
+++ b/modules/geom/src/quat.hh
@@ -116,8 +116,6 @@ Quat DLLEXPORT_OST_GEOM Inv(const Quat& q);
Quat DLLEXPORT_OST_GEOM Exp(const Quat& q);
Quat DLLEXPORT_OST_GEOM Log(const Quat& q);
-Quat DLLEXPORT_OST_GEOM Grassmann(const Quat& lhs, const Quat& rhs);
-
//normalize quaternion
Quat DLLEXPORT_OST_GEOM Normalize(const Quat& q);
diff --git a/modules/geom/tests/CMakeLists.txt b/modules/geom/tests/CMakeLists.txt
index 42d7f4c43b393411b5c7edeaecec9bc1e8bc2ca4..027a167490b6256249a53a4cf1e569a237f3c8af 100644
--- a/modules/geom/tests/CMakeLists.txt
+++ b/modules/geom/tests/CMakeLists.txt
@@ -7,6 +7,7 @@ set(GEOM_UNITTESTS
test_op2.cc
test_op3.cc
test_op4.cc
+ test_quat.cc
test_vec2.cc
test_vec3.cc
test_vec4.cc
diff --git a/modules/geom/tests/test_composite3.cc b/modules/geom/tests/test_composite3.cc
index 6548b53da1d70c45acb5e39b5ecd5e9584a998a8..5602df4dda6492cd7f818066286a4355c40d81df 100644
--- a/modules/geom/tests/test_composite3.cc
+++ b/modules/geom/tests/test_composite3.cc
@@ -17,6 +17,7 @@
// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
//------------------------------------------------------------------------------
+#include <cmath>
#include <ost/geom/geom.hh>
#include "helper.hh"
@@ -193,4 +194,14 @@ BOOST_AUTO_TEST_CASE(func_composite3)
}
+BOOST_AUTO_TEST_CASE(rotation3)
+{
+ Vec3 v(1,0,0);
+ Rotation3 r(Vec3(0,1,0), 30.0*M_PI/180.0);
+ Vec3 vrot=r.Apply(v);
+ BOOST_CHECK_CLOSE(cos(30.0*M_PI/180.0),vrot[0],float(1e-5));
+ BOOST_CHECK_SMALL(vrot[1],float(1e-5));
+ BOOST_CHECK_CLOSE(-sin(30.0*M_PI/180.0),vrot[2],float(1e-5));
+}
+
BOOST_AUTO_TEST_SUITE_END()
diff --git a/modules/geom/tests/test_quat.cc b/modules/geom/tests/test_quat.cc
new file mode 100644
index 0000000000000000000000000000000000000000..0869fd518912d3f55f3c6bfd52b12955b238a391
--- /dev/null
+++ b/modules/geom/tests/test_quat.cc
@@ -0,0 +1,58 @@
+//------------------------------------------------------------------------------
+// This file is part of the OpenStructure project <www.openstructure.org>
+//
+// Copyright (C) 2008-2011 by the OpenStructure authors
+//
+// This library is free software; you can redistribute it and/or modify it under
+// the terms of the GNU Lesser General Public License as published by the Free
+// Software Foundation; either version 3.0 of the License, or (at your option)
+// any later version.
+// This library is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
+// details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this library; if not, write to the Free Software Foundation, Inc.,
+// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+//------------------------------------------------------------------------------
+
+#include <ost/geom/geom.hh>
+
+#include "helper.hh"
+using namespace geom;
+
+#define BOOST_TEST_DYN_LINK
+#include <boost/test/unit_test.hpp>
+
+BOOST_AUTO_TEST_SUITE( geom )
+
+BOOST_AUTO_TEST_CASE(init_quat)
+{
+ // default
+ Quat q1;
+ BOOST_CHECK_CLOSE(q1.w,1.0,1.0e-5);
+ BOOST_CHECK_CLOSE(q1.x,0.0,1.0e-5);
+ BOOST_CHECK_CLOSE(q1.y,0.0,1.0e-5);
+ BOOST_CHECK_CLOSE(q1.z,0.0,1.0e-5);
+
+ Quat q2(2.0,3.0,4.0,5.0);
+ BOOST_CHECK_CLOSE(q2.w,2.0,1.0e-5);
+ BOOST_CHECK_CLOSE(q2.x,3.0,1.0e-5);
+ BOOST_CHECK_CLOSE(q2.y,4.0,1.0e-5);
+ BOOST_CHECK_CLOSE(q2.z,5.0,1.0e-5);
+}
+
+BOOST_AUTO_TEST_CASE(quat_rotate)
+{
+ Vec3 v(1,0,0);
+ Quat q(30.0*M_PI/180.0,Vec3(0,1,0));
+ Vec3 vrot=q.Rotate(v);
+ BOOST_CHECK_CLOSE(cos(30.0*M_PI/180.0),vrot[0],float(1e-5));
+ BOOST_CHECK_SMALL(vrot[1],float(1e-5));
+ BOOST_CHECK_CLOSE(-sin(30.0*M_PI/180.0),vrot[2],float(1e-5));
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()
+