diff --git a/VirtualRobot/MathTools.cpp b/VirtualRobot/MathTools.cpp
index fb194bffe2485dbca6ff805300c40b86463ceab7..fe43da3cdefcaba3bfcbe8de460146443378ec23 100644
--- a/VirtualRobot/MathTools.cpp
+++ b/VirtualRobot/MathTools.cpp
@@ -1881,11 +1881,21 @@ namespace VirtualRobot
Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::quat2hopf(const MathTools::Quaternion &q)
{
Eigen::Vector3f resF;
- float a_x4_x3 = atan2(q.z, q.y);
- float a_x1_x2 = atan2(q.w, q.x);
+ float a_x4_x3 = atan2(q.z, q.w);
+ float a_x1_x2 = atan2(q.y, q.x);
resF(0) = a_x1_x2 - a_x4_x3;
resF(1) = a_x4_x3 + a_x1_x2;
- resF(2) = asin(sqrt(q.w*q.w + q.x*q.x));
+
+ double p = sqrt(double(q.y)*double(q.y) + double(q.x)*double(q.x));
+
+ if (fabs(p-1) < 1e-6)
+ resF(2) = float(M_PI*0.5);
+ else if (fabs(p+1) < 1e-6)
+ resF(2) = float(-M_PI*0.5);
+ else
+ resF(2) = asin(p);
+
+ VR_ASSERT (!isnan(resF(2)));
if (resF(0)<0)
resF(0) = float(2.0f * M_PI) + resF(0); // -2PI,2PI -> 0,2PI
@@ -1921,8 +1931,8 @@ namespace VirtualRobot
{
MathTools::Quaternion q;
q.x = cos((hopf(0) + hopf(1))*0.5f) * sin(hopf(2));
- q.w = sin((hopf(0) + hopf(1))*0.5f) * sin(hopf(2));
- q.y = cos((hopf(1) - hopf(0))*0.5f) * cos(hopf(2));
+ q.y = sin((hopf(0) + hopf(1))*0.5f) * sin(hopf(2));
+ q.w = cos((hopf(1) - hopf(0))*0.5f) * cos(hopf(2)); // q.w needs to get zero if hopf coords are zero
q.z = sin((hopf(1) - hopf(0))*0.5f) * cos(hopf(2));
return q;
diff --git a/VirtualRobot/Visualization/TriMeshModel.cpp b/VirtualRobot/Visualization/TriMeshModel.cpp
index 1bce94244f9dcaca2915d206d0e2229f01f0505c..8897224719b47487119bbd22aa80ddc8be65a52e 100644
--- a/VirtualRobot/Visualization/TriMeshModel.cpp
+++ b/VirtualRobot/Visualization/TriMeshModel.cpp
@@ -135,19 +135,29 @@ namespace VirtualRobot
faces.push_back(face);
}
+ void TriMeshModel::addFace(unsigned int id0, unsigned int id1, unsigned int id2)
+ {
+ MathTools::TriangleFace f;
+ f.id1 = id0;
+ f.id2 = id1;
+ f.id3 = id2;
+ addFace(f);
+ }
+
/**
* This method adds a vertex to the internal data structure TriMeshModel::vertices.
*/
- void TriMeshModel::addVertex(const Eigen::Vector3f& vertex)
+ unsigned int TriMeshModel::addVertex(const Eigen::Vector3f& vertex)
{
if (std::isnan(vertex[0]) || std::isnan(vertex[1]) || std::isnan(vertex[2]))
{
VR_ERROR << "NAN vertex added!!!" << endl;
- return;
+ return 0;
}
vertices.push_back(vertex);
boundingBox.addPoint(vertex);
+ return vertices.size()-1;
}
/**
@@ -408,24 +418,48 @@ namespace VirtualRobot
return flippedFacesCount;
}
+
void TriMeshModel::print()
{
cout << "TriMeshModel\nNr of Faces:" << faces.size() << "\nNr of vertices:" << vertices.size() << endl;
- cout << "Normals:" << endl;
+ boundingBox.print();
+ }
+ void TriMeshModel::printNormals()
+ {
+ cout << "TriMeshModel Normals:" << endl;
std::streamsize pr = cout.precision(2);
-
for (size_t i = 0; i < faces.size(); i++)
{
cout << "<" << faces[i].normal(0) << "," << faces[i].normal(1) << "," << faces[i].normal(2) << ">,";
}
+ cout.precision(pr);
+ }
- cout << endl;
- boundingBox.print();
+ void TriMeshModel::printVertices()
+ {
+ cout << "TriMeshModel Vertices:" << endl;
+ std::streamsize pr = cout.precision(2);
+ for (size_t i = 0; i < vertices.size(); i++)
+ {
+ cout << vertices[i].transpose() << endl;
+ }
cout.precision(pr);
}
+ void TriMeshModel::printFaces()
+ {
+ cout << "TriMeshModel Faces (vertex indices):" << endl;
+ std::streamsize pr = cout.precision(2);
+ for (size_t i = 0; i < faces.size(); i++)
+ {
+ cout << faces[i].id1 << "," << faces[i].id2 << "," << faces[i].id3 << endl;
+ }
+ cout.precision(pr);
+ }
+
+
void TriMeshModel::scale(Eigen::Vector3f& scaleFactor)
{
if (scaleFactor(0) == 1.0f && scaleFactor(1) == 1.0f && scaleFactor(2) == 1.0f)
diff --git a/VirtualRobot/Visualization/TriMeshModel.h b/VirtualRobot/Visualization/TriMeshModel.h
index 70f9a59f9b745950585b08ac3485e168ea8ca945..1f577aa98567605a7be0def539c71f265057a8df 100644
--- a/VirtualRobot/Visualization/TriMeshModel.h
+++ b/VirtualRobot/Visualization/TriMeshModel.h
@@ -57,7 +57,13 @@ namespace VirtualRobot
void addTriangleWithFace(Eigen::Vector3f& vertex1, Eigen::Vector3f& vertex2, Eigen::Vector3f& vertex3, Eigen::Vector4f& vertexColor1, Eigen::Vector4f& vertexColor2, Eigen::Vector4f& vertexColor3);
static Eigen::Vector3f CreateNormal(Eigen::Vector3f& vertex1, Eigen::Vector3f& vertex2, Eigen::Vector3f& vertex3);
void addFace(const MathTools::TriangleFace& face);
- void addVertex(const Eigen::Vector3f& vertex);
+ void addFace(unsigned int id0, unsigned int id1, unsigned int id2);
+ /*!
+ * \brief addVertex Adds a vertex to this object
+ * \param vertex
+ * \return The vertex id (== position in vertex array)
+ */
+ unsigned int addVertex(const Eigen::Vector3f& vertex);
void addNormal(const Eigen::Vector3f& normal);
void addColor(const VisualizationFactory::Color& color);
void addColor(const Eigen::Vector4f& color);
@@ -69,6 +75,9 @@ namespace VirtualRobot
void setColor(VisualizationFactory::Color color);
void print();
+ void printNormals();
+ void printVertices();
+ void printFaces();
Eigen::Vector3f getCOM();
bool getSize(Eigen::Vector3f& storeMinSize, Eigen::Vector3f& storeMaxSize);
bool checkFacesHaveSameEdge(const MathTools::TriangleFace& face1, const MathTools::TriangleFace& face2, std::vector<std::pair<int, int> >& commonVertexIds) const;
diff --git a/VirtualRobot/Visualization/VisualizationFactory.h b/VirtualRobot/Visualization/VisualizationFactory.h
index c634b19711a240d339a0b47dc341213aa0548d80..5c446c61074f7abe063679478612f4c98b240b21 100644
--- a/VirtualRobot/Visualization/VisualizationFactory.h
+++ b/VirtualRobot/Visualization/VisualizationFactory.h
@@ -140,17 +140,17 @@ namespace VirtualRobot
{
return VisualizationNodePtr();
}
- virtual VisualizationNodePtr createCircle(float radius, float circleCompletion, float width, float colorR = 1.0f, float colorG = 0.5f, float colorB = 0.5f, size_t numberOfCircleParts = 30)
+ virtual VisualizationNodePtr createCircle(float /*radius*/, float /*circleCompletion*/, float /*width*/, float /*colorR*/ = 1.0f, float /*colorG*/ = 0.5f, float /*colorB*/ = 0.5f, size_t /*numberOfCircleParts*/ = 30)
{
return VisualizationNodePtr();
}
- virtual VisualizationNodePtr createTorus(float radius, float tubeRadius, float completion = 1.0f, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f, float transparency = 0.0f, int sides = 8, int rings = 30)
+ virtual VisualizationNodePtr createTorus(float /*radius*/, float /*tubeRadius*/, float /*completion*/ = 1.0f, float /*colorR*/ = 0.5f, float /*colorG*/ = 0.5f, float /*colorB*/ = 0.5f, float /*transparency*/ = 0.0f, int /*sides*/ = 8, int /*rings*/ = 30)
{
return VisualizationNodePtr();
}
- virtual VisualizationNodePtr createCircleArrow(float radius, float tubeRadius, float completion = 1, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f, float transparency = 0.0f, int sides = 8, int rings = 30)
+ virtual VisualizationNodePtr createCircleArrow(float /*radius*/, float /*tubeRadius*/, float /*completion*/ = 1, float /*colorR*/ = 0.5f, float /*colorG*/ = 0.5f, float /*colorB*/ = 0.5f, float /*transparency*/ = 0.0f, int /*sides*/ = 8, int /*rings*/ = 30)
{
return VisualizationNodePtr();
}
diff --git a/VirtualRobot/tests/VirtualRobotWorkSpaceTest.cpp b/VirtualRobot/tests/VirtualRobotWorkSpaceTest.cpp
index 6a8637f3b68bb7a481b8a7b304b6f9acd1fea43a..55db998f0218b69fa8bf24044ad0c83911216bb7 100644
--- a/VirtualRobot/tests/VirtualRobotWorkSpaceTest.cpp
+++ b/VirtualRobot/tests/VirtualRobotWorkSpaceTest.cpp
@@ -35,16 +35,19 @@ BOOST_AUTO_TEST_CASE(testWorkSpaceEuler)
ws->setOrientationType(VirtualRobot::WorkspaceRepresentation::Hopf);
Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
float x[6];
+ Eigen::Vector3f ax;
+ float a;
+ Eigen::Matrix3f m3;
// identity, matrix -> vector
ws->matrix2Vector(m, x);
- BOOST_CHECK_CLOSE(x[0], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(x[1], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(x[2], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(x[3], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(x[4], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(x[5], 0.0f, 1e-6f);
+ BOOST_CHECK_SMALL(x[0], 1e-6f);
+ BOOST_CHECK_SMALL(x[1], 1e-6f);
+ BOOST_CHECK_SMALL(x[2], 1e-6f);
+ BOOST_CHECK_SMALL(x[3], 1e-6f);
+ BOOST_CHECK_SMALL(x[4], 1e-6f);
+ BOOST_CHECK_SMALL(x[5], 1e-6f);
// identity, vector -> matrix
for (int i = 0; i < 6; i++)
@@ -53,33 +56,33 @@ BOOST_AUTO_TEST_CASE(testWorkSpaceEuler)
}
ws->vector2Matrix(x, m);
- BOOST_CHECK_CLOSE(m(0, 3), 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(1, 3), 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(2, 3), 0.0f, 1e-6f);
- Eigen::Vector3f ax;
- float a;
+ BOOST_CHECK_SMALL(m(0, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(1, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(2, 3), 1e-6f);
+
VirtualRobot::MathTools::eigen4f2axisangle(m, ax, a);
BOOST_CHECK_CLOSE(a, 0.0f, 1e-6f);
// rot x
+
m.setIdentity();
- Eigen::Matrix3f m3 = Eigen::AngleAxisf(float(M_PI) / 4.0f, Eigen::Vector3f::UnitX()).matrix();
+ m3 = Eigen::AngleAxisf(float(M_PI) / 4.0f, Eigen::Vector3f::UnitX()).matrix();
m.block(0, 0, 3, 3) = m3;
ws->matrix2Vector(m, x);
ws->vector2Matrix(x, m);
- BOOST_CHECK_CLOSE(x[0], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(x[1], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(x[2], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(0, 3), 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(1, 3), 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(2, 3), 0.0f, 1e-6f);
+ BOOST_CHECK_SMALL(x[0], 1e-6f);
+ BOOST_CHECK_SMALL(x[1], 1e-6f);
+ BOOST_CHECK_SMALL(x[2], 1e-6f);
+ BOOST_CHECK_SMALL(m(0, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(1, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(2, 3), 1e-6f);
VirtualRobot::MathTools::eigen4f2axisangle(m, ax, a);
- BOOST_CHECK_CLOSE(a, float(M_PI) / 4.0f, 1e-3f);
- BOOST_CHECK_CLOSE(ax(0), 1.0f, 1e-6f);
- BOOST_CHECK_CLOSE(ax(1), 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(ax(2), 0.0f, 1e-6f);
+ BOOST_CHECK_SMALL(a - float(M_PI) / 4.0f, 1e-3f);
+ BOOST_CHECK_SMALL(ax(0) - 1.0f, 1e-6f);
+ BOOST_CHECK_SMALL(ax(1), 1e-6f);
+ BOOST_CHECK_SMALL(ax(2), 1e-6f);
// rot y
@@ -88,17 +91,17 @@ BOOST_AUTO_TEST_CASE(testWorkSpaceEuler)
m.block(0, 0, 3, 3) = m3;
ws->matrix2Vector(m, x);
ws->vector2Matrix(x, m);
- BOOST_CHECK_CLOSE(x[0], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(x[1], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(x[2], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(0, 3), 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(1, 3), 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(2, 3), 0.0f, 1e-6f);
+ BOOST_CHECK_SMALL(x[0], 1e-6f);
+ BOOST_CHECK_SMALL(x[1], 1e-6f);
+ BOOST_CHECK_SMALL(x[2], 1e-6f);
+ BOOST_CHECK_SMALL(m(0, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(1, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(2, 3), 1e-6f);
VirtualRobot::MathTools::eigen4f2axisangle(m, ax, a);
- BOOST_CHECK_CLOSE(a, float(M_PI) / 4.0f, 1e-3f);
- BOOST_CHECK_CLOSE(ax(1), 1.0f, 1e-6f);
- BOOST_CHECK_CLOSE(ax(0), 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(ax(2), 0.0f, 1e-6f);
+ BOOST_CHECK_SMALL(a - float(M_PI) / 4.0f, 1e-3f);
+ BOOST_CHECK_SMALL(ax(1) - 1.0f, 1e-6f);
+ BOOST_CHECK_SMALL(ax(0), 1e-6f);
+ BOOST_CHECK_SMALL(ax(2), 1e-6f);
// rot z
m.setIdentity();
@@ -106,17 +109,17 @@ BOOST_AUTO_TEST_CASE(testWorkSpaceEuler)
m.block(0, 0, 3, 3) = m3;
ws->matrix2Vector(m, x);
ws->vector2Matrix(x, m);
- BOOST_CHECK_CLOSE(x[0], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(x[1], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(x[2], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(0, 3), 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(1, 3), 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(2, 3), 0.0f, 1e-6f);
+ BOOST_CHECK_SMALL(x[0], 1e-6f);
+ BOOST_CHECK_SMALL(x[1], 1e-6f);
+ BOOST_CHECK_SMALL(x[2], 1e-6f);
+ BOOST_CHECK_SMALL(m(0, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(1, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(2, 3), 1e-6f);
VirtualRobot::MathTools::eigen4f2axisangle(m, ax, a);
- BOOST_CHECK_CLOSE(a, float(M_PI) / 4.0f, 1e-3f);
- BOOST_CHECK_CLOSE(ax(2), 1.0f, 1e-6f);
- BOOST_CHECK_CLOSE(ax(1), 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(ax(0), 0.0f, 1e-6f);
+ BOOST_CHECK_SMALL(a - float(M_PI) / 4.0f, 1e-3f);
+ BOOST_CHECK_SMALL(ax(2) - 1.0f, 1e-6f);
+ BOOST_CHECK_SMALL(ax(1), 1e-6f);
+ BOOST_CHECK_SMALL(ax(0), 1e-6f);
// rot x,y
@@ -127,16 +130,16 @@ BOOST_AUTO_TEST_CASE(testWorkSpaceEuler)
m.block(0, 0, 3, 3) = m3;
ws->matrix2Vector(m, x);
ws->vector2Matrix(x, m);
- BOOST_CHECK_CLOSE(x[0], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(x[1], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(x[2], 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(0, 3), 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(1, 3), 0.0f, 1e-6f);
- BOOST_CHECK_CLOSE(m(2, 3), 0.0f, 1e-6f);
+ BOOST_CHECK_SMALL(x[0], 1e-6f);
+ BOOST_CHECK_SMALL(x[1], 1e-6f);
+ BOOST_CHECK_SMALL(x[2], 1e-6f);
+ BOOST_CHECK_SMALL(m(0, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(1, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(2, 3), 1e-6f);
VirtualRobot::MathTools::eigen4f2axisangle(m, ax, a);
- BOOST_CHECK_CLOSE(a, float(M_PI) / 4.0f, 1e-3f);
- BOOST_CHECK_CLOSE(ax(0), 1.0f / sqrt(2.0f), 1e-3f);
- BOOST_CHECK_CLOSE(ax(1), 1.0f / sqrt(2.0f), 1e-3f);
+ BOOST_CHECK_SMALL(a - float(M_PI) / 4.0f, 1e-3f);
+ BOOST_CHECK_SMALL(ax(0) - 1.0f / float(sqrt(2.0f)), 1e-3f);
+ BOOST_CHECK_SMALL(ax(1) - 1.0f / float(sqrt(2.0f)), 1e-3f);
BOOST_CHECK_SMALL(ax(2), 1e-4f);
}