dartpy 7.0.0.dev0__cp313-cp313-manylinux_2_39_x86_64.whl

include/dart/math/Mesh.hpp

@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_MATH_MESH_HPP_
+#define DART_MATH_MESH_HPP_
+
+#include <Eigen/Core>
+
+#include <vector>
+
+namespace dart {
+namespace math {
+
+/// Base class for meshes.
+template <typename S_>
+class Mesh
+{
+public:
+  // Type aliases
+  using S = S_;
+  using Index = std::size_t;
+  using Vector3 = Eigen::Matrix<S, 3, 1>;
+  using Vertices = std::vector<Vector3>;
+  using Normals = std::vector<Vector3>;
+  using Indices = std::vector<Index>;
+
+  /// Destructor.
+  virtual ~Mesh();
+
+  /// Returns true if the mesh contains vertices.
+  bool hasVertices() const;
+
+  /// Returns true if the mesh contains vertex normals.
+  bool hasVertexNormals() const;
+
+  /// Returns the vertices of the mesh.
+  const Vertices& getVertices() const;
+
+  /// Returns the vertex normals of the mesh.
+  const Normals& getVertexNormals() const;
+
+  /// Clears all the vertices and vertex normals.
+  virtual void clear();
+
+  /// Returns true if the mesh has no vertices.
+  bool isEmpty() const;
+
+  /// Translates the mesh vertices by adding \c translation to the vertices.
+  void translate(const Vector3& translation);
+
+  /// Addition operator.
+  Mesh operator+(const Mesh& other) const;
+
+  /// Addition assignment operator.
+  Mesh& operator+=(const Mesh& other);
+
+protected:
+  /// Default constructor.
+  Mesh();
+
+  /// Normalizes the vertex normals.
+  void normalizeVertexNormals();
+
+  /// Vertices of the mesh.
+  Vertices mVertices;
+
+  /// Vertex normals of the mesh.
+  Normals mVertexNormals;
+};
+
+using Meshf = Mesh<float>;
+using Meshd = Mesh<double>;
+
+} // namespace math
+} // namespace dart
+
+#include <dart/math/detail/Mesh-impl.hpp>
+
+#endif // DART_MATH_MESH_HPP_

include/dart/math/Random.hpp

@@ -0,0 +1,225 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_MATH_RANDOM_HPP_
+#define DART_MATH_RANDOM_HPP_
+
+#include <Eigen/Core>
+
+#include <random>
+
+namespace dart {
+namespace math {
+
+class Random final
+{
+public:
+  using GeneratorType = std::mt19937;
+
+  template <typename FloatType>
+  using UniformRealDist = std::uniform_real_distribution<FloatType>;
+
+  template <typename IntType>
+  using UniformIntDist = std::uniform_int_distribution<IntType>;
+
+  template <typename FloatType>
+  using NormalRealDist = std::normal_distribution<FloatType>;
+
+  /// Returns a mutable reference to the random generator
+  static GeneratorType& getGenerator();
+
+  /// Sets the seed value.
+  ///
+  /// The same seed gives the same sequence of random values so that you can
+  /// regenerate the same sequencial random values as long as you knot the seed
+  /// value.
+  static void setSeed(unsigned int seed);
+
+  /// Generates a seed value using the default random device.
+  ///
+  /// \param[in] applyGeneratedSeed Whether to apply the generated seed.
+  /// \return The new seed value.
+  static unsigned int generateSeed(bool applyGeneratedSeed = false);
+
+  /// \return The current seed value.
+  static unsigned int getSeed();
+
+  /// Returns a random number from an uniform distribution.
+  ///
+  ///
+  /// This template function can generate different scalar types of random
+  /// numbers as:
+  /// - Floating-point number: \c float, \c double, \c long double
+  /// - Integer number: [\c unsigned] \c short, [\c unsigned] \c int,
+  ///   [\c unsigned] \c long, [\c unsigned] \c long \c long
+  ///
+  /// and vectors and matrices as:
+  /// - Fixed-size: Eigen::Vector3i, Eigen::Vector3d, Eigen::Matrix4d, and so
+  ///   on.
+  /// - Dynamic-size: Eigen::VectorXi, Eigen::VectorXd, Eigen::MatrixXd, and so
+  ///   on.
+  ///
+  /// Example:
+  /// \code
+  /// // Generate a random int in [0, 10]
+  /// int intVal1 = Random::uniform(0, 10);
+  /// int intVal2 = Random::uniform<int>(0, 10);
+  ///
+  /// // Generate a random double in [0.0, 10.0)
+  /// double dblVal1 = Random::uniform(0.0, 10.0);
+  /// double dblVal2 = Random::uniform<double>(0, 10);
+  ///
+  /// // Generate a random vector in [lb, ub)
+  /// Eigen::Vector3d lb = Eigen::Vector3d::Constant(1);
+  /// Eigen::Vector3d ub = Eigen::Vector3d::Constant(4);
+  /// Eigen::Vector3d vecVal1 = Random::uniform(lb, ub);
+  /// Eigen::Vector3d vecVal2 = Random::uniform<Eigen::Vector3d>(lb, ub);
+  ///
+  /// // Generate a random matrix in [lb, ub)
+  /// Eigen::Matrix4f lb = Eigen::Matrix4f::Constant(1);
+  /// Eigen::Matrix4f ub = Eigen::Matrix4f::Constant(4);
+  /// Eigen::Matrix4f vecVal1 = Random::uniform(lb, ub);
+  /// Eigen::Matrix4f vecVal2 = Random::uniform<Eigen::Matrix4f>(lb, ub);
+  /// \endcode
+  ///
+  /// Note that the end of the range is closed for integer types (i.e.,
+  /// [int_min, int_max]), but open for floating-point types (i.e., [float_min,
+  /// float_max)).
+  ///
+  /// \tparam S The type of random value.
+  /// \param[in] min Lower bound of the distribution.
+  /// \param[in] max Upper bound of the distribution.
+  ///
+  /// \sa normal()
+  template <typename S>
+  static S uniform(S min, S max);
+
+  /// Returns a random vector or matrix from an uniform distribution.
+  ///
+  /// This is a helper function for the case that the each of lower and upper
+  /// bound has an uniform element value in it. For example, the lower bound is
+  /// [1, 1, 1] or [-2, -2].
+  ///
+  /// This variant is meant to be used for fixed-size vector or matrix types.
+  /// For dynamic-size types, please use other variants that takes the size of
+  /// vector or matrix.
+  ///
+  /// Example:
+  /// \code
+  /// // Generate random vectors
+  /// Eigen::VectorXi vecXi = Random::uniform<Eigen::VectorXi>(0, 10);
+  /// Eigen::VectorXd vecXd = Random::uniform<Eigen::VectorXd>(0.0, 10.0);
+  /// \endcode
+  ///
+  /// \tparam FixedSizeT The type of fixed-size vector or fixed-size matrix.
+  /// \param[in] min The constant value of the lower bound.
+  /// \param[in] max The constant value of the upper bound.
+  ///
+  /// \sa uniform()
+  template <typename FixedSizeT>
+  static FixedSizeT uniform(
+      typename FixedSizeT::Scalar min, typename FixedSizeT::Scalar max);
+
+  /// Returns a random vector from an uniform distribution.
+  ///
+  /// This variant is meant to be used for dynamic-size vector.
+  ///
+  /// Example:
+  /// \code
+  /// // Generate random matrices
+  /// Eigen::MatrixXi matXi = Random::uniform<Eigen::MatrixXi>(0, 10);
+  /// Eigen::MatrixXd matXd = Random::uniform<Eigen::MatrixXd>(0.0, 10.0);
+  /// \endcode
+  ///
+  /// \tparam DynamicSizeVectorT The type of dynamic-size vector.
+  /// \param[in] size The size of the vectors.
+  /// \param[in] min The constant value of the lower bound vector.
+  /// \param[in] max The constant value of the upper bound vector.
+  template <typename DynamicSizeVectorT>
+  static DynamicSizeVectorT uniform(
+      int size,
+      typename DynamicSizeVectorT::Scalar min,
+      typename DynamicSizeVectorT::Scalar max);
+
+  /// Returns a random matrix from an uniform distribution.
+  ///
+  /// This variant is meant to be used for dynamic-size matrix.
+  ///
+  /// \tparam DynamicSizeMatrixT The type of dynamic-size matrix.
+  /// \param[in] rows The row size of the matrices.
+  /// \param[in] cols The col size of the matrices.
+  /// \param[in] min The constant value of the lower bound matrix.
+  /// \param[in] max The constant value of the upper bound matrix.
+  ///
+  /// \sa uniform()
+  template <typename DynamicSizeMatrixT>
+  static DynamicSizeMatrixT uniform(
+      int rows,
+      int cols,
+      typename DynamicSizeMatrixT::Scalar min,
+      typename DynamicSizeMatrixT::Scalar max);
+
+  /// Returns a random number from a normal distribution.
+  ///
+  /// This template function can generate different scalar types of random
+  /// numbers as:
+  /// - Floating-point number: \c float, \c double, \c long double
+  /// - Integer number: [\c unsigned] \c short, [\c unsigned] \c int,
+  ///   [\c unsigned] \c long, [\c unsigned] \c long \c long
+  ///
+  /// Example:
+  /// \code
+  /// // Generate a random int
+  /// int intVal = Random::normal(0, 10);
+  ///
+  /// // Generate a random double
+  /// double dblVal = Random::normal(0.0, 10.0);
+  /// \endcode
+  ///
+  /// \param[in] mean Mean of the normal distribution.
+  /// \param[in] sigma Standard deviation of the distribution.
+  ///
+  /// \sa uniform()
+  template <typename S>
+  static S normal(S mean, S sigma);
+
+private:
+  /// \return A mutable reference to the seed.
+  static unsigned int& getSeedMutable();
+};
+
+} // namespace math
+} // namespace dart
+
+#include <dart/math/detail/Random-impl.hpp>
+
+#endif // DART_MATH_RANDOM_HPP_

include/dart/math/TriMesh.hpp

@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_MATH_TRIMESH_HPP_
+#define DART_MATH_TRIMESH_HPP_
+
+#include <dart/math/Mesh.hpp>
+
+#include <memory>
+
+namespace dart {
+namespace math {
+
+/// This class represents triangle meshes.
+template <typename S_>
+class TriMesh : public Mesh<S_>
+{
+public:
+  // Type aliases
+  using S = S_;
+  using Base = Mesh<S>;
+  using Index = typename Base::Index;
+  using Vector3 = typename Base::Vector3;
+  using Triangle = Eigen::Matrix<Index, 3, 1>;
+  using Vertices = typename Base::Vertices;
+  using Normals = typename Base::Normals;
+  using Triangles = std::vector<Triangle>;
+
+  /// Default constructor.
+  TriMesh();
+
+  /// Destructor
+  ~TriMesh() override = default;
+
+  /// Sets vertices and triangles.
+  void setTriangles(const Vertices& vertices, const Triangles& triangles);
+
+  /// Computes vertex normals.
+  void computeVertexNormals();
+
+  /// Returns true if the mesh contains triangles.
+  bool hasTriangles() const;
+
+  /// Returns true if the mesh contains triangle normals.
+  bool hasTriangleNormals() const;
+
+  /// Returns the triangles of the mesh.
+  const Triangles& getTriangles() const;
+
+  /// Returns the triangle normals of the mesh.
+  const Normals& getTriangleNormals() const;
+
+  /// Clears all the data in the trimesh.
+  void clear() override;
+
+  /// Addition operator.
+  TriMesh operator+(const TriMesh& other) const;
+
+  /// Addition assignment operator.
+  TriMesh& operator+=(const TriMesh& other);
+
+  /// Generates a convex hull that encloses the trimesh.
+  ///
+  /// \param[in] optimize: (Optional) Whether to discard vertices that are not
+  /// used in the convex hull.
+  std::shared_ptr<TriMesh<S>> generateConvexHull(bool optimize = true) const;
+
+protected:
+  /// Computes triangle normals.
+  void computeTriangleNormals();
+
+  /// Normalizes triangle normals.
+  void normalizeTriangleNormals();
+
+  /// Triangle indices of the mesh.
+  Triangles mTriangles;
+
+  /// Triangle normals of the mesh.
+  Normals mTriangleNormals;
+};
+
+extern template class TriMesh<double>;
+
+using TriMeshf = TriMesh<float>;
+using TriMeshd = TriMesh<double>;
+
+} // namespace math
+} // namespace dart
+
+#include <dart/math/detail/TriMesh-impl.hpp>
+
+#endif // DART_MATH_TRIMESH_HPP_

include/dart/math/all.hpp

@@ -0,0 +1,11 @@
+// Automatically generated file by cmake
+
+#include "dart/math/ConfigurationSpace.hpp"
+#include "dart/math/Constants.hpp"
+#include "dart/math/Geometry.hpp"
+#include "dart/math/Helpers.hpp"
+#include "dart/math/Icosphere.hpp"
+#include "dart/math/MathTypes.hpp"
+#include "dart/math/Mesh.hpp"
+#include "dart/math/Random.hpp"
+#include "dart/math/TriMesh.hpp"

include/dart/math/detail/ConfigurationSpace.hpp

@@ -0,0 +1,235 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_MATH_DETAIL_CONFIGURATIONSPACE_H_
+#define DART_MATH_DETAIL_CONFIGURATIONSPACE_H_
+
+#include <dart/math/ConfigurationSpace.hpp>
+#include <dart/math/Geometry.hpp>
+
+namespace dart {
+namespace math {
+
+namespace detail {
+
+template <bool>
+struct Range;
+
+//==============================================================================
+template <typename MatrixType, int Size, typename Enable = Range<true>>
+struct inverseImpl
+{
+  static void run(const MatrixType& matrix, MatrixType& result)
+  {
+    result = matrix.ldlt().solve(MatrixType::Identity());
+  }
+};
+
+//==============================================================================
+template <typename MatrixType, int Size>
+struct inverseImpl<MatrixType, Size, Range<(0 <= Size && Size <= 4)>>
+{
+  static void run(const MatrixType& matrix, MatrixType& result)
+  {
+    result = matrix.inverse();
+  }
+};
+
+//==============================================================================
+template <typename SpaceT>
+struct toEuclideanPointImpl
+{
+  static typename SpaceT::EuclideanPoint run(
+      const typename SpaceT::Point& point)
+  {
+    return point;
+  }
+};
+
+//==============================================================================
+template <>
+struct toEuclideanPointImpl<SO3Space>
+{
+  static typename SO3Space::EuclideanPoint run(
+      const typename SO3Space::Point& point)
+  {
+    return math::logMap(point);
+  }
+};
+
+//==============================================================================
+template <>
+struct toEuclideanPointImpl<SE3Space>
+{
+  static typename SE3Space::EuclideanPoint run(
+      const typename SE3Space::Point& point)
+  {
+    Eigen::Vector6d x;
+
+    x.head<3>() = math::logMap(point.linear());
+    x.tail<3>() = point.translation();
+
+    return x;
+  }
+};
+
+//==============================================================================
+template <typename SpaceT>
+struct toManifoldPointImpl
+{
+  static typename SpaceT::Point run(
+      const typename SpaceT::EuclideanPoint& point)
+  {
+    return point;
+  }
+};
+
+//==============================================================================
+template <>
+struct toManifoldPointImpl<SO3Space>
+{
+  static typename SO3Space::Point run(
+      const typename SO3Space::EuclideanPoint& point)
+  {
+    return math::expMapRot(point);
+  }
+};
+
+//==============================================================================
+template <>
+struct toManifoldPointImpl<SE3Space>
+{
+  static typename SE3Space::Point run(
+      const typename SE3Space::EuclideanPoint& point)
+  {
+    Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
+
+    tf.linear() = math::expMapRot(point.head<3>());
+    tf.translation() = point.tail<3>();
+
+    return tf;
+  }
+};
+
+//==============================================================================
+template <typename SpaceT>
+struct integratePositionImpl
+{
+  static typename SpaceT::Point run(
+      const typename SpaceT::Point& pos,
+      const typename SpaceT::Vector& vel,
+      double dt)
+  {
+    return pos + dt * vel;
+  }
+};
+
+//==============================================================================
+template <>
+struct integratePositionImpl<SO3Space>
+{
+  static typename SO3Space::Point run(
+      const typename SO3Space::Point& pos,
+      const typename SO3Space::Vector& vel,
+      double dt)
+  {
+    return pos * toManifoldPoint<SO3Space>(vel * dt);
+  }
+};
+
+//==============================================================================
+template <>
+struct integratePositionImpl<SE3Space>
+{
+  static typename SE3Space::Point run(
+      const typename SE3Space::Point& pos,
+      const typename SE3Space::Vector& vel,
+      double dt)
+  {
+    return pos * toManifoldPoint<SE3Space>(vel * dt);
+  }
+};
+
+} // namespace detail
+
+//==============================================================================
+template <typename SpaceT>
+typename SpaceT::Matrix inverse(const typename SpaceT::Matrix& mat)
+{
+  typename SpaceT::Matrix res;
+
+  detail::inverseImpl<typename SpaceT::Matrix, SpaceT::NumDofsEigen>::run(
+      mat, res);
+
+  return res;
+}
+
+//==============================================================================
+template <typename SpaceT>
+typename SpaceT::EuclideanPoint toEuclideanPoint(
+    const typename SpaceT::Point& point)
+{
+  return detail::toEuclideanPointImpl<SpaceT>::run(point);
+}
+
+//==============================================================================
+template <typename SpaceT>
+typename SpaceT::Point toManifoldPoint(
+    const typename SpaceT::EuclideanPoint& point)
+{
+  return detail::toManifoldPointImpl<SpaceT>::run(point);
+}
+
+//==============================================================================
+template <typename SpaceT>
+typename SpaceT::Point integratePosition(
+    const typename SpaceT::Point& pos,
+    const typename SpaceT::Vector& vel,
+    double dt)
+{
+  return detail::integratePositionImpl<SpaceT>::run(pos, vel, dt);
+}
+
+//==============================================================================
+template <typename SpaceT>
+typename SpaceT::Vector integrateVelocity(
+    const typename SpaceT::Vector& vel,
+    const typename SpaceT::Vector& acc,
+    double dt)
+{
+  return vel + acc * dt;
+}
+
+} // namespace math
+} // namespace dart
+
+#endif // DART_MATH_DETAIL_CONFIGURATIONSPACE_H_