diff --git a/MotionPlanning/PostProcessing/ElasticBandProcessor.cpp b/MotionPlanning/PostProcessing/ElasticBandProcessor.cpp
index 7ac85fcab3db697422d0b7f6559a0d952f950e0b..f3c9696fba1a677659a63cdbb317931b81d81977 100644
--- a/MotionPlanning/PostProcessing/ElasticBandProcessor.cpp
+++ b/MotionPlanning/PostProcessing/ElasticBandProcessor.cpp
@@ -175,7 +175,7 @@ using namespace VirtualRobot;
Eigen::VectorXf fc;
- for (int i=1;i<optimizedPath->getNrOfPoints()-1;i++)
+ for (unsigned int i=1;i<optimizedPath->getNrOfPoints()-1;i++)
{
Eigen::VectorXf& before = optimizedPath->getPointRef(i-1);
@@ -227,7 +227,7 @@ using namespace VirtualRobot;
float ssd = 2 * cspace->getSamplingSize()*cspace->getSamplingSize();
- for (int i=1;i<optimizedPath->getNrOfPoints()-2;i++)
+ for (unsigned int i=1;i<optimizedPath->getNrOfPoints()-2;i++)
{
Eigen::VectorXf& before = optimizedPath->getPointRef(i-1);
Eigen::VectorXf& act = optimizedPath->getPointRef(i);
diff --git a/MotionPlanning/examples/PlatformDemo/PlatformWindow.cpp b/MotionPlanning/examples/PlatformDemo/PlatformWindow.cpp
index e17b0638674d528883be32ecbb96de41dbdf029a..da2c062dfb8d61560be68fd0b7e1f0a0c9c3c6c3 100644
--- a/MotionPlanning/examples/PlatformDemo/PlatformWindow.cpp
+++ b/MotionPlanning/examples/PlatformDemo/PlatformWindow.cpp
@@ -580,7 +580,7 @@ void PlatformWindow::sliderSolution(int pos)
std::stringstream d2;
d2 << setprecision(2) << fixed << "Pos: ";
- for (int i=0;i<rns->getSize();i++)
+ for (unsigned int i=0;i<rns->getSize();i++)
d2 << rns->getNode(i)->getJointValue() << ", ";
QString t2(d2.str().c_str());
UI.labelPos->setText(t2);
diff --git a/VirtualRobot/IK/constraints/BalanceConstraint.cpp b/VirtualRobot/IK/constraints/BalanceConstraint.cpp
index fcd6e97063aa9575e869f52e7ad1765c8e42a15f..9a4bb5229241e5b9293faaa2b097548ed1cf2586 100644
--- a/VirtualRobot/IK/constraints/BalanceConstraint.cpp
+++ b/VirtualRobot/IK/constraints/BalanceConstraint.cpp
@@ -70,7 +70,7 @@ Eigen::VectorXf BalanceConstraintOptimizationFunction::evaluateOptimizationGradi
float v = (R * (com - d))[0];
f_i[i] = sigmoid(v);
- f_prime_i[i] = (R * Jcom).row(0) * sigmoid_prime(v);
+ f_prime_i[i] = (R * Jcom).row(0) * float(sigmoid_prime(v));
}
Eigen::VectorXf result(Jcom.cols());
@@ -89,7 +89,7 @@ Eigen::VectorXf BalanceConstraintOptimizationFunction::evaluateOptimizationGradi
prod *= f_i[k];
}
- result += f_prime_i[i] * prod;
+ result += f_prime_i[i] * float(prod);
}
return -result;
@@ -124,7 +124,7 @@ void BalanceConstraintOptimizationFunction::update()
double BalanceConstraintOptimizationFunction::sigmoid(double x)
{
- float beta = 0.1;
+ float beta = 0.1f;
return 1.0 / (1.0 + exp(-beta * x));
}
diff --git a/VirtualRobot/MathTools.cpp b/VirtualRobot/MathTools.cpp
index 8312fc5547c181d514a164f5e50a7e227daa117b..fe43da3cdefcaba3bfcbe8de460146443378ec23 100644
--- a/VirtualRobot/MathTools.cpp
+++ b/VirtualRobot/MathTools.cpp
@@ -1283,6 +1283,14 @@ namespace VirtualRobot
float VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::getAngle(const Quaternion& q)
{
+ Eigen::Quaternionf qe(q.w, q.x, q.y, q.z);
+ Eigen::AngleAxisf aa(qe);
+ float angle = aa.angle();
+ // sometimes angle is >PI?!
+ if (angle > float(M_PI))
+ angle = float(2.0f*M_PI) - angle;
+ return angle;
+ /*
float n = sqrtf(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
if (n < 1e-10)
@@ -1292,7 +1300,7 @@ namespace VirtualRobot
n = 1.0f / n;
- return (float)(2.0f * acosf(q.w * n));
+ return (float)(2.0f * acosf(q.w * n));*/
}
float VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::getAngle(const Eigen::Vector3f& v1, const Eigen::Vector3f& v2)
@@ -1873,8 +1881,29 @@ 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.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;
+
+ 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
+ if (resF(1)<0)
+ resF(1) = float(2.0f * M_PI) + resF(1); // -2PI,2PI -> 0,2PI
- Eigen::Matrix4f m4 = quat2eigen4f(q);
+ return resF;
+ /* Eigen::Matrix4f m4 = quat2eigen4f(q);
Eigen::Matrix3f m3 = m4.block(0,0,3,3);
// ZYZ Euler
@@ -1895,17 +1924,24 @@ namespace VirtualRobot
if (resF(2)<0)
resF(2) = 2*M_PI + resF(2); // -PI,PI -> 0,2PI
- return resF;
+ return resF;*/
}
MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::hopf2quat(const Eigen::Vector3f &hopf)
{
MathTools::Quaternion q;
+ q.x = cos((hopf(0) + hopf(1))*0.5f) * sin(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;
+
+
/*q.w = cos(hopf(0)*0.5f) * cos(hopf(2)*0.5f);
q.x = cos(hopf(0)*0.5f) * sin(hopf(2)*0.5f);
q.y = sin(hopf(0)*0.5f) * cos(hopf(1) + hopf(2)*0.5f);
q.z = sin(hopf(0)*0.5f) * sin(hopf(1) + hopf(2)*0.5f);*/
-
+ /*
Eigen::Vector3f h2 = hopf;
if (h2(0)>M_PI)
h2(0) = h2(0) - 2*M_PI ; // 0,2PI -> -PI,PI
@@ -1927,7 +1963,7 @@ namespace VirtualRobot
q.z = qE.z();
q.w = qE.w();
- return q;
+ return q;*/
}
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index 0a6f3e2702ea544b3319aceba5276abaa5436c12..721c16de385478e1037565c3f7a70b0b9c358564 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -557,7 +557,7 @@ namespace VirtualRobot
circleCompletion = std::min<float>(1.0f, circleCompletion);
circleCompletion = std::max<float>(-1.0f, circleCompletion);
float offset = 0;
- for (int i = 0; i < numberOfCircleParts; ++i)
+ for (size_t i = 0; i < numberOfCircleParts; ++i)
{
SbVec3f point;
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
index 622a021d293678748d26c9a755668f2cdc1e1ea1..dc6627a892ccde009732bd6d02c82bc9099e20f7 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
@@ -454,4 +454,4 @@ namespace VirtualRobot
} // namespace VirtualRobot
-#endif // _VirtualRobot_CoinVisualizationFactory_h_
+#endif // _VirtualRobot_CoinVisualizationFactory_h_
\ No newline at end of file
diff --git a/VirtualRobot/Visualization/TriMeshModel.cpp b/VirtualRobot/Visualization/TriMeshModel.cpp
index 163a1c8ab258adbebb6d9ce93d718028b629e11c..1888e5fd3fe9affd975770dc4639a1d8179e87ce 100644
--- a/VirtualRobot/Visualization/TriMeshModel.cpp
+++ b/VirtualRobot/Visualization/TriMeshModel.cpp
@@ -138,6 +138,15 @@ 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.
@@ -602,24 +611,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)
@@ -656,4 +689,4 @@ namespace VirtualRobot
}
-} // namespace VirtualRobot
+} // namespace VirtualRobot
\ No newline at end of file
diff --git a/VirtualRobot/Visualization/TriMeshModel.h b/VirtualRobot/Visualization/TriMeshModel.h
index ce9eb52a301a5dd7443c67b8cb0b3f25fbb178c6..6848f0744c8f91eac160977b7dda5aa4bd2b30f2 100644
--- a/VirtualRobot/Visualization/TriMeshModel.h
+++ b/VirtualRobot/Visualization/TriMeshModel.h
@@ -62,6 +62,7 @@ namespace VirtualRobot
int addColor(const VisualizationFactory::Color& color);
int addColor(const Eigen::Vector4f& color);
int addMaterial(const VisualizationFactory::PhongMaterial& material);
+ void addFace(unsigned int id0, unsigned int id1, unsigned int id2);
void clear();
void flipVertexOrientations();
/**
@@ -83,6 +84,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/VirtualRobotMathToolsTest.cpp b/VirtualRobot/tests/VirtualRobotMathToolsTest.cpp
index 81ee385eb7353af2a8f8586341e1e31470fdb0a6..6d5eb1de83f00c082ac2988b8c21e54659766dfa 100644
--- a/VirtualRobot/tests/VirtualRobotMathToolsTest.cpp
+++ b/VirtualRobot/tests/VirtualRobotMathToolsTest.cpp
@@ -17,53 +17,103 @@ BOOST_AUTO_TEST_SUITE(MathTools)
BOOST_AUTO_TEST_CASE(testMathToolsHopf)
{
Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
- const int NR_TESTS = 1000;
+ const int NR_TESTS = 10000;
for (int i=0;i<NR_TESTS;i++)
{
Eigen::Vector3f ax = Eigen::Vector3f::Random();
ax.normalize();
- float ang = rand() % 1000 / 1000.0f * 2.0f*M_PI -M_PI;
- float xa = rand() % 1000 / 1000.0f * 100.0f;
- float ya = rand() % 1000 / 1000.0f * 100.0f;
- float za = rand() % 1000 / 1000.0f * 100.0f;
+ float ang = float(rand() % 1000 / 1000.0f * 2.0f*M_PI -M_PI);
Eigen::Matrix3f m3 = Eigen::AngleAxisf(ang, ax).matrix();
m.block(0, 0, 3, 3) = m3;
VirtualRobot::MathTools::Quaternion q = VirtualRobot::MathTools::eigen4f2quat(m);
-
Eigen::Vector3f h = VirtualRobot::MathTools::quat2hopf(q);
BOOST_CHECK_LE(h(0), 2.0f*(float)M_PI);
BOOST_CHECK_LE(h(1), 2.0f*(float)M_PI);
- BOOST_CHECK_LE(h(2), 2.0f*(float)M_PI);
+ BOOST_CHECK_LE(h(2), (float)M_PI*0.5f);
BOOST_CHECK_GE(h(0), 0.0f);
BOOST_CHECK_GE(h(1), 0.0f);
BOOST_CHECK_GE(h(2), 0.0f);
- //BOOST_CHECK_LE(fabs(h(1)), (float)M_PI);
- //BOOST_CHECK_LE(fabs(h(2)), (float)M_PI);
-
- /*BOOST_CHECK_LE(fabs(h(0)), 2.0f*(float)M_PI);
- BOOST_CHECK_LE(fabs(h(1)), (float)M_PI);
- BOOST_CHECK_LE(fabs(h(2)), 2.0f*(float)M_PI);
-*/
VirtualRobot::MathTools::Quaternion q2 = VirtualRobot::MathTools::hopf2quat(h);
- /*
- if (fabs(q.w-q2.w) > 0.1 || fabs(q.x-q2.x) > 0.1 || fabs(q.y-q2.y) > 0.1 || fabs(q.z-q2.z) > 0.1)
+ VirtualRobot::MathTools::Quaternion qDelta = VirtualRobot::MathTools::getDelta(q, q2);
+ float a = (float)fabs(VirtualRobot::MathTools::getAngle(qDelta));
+ BOOST_CHECK_SMALL(a, 0.01f);
+ if (a>0.01)
{
- std::cout << "H:" << h.transpose() << ", Q:" << q.w << ", " << q.x << "," << q.y << "," << q.z << " / Q2:"<< q2.w << ", " << q2.x << "," << q2.y << "," << q2.z << std::endl;
- }*/
- BOOST_CHECK_SMALL((float)fabs(q.w-q2.w),0.1f);
- BOOST_CHECK_SMALL((float)fabs(q.x-q2.x),0.1f);
- BOOST_CHECK_SMALL((float)fabs(q.y-q2.y),0.1f);
- BOOST_CHECK_SMALL((float)fabs(q.z-q2.z),0.1f);
-
+ std::cout << "H:" << h.transpose() << ", Q:" << q.w << ", " << q.x << "," << q.y << "," << q.z << " / Q2:" << q2.w << ", " << q2.x << "," << q2.y << "," << q2.z << std::endl;
+ }
}
+}
+
+
+BOOST_AUTO_TEST_CASE(testMathToolsHopfNeighborhood)
+{
+ Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
+ const int NR_TESTS = 50000;
+ const float maxAngHopf = 0.01f;
+ const float maxAngQuat = 2.5f*maxAngHopf;
+ for (int i = 0; i<NR_TESTS; i++)
+ {
+ Eigen::Vector3f ax = Eigen::Vector3f::Random();
+ ax.normalize();
+ float ang = float(rand() % 1000 / 1000.0f * 2.0f*M_PI - M_PI);
+ Eigen::Matrix3f m3 = Eigen::AngleAxisf(ang, ax).matrix();
+ m.block(0, 0, 3, 3) = m3;
+ VirtualRobot::MathTools::Quaternion q = VirtualRobot::MathTools::eigen4f2quat(m);
+ Eigen::Vector3f h0 = VirtualRobot::MathTools::quat2hopf(q);
+ ax = Eigen::Vector3f::Random();
+ ax.normalize();
+ ax *= maxAngHopf;
+
+ Eigen::Vector3f h = h0 + ax;
+
+ if (h(0) < 0)
+ continue;
+ //h(0) = float(2.0f*M_PI) - h(0);
+ if (h(0) > float(2.0f*M_PI))
+ continue;
+ //h(0) = h(0) - float(2.0f*M_PI);
+ if (h(1) < 0)
+ continue;
+ //h(1) = float(2.0f*M_PI) - h(1);
+ if (h(1) > float(2.0f*M_PI))
+ continue;
+ //h(1) = h(1) - float(2.0f*M_PI);
+ if (h(2) < 0)
+ continue;
+ //h(2) = float(M_PI*0.5f) - h(2);
+ if (h(2) > float(M_PI*0.5f))
+ continue;
+ //h(2) = h(2) - float(M_PI*0.5f);
+
+
+ BOOST_CHECK_GE(h(0), 0.0f);
+ BOOST_CHECK_GE(h(1), 0.0f);
+ BOOST_CHECK_GE(h(2), 0.0f);
+ BOOST_CHECK_LE(h(0), 2.0f*(float)M_PI);
+ BOOST_CHECK_LE(h(1), 2.0f*(float)M_PI);
+ BOOST_CHECK_LE(h(2), (float)M_PI*0.5f);
+
+ VirtualRobot::MathTools::Quaternion q2 = VirtualRobot::MathTools::hopf2quat(h);
+
+ VirtualRobot::MathTools::Quaternion qDelta = VirtualRobot::MathTools::getDelta(q, q2);
+ float a = (float)fabs(VirtualRobot::MathTools::getAngle(qDelta));
+ BOOST_CHECK_SMALL(a, maxAngQuat);
+
+ if (a>maxAngQuat)
+ {
+ std::cout << "H0:" << h0.transpose() << ", " << "h-new:" << h.transpose() << std::endl;
+ std::cout << "Q:" << q.w << ", " << q.x << "," << q.y << "," << q.z << " / Q2:" << q2.w << ", " << q2.x << "," << q2.y << "," << q2.z << std::endl;
+ }
+ }
}
+
BOOST_AUTO_TEST_CASE(testMathToolsRPY)
{
float r = (float)M_PI * 0.25f;
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);
}