dartpy 7.0.0.dev0__cp313-cp313-manylinux_2_39_x86_64.whl

include/dart/math/Helpers.hpp

@@ -0,0 +1,481 @@
+/*
+ * 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_HELPERS_HPP_
+#define DART_MATH_HELPERS_HPP_
+
+// Standard Libraries
+#include <iomanip>
+#include <iostream>
+#include <random>
+
+#include <cfloat>
+#include <climits>
+#include <cmath>
+#include <cstdlib>
+#include <ctime>
+
+// External Libraries
+#include <Eigen/Dense>
+// Local Headers
+#include <dart/math/Constants.hpp>
+#include <dart/math/MathTypes.hpp>
+#include <dart/math/Random.hpp>
+
+namespace dart {
+namespace math {
+
+//==============================================================================
+template <typename T>
+constexpr T toRadian(const T& degree)
+{
+  return degree * constants<T>::pi() / 180.0;
+}
+
+//==============================================================================
+template <typename T>
+constexpr T toDegree(const T& radian)
+{
+  return radian * 180.0 / constants<T>::pi();
+}
+
+/// \brief a cross b = (CR*a) dot b
+/// const Matd CR(2,2,0.0,-1.0,1.0,0.0);
+const Eigen::Matrix2d CR((Eigen::Matrix2d() << 0.0, -1.0, 1.0, 0.0).finished());
+
+inline int delta(int _i, int _j)
+{
+  if (_i == _j)
+    return 1;
+  return 0;
+}
+
+template <typename T>
+inline constexpr int sign(T x, std::false_type)
+{
+  return static_cast<T>(0) < x;
+}
+
+template <typename T>
+inline constexpr int sign(T x, std::true_type)
+{
+  return (static_cast<T>(0) < x) - (x < static_cast<T>(0));
+}
+
+template <typename T>
+inline constexpr int sign(T x)
+{
+  return sign(x, std::is_signed<T>());
+}
+
+inline double sqr(double _x)
+{
+  return _x * _x;
+}
+
+inline double Tsinc(double _theta)
+{
+  return 0.5 - sqrt(_theta) / 48;
+}
+
+inline bool isZero(double _theta)
+{
+  return (std::abs(_theta) < 1e-6);
+}
+
+inline double asinh(double _X)
+{
+  return log(_X + sqrt(_X * _X + 1));
+}
+
+inline double acosh(double _X)
+{
+  return log(_X + sqrt(_X * _X - 1));
+}
+
+inline double atanh(double _X)
+{
+  return log((1 + _X) / (1 - _X)) / 2;
+}
+
+inline double asech(double _X)
+{
+  return log((sqrt(-_X * _X + 1) + 1) / _X);
+}
+
+inline double acosech(double _X)
+{
+  return log((sign(_X) * sqrt(_X * _X + 1) + 1) / _X);
+}
+
+inline double acotanh(double _X)
+{
+  return log((_X + 1) / (_X - 1)) / 2;
+}
+
+inline double round2(double _x)
+{
+  int gintx = static_cast<int>(std::floor(_x));
+  if (_x - gintx < 0.5)
+    return static_cast<double>(gintx);
+  else
+    return static_cast<double>(gintx + 1.0);
+}
+
+template <typename T>
+inline T clip(const T& val, const T& lower, const T& upper)
+{
+  return std::max(lower, std::min(val, upper));
+}
+
+template <typename DerivedA, typename DerivedB>
+inline typename DerivedA::PlainObject clip(
+    const Eigen::MatrixBase<DerivedA>& val,
+    const Eigen::MatrixBase<DerivedB>& lower,
+    const Eigen::MatrixBase<DerivedB>& upper)
+{
+  return lower.cwiseMax(val.cwiseMin(upper));
+}
+
+inline bool isEqual(double _x, double _y)
+{
+  return (std::abs(_x - _y) < 1e-6);
+}
+
+// check if it is an integer
+inline bool isInt(double _x)
+{
+  if (isEqual(round(_x), _x))
+    return true;
+  return false;
+}
+
+/// \brief Returns whether _v is a NaN (Not-A-Number) value
+inline bool isNan(double _v)
+{
+#ifdef _WIN32
+  return _isnan(_v) != 0;
+#else
+  return std::isnan(_v);
+#endif
+}
+
+/// \brief Returns whether _m is a NaN (Not-A-Number) matrix
+inline bool isNan(const Eigen::MatrixXd& _m)
+{
+  for (int i = 0; i < _m.rows(); ++i)
+    for (int j = 0; j < _m.cols(); ++j)
+      if (isNan(_m(i, j)))
+        return true;
+
+  return false;
+}
+
+/// \brief Returns whether _v is an infinity value (either positive infinity or
+/// negative infinity).
+inline bool isInf(double _v)
+{
+#ifdef _WIN32
+  return !_finite(_v);
+#else
+  return std::isinf(_v);
+#endif
+}
+
+/// \brief Returns whether _m is an infinity matrix (either positive infinity or
+/// negative infinity).
+inline bool isInf(const Eigen::MatrixXd& _m)
+{
+  for (int i = 0; i < _m.rows(); ++i)
+    for (int j = 0; j < _m.cols(); ++j)
+      if (isInf(_m(i, j)))
+        return true;
+
+  return false;
+}
+
+/// \brief Returns whether _m is symmetric or not
+inline bool isSymmetric(const Eigen::MatrixXd& _m, double _tol = 1e-6)
+{
+  std::size_t rows = _m.rows();
+  std::size_t cols = _m.cols();
+
+  if (rows != cols)
+    return false;
+
+  for (std::size_t i = 0; i < rows; ++i) {
+    for (std::size_t j = i + 1; j < cols; ++j) {
+      if (std::abs(_m(i, j) - _m(j, i)) > _tol) {
+        std::cout << "A: " << std::endl;
+        for (std::size_t k = 0; k < rows; ++k) {
+          for (std::size_t l = 0; l < cols; ++l)
+            std::cout << std::setprecision(4) << _m(k, l) << " ";
+          std::cout << std::endl;
+        }
+
+        std::cout << "A(" << i << ", " << j << "): " << _m(i, j) << std::endl;
+        std::cout << "A(" << j << ", " << i << "): " << _m(i, j) << std::endl;
+        return false;
+      }
+    }
+  }
+
+  return true;
+}
+
+inline unsigned seedRand()
+{
+  time_t now = time(0);
+  unsigned char* p = reinterpret_cast<unsigned char*>(&now);
+  unsigned seed = 0;
+  std::size_t i;
+
+  for (i = 0; i < sizeof(now); i++)
+    seed = seed * (UCHAR_MAX + 2U) + p[i];
+
+  srand(seed);
+  return seed;
+}
+
+/// \deprecated Please use Random::uniform() instead.
+DART_DEPRECATED(6.7)
+inline double random(double _min, double _max)
+{
+  return _min
+         + ((static_cast<double>(rand()) / (RAND_MAX + 1.0)) * (_max - _min));
+}
+
+/// \deprecated Please use Random::uniform() instead.
+template <int N>
+DART_DEPRECATED(6.7)
+Eigen::Matrix<double, N, 1> randomVector(double _min, double _max)
+{
+  Eigen::Matrix<double, N, 1> v;
+  DART_SUPPRESS_DEPRECATED_BEGIN
+  for (std::size_t i = 0; i < N; ++i)
+    v[i] = random(_min, _max);
+  DART_SUPPRESS_DEPRECATED_END
+
+  return v;
+}
+
+/// \deprecated Please use Random::uniform() instead.
+template <int N>
+DART_DEPRECATED(6.7)
+Eigen::Matrix<double, N, 1> randomVector(double _limit)
+{
+  DART_SUPPRESS_DEPRECATED_BEGIN
+  return randomVector<N>(-std::abs(_limit), std::abs(_limit));
+  DART_SUPPRESS_DEPRECATED_END
+}
+
+//==============================================================================
+/// \deprecated Please use Random::uniform() instead.
+DART_DEPRECATED(6.7)
+inline Eigen::VectorXd randomVectorXd(std::size_t size, double min, double max)
+{
+  Eigen::VectorXd v = Eigen::VectorXd::Zero(size);
+
+  DART_SUPPRESS_DEPRECATED_BEGIN
+  for (std::size_t i = 0; i < size; ++i)
+    v[i] = random(min, max);
+  DART_SUPPRESS_DEPRECATED_END
+
+  return v;
+}
+
+//==============================================================================
+/// \deprecated Please use Random::uniform() instead.
+DART_DEPRECATED(6.7)
+inline Eigen::VectorXd randomVectorXd(std::size_t size, double limit)
+{
+  DART_SUPPRESS_DEPRECATED_BEGIN
+  return randomVectorXd(size, -std::abs(limit), std::abs(limit));
+  DART_SUPPRESS_DEPRECATED_END
+}
+
+namespace suffixes {
+
+//==============================================================================
+constexpr double operator"" _pi(long double x)
+{
+  return x * constants<double>::pi();
+}
+
+//==============================================================================
+constexpr double operator"" _pi(unsigned long long int x)
+{
+  return operator"" _pi(static_cast<long double>(x));
+}
+
+//==============================================================================
+constexpr double operator"" _rad(long double angle)
+{
+  return angle;
+}
+
+//==============================================================================
+constexpr double operator"" _rad(unsigned long long int angle)
+{
+  return operator"" _rad(static_cast<long double>(angle));
+}
+
+//==============================================================================
+constexpr double operator"" _deg(long double angle)
+{
+  return toRadian(angle);
+}
+
+//==============================================================================
+constexpr double operator"" _deg(unsigned long long int angle)
+{
+  return operator"" _deg(static_cast<long double>(angle));
+}
+
+} // namespace suffixes
+
+} // namespace math
+
+namespace Color {
+
+inline Eigen::Vector4d Red(double alpha)
+{
+  return Eigen::Vector4d(0.9, 0.1, 0.1, alpha);
+}
+
+inline Eigen::Vector3d Red()
+{
+  return Eigen::Vector3d(0.9, 0.1, 0.1);
+}
+
+inline Eigen::Vector3d Fuchsia()
+{
+  return Eigen::Vector3d(1.0, 0.0, 0.5);
+}
+
+inline Eigen::Vector4d Fuchsia(double alpha)
+{
+  return Eigen::Vector4d(1.0, 0.0, 0.5, alpha);
+}
+
+inline Eigen::Vector4d Orange(double alpha)
+{
+  return Eigen::Vector4d(1.0, 0.63, 0.0, alpha);
+}
+
+inline Eigen::Vector3d Orange()
+{
+  return Eigen::Vector3d(1.0, 0.63, 0.0);
+}
+
+inline Eigen::Vector4d Green(double alpha)
+{
+  return Eigen::Vector4d(0.1, 0.9, 0.1, alpha);
+}
+
+inline Eigen::Vector3d Green()
+{
+  return Eigen::Vector3d(0.1, 0.9, 0.1);
+}
+
+inline Eigen::Vector4d Blue(double alpha)
+{
+  return Eigen::Vector4d(0.1, 0.1, 0.9, alpha);
+}
+
+inline Eigen::Vector3d Blue()
+{
+  return Eigen::Vector3d(0.1, 0.1, 0.9);
+}
+
+inline Eigen::Vector4d White(double alpha)
+{
+  return Eigen::Vector4d(1.0, 1.0, 1.0, alpha);
+}
+
+inline Eigen::Vector3d White()
+{
+  return Eigen::Vector3d(1.0, 1.0, 1.0);
+}
+
+inline Eigen::Vector4d Black(double alpha)
+{
+  return Eigen::Vector4d(0.05, 0.05, 0.05, alpha);
+}
+
+inline Eigen::Vector3d Black()
+{
+  return Eigen::Vector3d(0.05, 0.05, 0.05);
+}
+
+inline Eigen::Vector4d LightGray(double alpha)
+{
+  return Eigen::Vector4d(0.9, 0.9, 0.9, alpha);
+}
+
+inline Eigen::Vector3d LightGray()
+{
+  return Eigen::Vector3d(0.9, 0.9, 0.9);
+}
+
+inline Eigen::Vector4d Gray(double alpha)
+{
+  return Eigen::Vector4d(0.6, 0.6, 0.6, alpha);
+}
+
+inline Eigen::Vector3d Gray()
+{
+  return Eigen::Vector3d(0.6, 0.6, 0.6);
+}
+
+inline Eigen::Vector4d Random(double alpha)
+{
+  return Eigen::Vector4d(
+      math::Random::uniform(0.0, 1.0),
+      math::Random::uniform(0.0, 1.0),
+      math::Random::uniform(0.0, 1.0),
+      alpha);
+}
+
+inline Eigen::Vector3d Random()
+{
+  return Eigen::Vector3d(
+      math::Random::uniform(0.0, 1.0),
+      math::Random::uniform(0.0, 1.0),
+      math::Random::uniform(0.0, 1.0));
+}
+
+} // namespace Color
+
+} // namespace dart
+
+#endif // DART_MATH_HELPERS_HPP_

include/dart/math/Icosphere.hpp

@@ -0,0 +1,106 @@
+/*
+ * 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_ICOSPHERE_HPP_
+#define DART_MATH_ICOSPHERE_HPP_
+
+#include <dart/math/TriMesh.hpp>
+
+#include <Eigen/Core>
+
+#include <map>
+#include <vector>
+
+namespace dart {
+namespace math {
+
+/// The class Icosphere represents an icosphere where the subdivision and radius
+/// are configurable.
+template <typename S_>
+class Icosphere : public TriMesh<S_>
+{
+public:
+  // Type aliases
+  using S = S_;
+  using Base = TriMesh<S>;
+  using Index = typename Base::Index;
+  using Vector3 = typename Base::Vector3;
+  using Triangle = typename Base::Triangle;
+  using Vertices = std::vector<Vector3>;
+  using Normals = typename Base::Normals;
+  using Triangles = std::vector<Triangle>;
+
+  /// Returns the number of vertices of icosphere given subdivisions.
+  static std::size_t getNumVertices(std::size_t subdivisions);
+
+  /// Returns the number of edges of icosphere given subdivisions.
+  static std::size_t getNumEdges(std::size_t subdivisions);
+
+  /// Returns the number of triangles of icosphere given subdivisions.
+  static std::size_t getNumTriangles(std::size_t subdivisions);
+
+  /// Returns vertices and faces of icosahedron given radius.
+  static std::pair<Vertices, Triangles> computeIcosahedron(S radius);
+
+  /// Construct an icosphere given radius and subdivisions.
+  ///
+  /// \param[in] radius: The radius of the icosphere.
+  /// \param[in] subdivisions: The number of subdividing an icosahedron. Passing
+  /// 1 generates icosahedron without subdividing.
+  Icosphere(S radius, std::size_t subdivisions);
+
+  /// Returns the radius of the icosphere.
+  S getRadius() const;
+
+  /// Returns the number of subdivisions of the icosphere.
+  std::size_t getNumSubdivisions() const;
+
+private:
+  /// Internal function to build icosphere given radius and subdivisions.
+  void build();
+
+  /// Radius of icosphere.
+  S mRadius;
+
+  /// Number of subdividing an icosahedron.
+  std::size_t mSubdivisions;
+};
+
+using Icospheref = Icosphere<float>;
+using Icosphered = Icosphere<double>;
+
+} // namespace math
+} // namespace dart
+
+#include <dart/math/detail/Icosphere-impl.hpp>
+
+#endif // DART_MATH_ICOSPHERE_HPP_

include/dart/math/MathTypes.hpp

@@ -0,0 +1,100 @@
+/*
+ * 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_MATHTYPES_HPP_
+#define DART_MATH_MATHTYPES_HPP_
+
+#include <dart/common/Deprecated.hpp>
+#include <dart/common/Memory.hpp>
+
+#include <Eigen/Dense>
+
+#include <map>
+#include <vector>
+
+//------------------------------------------------------------------------------
+// Types
+//------------------------------------------------------------------------------
+namespace Eigen {
+
+using Vector6d = Matrix<double, 6, 1>;
+using Matrix6d = Matrix<double, 6, 6>;
+
+inline Vector6d compose(
+    const Eigen::Vector3d& _angular, const Eigen::Vector3d& _linear)
+{
+  Vector6d composition;
+  composition << _angular, _linear;
+  return composition;
+}
+
+// Deprecated
+using EIGEN_V_VEC3D = std::vector<Eigen::Vector3d>;
+
+// Deprecated
+using EIGEN_VV_VEC3D = std::vector<std::vector<Eigen::Vector3d>>;
+
+// Deprecated in favor of dart::common::aligned_vector
+template <typename _Tp>
+using aligned_vector = std::vector<_Tp, Eigen::aligned_allocator<_Tp>>;
+
+// Deprecated in favor of dart::common::aligned_map
+template <typename _Key, typename _Tp, typename _Compare = std::less<_Key>>
+using aligned_map = std::map<
+    _Key,
+    _Tp,
+    _Compare,
+    Eigen::aligned_allocator<std::pair<const _Key, _Tp>>>;
+
+// Deprecated in favor of dart::common::make_aligned_shared
+template <typename _Tp, typename... _Args>
+DART_DEPRECATED(6.2)
+std::shared_ptr<_Tp> make_aligned_shared(_Args&&... __args)
+{
+  return ::dart::common::make_aligned_shared<_Tp, _Args...>(
+      std::forward<_Args>(__args)...);
+}
+
+} // namespace Eigen
+
+namespace dart {
+namespace math {
+
+using Inertia = Eigen::Matrix6d;
+using LinearJacobian = Eigen::Matrix<double, 3, Eigen::Dynamic>;
+using AngularJacobian = Eigen::Matrix<double, 3, Eigen::Dynamic>;
+using Jacobian = Eigen::Matrix<double, 6, Eigen::Dynamic>;
+
+} // namespace math
+} // namespace dart
+
+#endif // DART_MATH_MATHTYPES_HPP_