Merge pull request #669 from dartsim/feature/constants

toRadian(), toDegree, and mathematical constants

apps/atlasSimbicon/Main.cpp

@@ -45,6 +45,7 @@
 using namespace std;
 using namespace Eigen;
 using namespace dart::common;
+using namespace dart::math;
 using namespace dart::dynamics;
 using namespace dart::simulation;
 using namespace dart::utils;
@@ -67,7 +68,7 @@ int main(int argc, char* argv[])
 
   // Set initial configuration for Atlas robot
   VectorXd q = atlas->getPositions();
-  q[0] = -0.5 * DART_PI;
+  q[0] = -0.5 * constantsd::pi();
   atlas->setPositions(q);
 
   // Set gravity of the world

apps/hardcodedDesign/Main.cpp

@@ -65,8 +65,8 @@ int main(int argc, char* argv[]) {
   dart::dynamics::RevoluteJoint::Properties joint;
   joint.mName = "LHY";
   joint.mAxis = Eigen::Vector3d(0.0, 0.0, 1.0);
-  joint.mPositionLowerLimit = -DART_PI;
-  joint.mPositionUpperLimit =  DART_PI;
+  joint.mPositionLowerLimit = -dart::math::constantsd::pi();
+  joint.mPositionUpperLimit =  dart::math::constantsd::pi();
 
   // You can get the newly created Joint and BodyNode pointers like this
   std::pair<dart::dynamics::Joint*, dart::dynamics::BodyNode*> pair =

apps/rigidShapes/MyWindow.cpp

@@ -77,7 +77,8 @@ Isometry3d getRandomTransform()
 {
   Isometry3d T = Isometry3d::Identity();
 
-  const Vector3d rotation = math::randomVector<3>(-DART_PI, DART_PI);
+  const Vector3d rotation = math::randomVector<3>(-math::constantsd::pi(),
+                                                   math::constantsd::pi());
   const Vector3d position = Vector3d(math::random(-1.0, 1.0),
                                      math::random( 0.5, 1.0),
                                      math::random(-1.0, 1.0));

apps/vehicle/MyWindow.cpp

@@ -77,6 +77,9 @@ void MyWindow::drawWorld() const {
 }
 
 void MyWindow::keyboard(unsigned char _key, int _x, int _y) {
+
+  using namespace dart::math::suffixes;
+
   switch (_key) {
     case ' ':  // use space key to play or stop the motion
       mSimulating = !mSimulating;
@@ -112,23 +115,23 @@ void MyWindow::keyboard(unsigned char _key, int _x, int _y) {
       mShowMarkers = !mShowMarkers;
       break;
     case 'w':  // move forward
-      mBackWheelVelocity = DART_RADIAN * -420.0;
+      mBackWheelVelocity = -420.0_deg;
       break;
     case 's':  // stop
-      mBackWheelVelocity = DART_RADIAN * 0.0;
+      mBackWheelVelocity = 0.0_deg;
       break;
     case 'x':  // move backward
-      mBackWheelVelocity = DART_RADIAN * +420.0;
+      mBackWheelVelocity = +420.0_deg;
       break;
     case 'a':  // rotate steering wheels to left
-      mSteeringWheelAngle += DART_RADIAN * +10;
-      if (mSteeringWheelAngle > DART_RADIAN * 30.0)
-        mSteeringWheelAngle = DART_RADIAN * 30.0;
+      mSteeringWheelAngle += +10_deg;
+      if (mSteeringWheelAngle > 30.0_deg)
+        mSteeringWheelAngle = 30.0_deg;
       break;
     case 'd':  // rotate steering wheels to right
-      mSteeringWheelAngle += DART_RADIAN * -10;
-      if (mSteeringWheelAngle < DART_RADIAN * -30.0)
-        mSteeringWheelAngle = DART_RADIAN * -30.0;
+      mSteeringWheelAngle += -10_deg;
+      if (mSteeringWheelAngle < -30.0_deg)
+        mSteeringWheelAngle = -30.0_deg;
       break;
     default:
       Win3D::keyboard(_key, _x, _y);

dart/collision/dart/DARTCollide.cpp

@@ -39,7 +39,6 @@
 
 #include <memory>
 
-#include "dart/math/Helpers.h"
 #include "dart/dynamics/BoxShape.h"
 #include "dart/dynamics/EllipsoidShape.h"
 #include "dart/dynamics/CylinderShape.h"
@@ -165,13 +164,13 @@ void cullPoints (int n, double p[], int m, int i0, int iret[])
   iret[0] = i0;
   iret++;
   for (j=1; j<m; j++) {
-    a = double(j)*(2*DART_PI/m) + A[i0];
-    if (a > DART_PI) a -= 2*DART_PI;
+    a = double(j)*(2*math::constantsd::pi()/m) + A[i0];
+    if (a > math::constantsd::pi()) a -= 2*math::constantsd::pi();
     double maxdiff=1e9,diff;
     for (i=0; i<n; i++) {
       if (avail[i]) {
         diff = fabs (A[i]-a);
-        if (diff > DART_PI) diff = 2*DART_PI - diff;
+        if (diff > math::constantsd::pi()) diff = 2*math::constantsd::pi() - diff;
         if (diff < maxdiff) {
           maxdiff = diff;
           *iret = i;

dart/constraint/ContactConstraint.cpp

@@ -39,12 +39,12 @@
 #include <iostream>
 
 #include "dart/common/Console.h"
-#include "dart/math/Helpers.h"
 #include "dart/dynamics/BodyNode.h"
 #include "dart/dynamics/Skeleton.h"
 #include "dart/collision/CollisionObject.h"
 #include "dart/lcpsolver/lcp.h"
 
+#define DART_EPSILON 1e-6
 #define DART_ERROR_ALLOWANCE 0.0
 #define DART_ERP     0.01
 #define DART_MAX_ERV 1e-3
@@ -786,6 +786,8 @@ void ContactConstraint::updateFirstFrictionalDirection()
 Eigen::MatrixXd ContactConstraint::getTangentBasisMatrixODE(
     const Eigen::Vector3d& _n)
 {
+  using namespace math::suffixes;
+
   // TODO(JS): Use mNumFrictionConeBases
   // Check if the number of bases is even number.
 //  bool isEvenNumBases = mNumFrictionConeBases % 2 ? true : false;
@@ -809,7 +811,7 @@ Eigen::MatrixXd ContactConstraint::getTangentBasisMatrixODE(
   // Each basis and its opposite belong in the matrix, so we iterate half as
   // many times
   T.col(0) = tangent;
-  T.col(1) = Eigen::Quaterniond(Eigen::AngleAxisd(DART_PI_HALF, _n)) * tangent;
+  T.col(1) = Eigen::Quaterniond(Eigen::AngleAxisd(0.5_pi, _n)) * tangent;
   return T;
 }
 

dart/constraint/SoftContactConstraint.cpp

@@ -39,7 +39,6 @@
 #include <iostream>
 
 #include "dart/common/Console.h"
-#include "dart/math/Helpers.h"
 #include "dart/dynamics/BodyNode.h"
 #include "dart/dynamics/PointMass.h"
 #include "dart/dynamics/SoftBodyNode.h"
@@ -48,6 +47,7 @@
 #include "dart/collision/CollisionObject.h"
 #include "dart/lcpsolver/lcp.h"
 
+#define DART_EPSILON 1e-6
 #define DART_ERROR_ALLOWANCE 0.0
 #define DART_ERP     0.01
 #define DART_MAX_ERV 1e+1
@@ -977,6 +977,8 @@ void SoftContactConstraint::updateFirstFrictionalDirection()
 Eigen::MatrixXd SoftContactConstraint::getTangentBasisMatrixODE(
     const Eigen::Vector3d& _n)
 {
+  using namespace math::suffixes;
+
   // TODO(JS): Use mNumFrictionConeBases
   // Check if the number of bases is even number.
 //  bool isEvenNumBases = mNumFrictionConeBases % 2 ? true : false;
@@ -1000,7 +1002,7 @@ Eigen::MatrixXd SoftContactConstraint::getTangentBasisMatrixODE(
   // Each basis and its opposite belong in the matrix, so we iterate half as
   // many times
   T.col(0) = tangent;
-  T.col(1) = Eigen::Quaterniond(Eigen::AngleAxisd(DART_PI_HALF, _n)) * tangent;
+  T.col(1) = Eigen::Quaterniond(Eigen::AngleAxisd(0.5_pi, _n)) * tangent;
   return T;
 }
 

dart/dynamics/CylinderShape.cpp

@@ -34,9 +34,11 @@
  *   POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <cmath>
 #include "dart/dynamics/CylinderShape.h"
 
+#include <cmath>
+#include "dart/math/Helpers.h"
+
 namespace dart {
 namespace dynamics {
 
@@ -75,7 +77,7 @@ void CylinderShape::setHeight(double _height) {
 //==============================================================================
 double CylinderShape::computeVolume(double radius, double height)
 {
-  return DART_PI * std::pow(radius, 2) * height;
+  return math::constantsd::pi() * std::pow(radius, 2) * height;
 }
 
 //==============================================================================

dart/dynamics/EllipsoidShape.cpp

@@ -37,6 +37,8 @@
 
 #include "dart/dynamics/EllipsoidShape.h"
 
+#include "dart/math/Helpers.h"
+
 namespace dart {
 namespace dynamics {
 
@@ -66,7 +68,7 @@ const Eigen::Vector3d&EllipsoidShape::getSize() const {
 double EllipsoidShape::computeVolume(const Eigen::Vector3d& size)
 {
   // 4/3* Pi* a/2* b/2* c/2
-  return DART_PI * size[0] * size[1] * size[2] / 6.0;
+  return math::constantsd::pi() * size[0] * size[1] * size[2] / 6.0;
 }
 
 //==============================================================================

dart/dynamics/EulerJoint.cpp

@@ -39,7 +39,6 @@
 #include <string>
 
 #include "dart/common/Console.h"
-#include "dart/math/Helpers.h"
 #include "dart/math/Geometry.h"
 #include "dart/dynamics/DegreeOfFreedom.h"
 
@@ -210,7 +209,7 @@ Eigen::Matrix<double, 6, 3> EulerJoint::getLocalJacobianStatic(
       J2 <<   0.0,      0.0, 1.0, 0.0, 0.0, 0.0;
 
 #ifndef NDEBUG
-      if (std::abs(getPositionsStatic()[1]) == DART_PI * 0.5)
+      if (std::abs(getPositionsStatic()[1]) == math::constantsd::pi() * 0.5)
         std::cout << "Singular configuration in ZYX-euler joint ["
                   << mJointP.mName << "]. ("
                   << _positions[0] << ", "
@@ -236,7 +235,7 @@ Eigen::Matrix<double, 6, 3> EulerJoint::getLocalJacobianStatic(
       J2 << 1.0,   0.0,   0.0, 0.0, 0.0, 0.0;
 
 #ifndef NDEBUG
-      if (std::abs(_positions[1]) == DART_PI * 0.5)
+      if (std::abs(_positions[1]) == math::constantsd::pi() * 0.5)
         std::cout << "Singular configuration in ZYX-euler joint ["
                   << mJointP.mName << "]. ("
                   << _positions[0] << ", "

dart/dynamics/PointMass.h

@@ -39,6 +39,7 @@
 
 #include <vector>
 #include <Eigen/Dense>
+#include "dart/math/Helpers.h"
 #include "dart/dynamics/Entity.h"
 
 namespace dart {
@@ -94,21 +95,21 @@ class PointMass : public common::Subject
                double _mass = 0.0005,
                const std::vector<size_t>& _connections = std::vector<size_t>(),
                const Eigen::Vector3d& _positionLowerLimits =
-                                      Eigen::Vector3d::Constant(-DART_DBL_INF),
+                                      Eigen::Vector3d::Constant(-math::constantsd::inf()),
                const Eigen::Vector3d& _positionUpperLimits =
-                                      Eigen::Vector3d::Constant( DART_DBL_INF),
+                                      Eigen::Vector3d::Constant( math::constantsd::inf()),
                const Eigen::Vector3d& _velocityLowerLimits =
-                                      Eigen::Vector3d::Constant(-DART_DBL_INF),
+                                      Eigen::Vector3d::Constant(-math::constantsd::inf()),
                const Eigen::Vector3d& _velocityUpperLimits =
-                                      Eigen::Vector3d::Constant( DART_DBL_INF),
+                                      Eigen::Vector3d::Constant( math::constantsd::inf()),
                const Eigen::Vector3d& _accelerationLowerLimits =
-                                      Eigen::Vector3d::Constant(-DART_DBL_INF),
+                                      Eigen::Vector3d::Constant(-math::constantsd::inf()),
                const Eigen::Vector3d& _accelerationUpperLimits =
-                                      Eigen::Vector3d::Constant( DART_DBL_INF),
+                                      Eigen::Vector3d::Constant( math::constantsd::inf()),
                const Eigen::Vector3d& _forceLowerLimits =
-                                      Eigen::Vector3d::Constant(-DART_DBL_INF),
+                                      Eigen::Vector3d::Constant(-math::constantsd::inf()),
                const Eigen::Vector3d& _forceUpperLimits =
-                                      Eigen::Vector3d::Constant( DART_DBL_INF));
+                                      Eigen::Vector3d::Constant( math::constantsd::inf()));
 
     void setRestingPosition(const Eigen::Vector3d& _x);
 

dart/dynamics/ScrewJoint.cpp

@@ -39,7 +39,6 @@
 #include <string>
 
 #include "dart/math/Geometry.h"
-#include "dart/math/Helpers.h"
 #include "dart/dynamics/BodyNode.h"
 
 namespace dart {
@@ -161,9 +160,11 @@ Joint* ScrewJoint::clone() const
 //==============================================================================
 void ScrewJoint::updateLocalTransform() const
 {
+  using namespace dart::math::suffixes;
+
   Eigen::Vector6d S = Eigen::Vector6d::Zero();
   S.head<3>() = getAxis();
-  S.tail<3>() = getAxis()*getPitch()/DART_2PI;
+  S.tail<3>() = getAxis()*getPitch()*0.5_pi;
   mT = mJointP.mT_ParentBodyToJoint
        * math::expMap(S * getPositionStatic())
        * mJointP.mT_ChildBodyToJoint.inverse();
@@ -173,11 +174,13 @@ void ScrewJoint::updateLocalTransform() const
 //==============================================================================
 void ScrewJoint::updateLocalJacobian(bool _mandatory) const
 {
+  using namespace dart::math::suffixes;
+
   if(_mandatory)
   {
     Eigen::Vector6d S = Eigen::Vector6d::Zero();
     S.head<3>() = getAxis();
-    S.tail<3>() = getAxis()*getPitch()/DART_2PI;
+    S.tail<3>() = getAxis()*getPitch()*0.5_pi;
     mJacobian = math::AdT(mJointP.mT_ChildBodyToJoint, S);
     assert(!math::isNan(mJacobian));
   }

dart/dynamics/SoftBodyNode.cpp

@@ -2453,6 +2453,8 @@ SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeEllipsoidProperties(
     double _edgeStiffness,
     double _dampingCoeff)
 {
+  using namespace dart::math::suffixes;
+
   SoftBodyNode::UniqueProperties properties(_vertexStiffness,
                                             _edgeStiffness,
                                             _dampingCoeff);
@@ -2472,8 +2474,8 @@ SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeEllipsoidProperties(
         PointMass::Properties(Eigen::Vector3d(0.0, 0.0, 0.5 * _size(2)), mass));
 
   // middle
-  float drho = (DART_PI / _nStacks);
-  float dtheta = (DART_2PI / _nSlices);
+  float drho = 1_pi / _nStacks;
+  float dtheta = 2_pi / _nSlices;
   for (size_t i = 1; i < _nStacks; i++)
   {
     float rho = i * drho;
@@ -2634,6 +2636,8 @@ SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeCylinderProperties(
     double _edgeStiffness,
     double _dampingCoeff)
 {
+  using namespace dart::math::suffixes;
+
   SoftBodyNode::UniqueProperties properties(_vertexStiffness,
                                             _edgeStiffness,
                                             _dampingCoeff);
@@ -2651,7 +2655,7 @@ SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeCylinderProperties(
 
   // Resting positions for each point mass
   float dradius = _radius / static_cast<float>(_nRings);
-  float dtheta = DART_2PI / static_cast<float>(_nSlices);
+  float dtheta = 2_pi / static_cast<float>(_nSlices);
 
   // -- top
   properties.addPointMass(PointMass::Properties(

dart/dynamics/detail/MultiDofJointProperties.h

@@ -37,6 +37,7 @@
 #ifndef DART_DYNAMICS_DETAIL_MULTIDOFJOINTPROPERTIES_H_
 #define DART_DYNAMICS_DETAIL_MULTIDOFJOINTPROPERTIES_H_
 
+#include "dart/math/Helpers.h"
 #include "dart/dynamics/Joint.h"
 #include "dart/common/AddonWithVersion.h"
 
@@ -107,14 +108,14 @@ struct MultiDofJointUniqueProperties
 
   /// Default constructor
   MultiDofJointUniqueProperties(
-      const Vector& _positionLowerLimits = Vector::Constant(-DART_DBL_INF),
-      const Vector& _positionUpperLimits = Vector::Constant( DART_DBL_INF),
-      const Vector& _velocityLowerLimits = Vector::Constant(-DART_DBL_INF),
-      const Vector& _velocityUpperLimits = Vector::Constant( DART_DBL_INF),
-      const Vector& _accelerationLowerLimits = Vector::Constant(-DART_DBL_INF),
-      const Vector& _accelerationUpperLimits = Vector::Constant( DART_DBL_INF),
-      const Vector& _forceLowerLimits = Vector::Constant(-DART_DBL_INF),
-      const Vector& _forceUpperLimits = Vector::Constant( DART_DBL_INF),
+      const Vector& _positionLowerLimits = Vector::Constant(-math::constantsd::inf()),
+      const Vector& _positionUpperLimits = Vector::Constant( math::constantsd::inf()),
+      const Vector& _velocityLowerLimits = Vector::Constant(-math::constantsd::inf()),
+      const Vector& _velocityUpperLimits = Vector::Constant( math::constantsd::inf()),
+      const Vector& _accelerationLowerLimits = Vector::Constant(-math::constantsd::inf()),
+      const Vector& _accelerationUpperLimits = Vector::Constant( math::constantsd::inf()),
+      const Vector& _forceLowerLimits = Vector::Constant(-math::constantsd::inf()),
+      const Vector& _forceUpperLimits = Vector::Constant( math::constantsd::inf()),
       const Vector& _springStiffness = Vector::Constant(0.0),
       const Vector& _restPosition = Vector::Constant(0.0),
       const Vector& _dampingCoefficient = Vector::Constant(0.0),