diff --git a/CMakeLists.txt b/CMakeLists.txt index 77790118..aa208284 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,13 +72,14 @@ set(TIGHT_INCLUSION_WITH_NO_ZERO_TOI OFF CACHE BOOL "Enable refinement if CCD pr add_library(ipc_rigid src/autodiff/autodiff.cpp + src/utils/sinc.cpp src/ccd/impact.cpp src/ccd/ccd.cpp src/ccd/linear/broad_phase.cpp src/ccd/piecewise_linear/time_of_impact.cpp - src/interval/filib_rounding.cpp + src/interval/interval.cpp src/interval/interval_root_finder.cpp src/ccd/rigid/broad_phase.cpp src/ccd/rigid/rigid_body_hash_grid.cpp @@ -193,10 +194,6 @@ target_link_libraries(ipc_rigid PUBLIC spdlog::spdlog) include(finite_diff) target_link_libraries(ipc_rigid PUBLIC finitediff::finitediff) -# Boost -include(boost) -target_link_libraries(ipc_rigid PUBLIC Boost::boost) - # TBB include(onetbb) target_link_libraries(ipc_rigid PUBLIC TBB::tbb) diff --git a/cmake/recipes/boost.cmake b/cmake/recipes/boost.cmake deleted file mode 100644 index 317ec070..00000000 --- a/cmake/recipes/boost.cmake +++ /dev/null @@ -1,66 +0,0 @@ -# -# Copyright 2021 Adobe. All rights reserved. -# This file is licensed to you under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. You may obtain a copy -# of the License at http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software distributed under -# the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR REPRESENTATIONS -# OF ANY KIND, either express or implied. See the License for the specific language -# governing permissions and limitations under the License. -# -if(TARGET Boost::boost) - return() -endif() - -message(STATUS "Third-party: creating targets 'Boost::boost'") - -set(PREVIOUS_CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS}) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") -set(OLD_CMAKE_POSITION_INDEPENDENT_CODE ${CMAKE_POSITION_INDEPENDENT_CODE}) -set(CMAKE_POSITION_INDEPENDENT_CODE ON) - -set(BOOST_URL "https://archives.boost.io/release/1.86.0/source/boost_1_86_0.tar.bz2" CACHE STRING "Boost download URL") -set(BOOST_URL_SHA256 "1bed88e40401b2cb7a1f76d4bab499e352fa4d0c5f31c0dbae64e24d34d7513b" CACHE STRING "Boost download URL SHA256 checksum") - -include(CPM) -CPMAddPackage( - NAME boost - URL ${BOOST_URL} - URL_HASH SHA256=${BOOST_URL_SHA256} - DOWNLOAD_ONLY ON -) -set(BOOST_SOURCE ${boost_SOURCE_DIR}) -set(Boost_POPULATED ON) - -# Only build the following Boost libs -set(BOOST_LIBS_OPTIONAL "" CACHE STRING "Boost libs to be compiled" FORCE) - -# File lcid.cpp from Boost_locale.cpp doesn't compile on MSVC, so we exclude them from the default -# targets being built by the project (only targets explicitly used by other targets will be built). -CPMAddPackage( - NAME boost-cmake - GITHUB_REPOSITORY Orphis/boost-cmake - GIT_TAG 7f97a08b64bd5d2e53e932ddf80c40544cf45edf - EXCLUDE_FROM_ALL -) - -set(CMAKE_POSITION_INDEPENDENT_CODE ${OLD_CMAKE_POSITION_INDEPENDENT_CODE}) -set(CMAKE_CXX_FLAGS "${PREVIOUS_CMAKE_CXX_FLAGS}") - -foreach(name IN ITEMS - atomic - chrono - container - date_time - filesystem - iostreams - log - system - thread - timer - ) - if(TARGET Boost_${name}) - set_target_properties(Boost_${name} PROPERTIES FOLDER third_party/boost) - endif() -endforeach() diff --git a/src/SimState.cpp b/src/SimState.cpp index 8faed2ba..79c9c7aa 100644 --- a/src/SimState.cpp +++ b/src/SimState.cpp @@ -153,14 +153,7 @@ bool SimState::init(const nlohmann::json& args_in) "energy_conv_tol": null, "velocity_conv_tol": null, "is_velocity_conv_tol_abs": false, - "line_search_lower_bound": null, - "linear_solver": { - "name": "Eigen::SimplicialLDLT", - "max_iter": 1000, - "tolerance": 1e-10, - "pre_max_iter": 1, - "mtype": 2 - } + "line_search_lower_bound": null }, "ipc_solver": { "dhat_epsilon": 1e-9, diff --git a/src/ccd/rigid/rigid_body_bvh.hpp b/src/ccd/rigid/rigid_body_bvh.hpp index 7a78e084..8810ea72 100644 --- a/src/ccd/rigid/rigid_body_bvh.hpp +++ b/src/ccd/rigid/rigid_body_bvh.hpp @@ -30,7 +30,7 @@ inline AABB vertex_aabb(const VectorMax3I& v, double inflation_radius = 0) VectorMax3d min(v.size()); VectorMax3d max(v.size()); for (int i = 0; i < v.size(); i++) { - if (empty(v(i))) { + if (v(i).is_empty()) { throw "interval is empty"; } min(i) = (v(i) - inflation_radius).lower(); diff --git a/src/ccd/rigid/rigid_body_hash_grid.cpp b/src/ccd/rigid/rigid_body_hash_grid.cpp index 7ab07e6d..328fecd1 100644 --- a/src/ccd/rigid/rigid_body_hash_grid.cpp +++ b/src/ccd/rigid/rigid_body_hash_grid.cpp @@ -221,7 +221,7 @@ int RigidBodyHashGrid::compute_vertices_intervals( // If the vertices' intervals are outside the scene bbox, then split t in // hopes that a smaller interval will be more accurate. - std::pair t_halves = bisect(t); + std::pair t_halves = t.bisect(); MatrixXI V_first, V_second; int n_subs0 = compute_vertices_intervals( body, pose_t0, pose_t1, V_first, inflation_radius, t_halves.first, @@ -236,7 +236,7 @@ int RigidBodyHashGrid::compute_vertices_intervals( // Take the hull of the two halves of the vertices' trajectories for (int i = 0; i < vertices.rows(); i++) { for (int j = 0; j < vertices.cols(); j++) { - vertices(i, j) = hull(V_first(i, j), V_second(i, j)); + vertices(i, j) = V_first(i, j) | V_second(i, j); assert(vertices(i, j).lower() - inflation_radius >= m_domainMin(j)); assert(vertices(i, j).upper() + inflation_radius <= m_domainMax(j)); } @@ -253,7 +253,7 @@ intervals_to_AABB(const Eigen::MatrixBase& x, double inflation_radius) VectorMax3d min(x.size()); VectorMax3d max(x.size()); for (int i = 0; i < x.size(); i++) { - if (empty(x(i))) { + if (x(i).is_empty()) { throw "interval is empty"; } min(i) = x(i).lower() - inflation_radius; diff --git a/src/ccd/rigid/time_of_impact.cpp b/src/ccd/rigid/time_of_impact.cpp index 8b4d5572..4a63b470 100644 --- a/src/ccd/rigid/time_of_impact.cpp +++ b/src/ccd/rigid/time_of_impact.cpp @@ -276,7 +276,7 @@ bool compute_face_vertex_time_of_impact( const auto is_domain_valid = [&](const VectorMax3I& params) { const Interval &t = params[0], &u = params[1], &v = params[2]; // 0 ≤ t, u, v ≤ 1 is satisfied by the initial domain of the solve - return overlap(u + v, Interval(0, 1)); + return (u + v).overlaps(Interval(0, 1)); }; Eigen::Vector3d tol = compute_face_vertex_tolerance( diff --git a/src/geometry/intersection.cpp b/src/geometry/intersection.cpp index b540c783..b6be3c9f 100644 --- a/src/geometry/intersection.cpp +++ b/src/geometry/intersection.cpp @@ -16,10 +16,10 @@ bool is_point_along_edge( Interval alpha = point_edge_closest_point(p, e0, e1); // Check this in case empty intervals are not allowed - if (!overlap(alpha, Interval(0, 1))) { + if (!alpha.overlaps(Interval(0, 1))) { return false; } - Interval valid_alpha = boost::numeric::intersect(alpha, Interval(0, 1)); + Interval valid_alpha = alpha & Interval(0, 1); // Check the distance to the closest point is small VectorMax3I edge_to_point = e0 + valid_alpha * e - p; diff --git a/src/interval/filib_rounding.cpp b/src/interval/filib_rounding.cpp deleted file mode 100644 index a18fb355..00000000 --- a/src/interval/filib_rounding.cpp +++ /dev/null @@ -1,276 +0,0 @@ -#include "filib_rounding.hpp" - -#include - -#include -#undef local - -#include - -namespace ipc::rigid { - -double FILibRounding::add_down(double x, double y) -{ - return x == -y ? 0 : q_pred(x + y); -} - -double FILibRounding::add_up(double x, double y) -{ - return x == -y ? 0 : q_succ(x + y); -} - -double FILibRounding::sub_down(double x, double y) -{ - return x == y ? 0 : q_pred(x - y); -} - -double FILibRounding::sub_up(double x, double y) -{ - return x == y ? 0 : q_succ(x - y); -} - -double FILibRounding::mul_down(double x, double y) -{ - return (x == 0 || y == 0) ? 0 : q_pred(x * y); -} - -double FILibRounding::mul_up(double x, double y) -{ - return (x == 0 || y == 0) ? 0 : q_succ(x * y); -} - -double FILibRounding::div_down(double x, double y) -{ - return x == 0 ? 0 : q_pred(x / y); -} - -double FILibRounding::div_up(double x, double y) -{ - return x == 0 ? 0 : q_succ(x / y); -} - -double FILibRounding::sqrt_down(double x) -{ - return x == 0 ? 0 : r_pred(q_sqrt(x)); -} - -double FILibRounding::sqrt_up(double x) -{ - return x == 0 ? 0 : r_succ(q_sqrt(x)); -} - -double FILibRounding::exp_down(double x) -{ - double r = x == 0 ? 1.0 : (x <= q_mine ? 0 : (q_exp(x) * q_exem)); - // Snap negative r values to 0 - if (r < 0) { - r = 0; - } - // Snap negative r values to 1 if x is positive - if (x >= 0 && r < 1.0) { - r = 1; - } - return r; -} - -double FILibRounding::exp_up(double x) -{ - double r = x == 0 ? 1.0 : (x <= q_mine ? q_minr : (q_exp(x) * q_exep)); - // Snap negative r values to 0 - if (r < 0) { - r = 0; - } - // Snap negative r values to 1 if x is positive - if (x >= 0 && r < 1.0) { - r = 1; - } - return r; -} - -// double FILibRounding::log_down(double x) -// { -// assert(false); -// throw NotImplementedError(""); -// } - -// double FILibRounding::log_up(double x) -// { -// assert(false); -// throw NotImplementedError(""); -// } - -double FILibRounding::cos_down(double x) -{ - double r; - if (x < -q_sint[2] || x > q_sint[2]) { - r = -1.0; - } else { - r = q_cos(x); - r *= r < 0 ? q_cosp : q_cosm; - } - if (r < -1.0) { - r = -1; - } - if (r > 1.0) { - r = 1; - } - return r; -} - -double FILibRounding::cos_up(double x) -{ - double r; - if (x < -q_sint[2] || x > q_sint[2]) { - r = 1.0; - } else { - r = q_cos(x); - r *= r < 0 ? q_cosm : q_cosp; - } - if (r < -1.0) { - r = -1; - } - if (r > 1.0) { - r = 1; - } - return r; -} - -// double FILibRounding::tan_down(double x) -// { -// assert(false); -// throw NotImplementedError("tan_down is not implemented!"); -// } - -// double FILibRounding::tan_up(double x) -// { -// assert(false); -// throw NotImplementedError("tan_up is not implemented!"); -// } - -// double FILibRounding::asin_down(double x) -// { -// assert(false); -// throw NotImplementedError("asin_down is not implemented!"); -// } - -// double FILibRounding::asin_up(double x) -// { -// assert(false); -// throw NotImplementedError("asin_up is not implemented!"); -// } - -// double FILibRounding::acos_down(double x) -// { -// assert(false); -// throw NotImplementedError("acos_down is not implemented!"); -// } - -// double FILibRounding::acos_up(double x) -// { -// assert(false); -// throw NotImplementedError("acos_up is not implemented!"); -// } - -// double FILibRounding::atan_down(double x) -// { -// assert(false); -// throw NotImplementedError("atan_down is not implemented!"); -// } - -// double FILibRounding::atan_up(double x) -// { -// assert(false); -// throw NotImplementedError("atan_up is not implemented!"); -// } - -// double FILibRounding::sinh_down(double x) -// { -// assert(false); -// throw NotImplementedError("sinh_down is not implemented!"); -// } - -// double FILibRounding::sinh_up(double x) -// { -// assert(false); -// throw NotImplementedError("sinh_up is not implemented!"); -// } - -// double FILibRounding::cosh_down(double x) -// { -// assert(false); -// throw NotImplementedError("cosh_down is not implemented!"); -// } - -// double FILibRounding::cosh_up(double x) -// { -// assert(false); -// throw NotImplementedError("cosh_up is not implemented!"); -// } - -// double FILibRounding::tanh_down(double x) -// { -// assert(false); -// throw NotImplementedError("tanh_down is not implemented!"); -// } - -// double FILibRounding::tanh_up(double x) -// { -// assert(false); -// throw NotImplementedError("tanh_up is not implemented!"); -// } - -// double FILibRounding::asinh_down(double x) -// { -// assert(false); -// throw NotImplementedError("asinh_down is not implemented!"); -// } - -// double FILibRounding::asinh_up(double x) -// { -// assert(false); -// throw NotImplementedError("asinh_up is not implemented!"); -// } - -// double FILibRounding::acosh_down(double x) -// { -// assert(false); -// throw NotImplementedError("acosh_down is not implemented!"); -// } - -// double FILibRounding::acosh_up(double x) -// { -// assert(false); -// throw NotImplementedError("acosh_up is not implemented!"); -// } - -// double FILibRounding::atanh_down(double x) -// { -// assert(false); -// throw NotImplementedError("atanh_down is not implemented!"); -// } - -// double FILibRounding::atanh_up(double x) -// { -// assert(false); -// throw NotImplementedError("atanh_up is not implemented!"); -// } - -double FILibRounding::median(double x, double y) -{ - this->to_nearest(); - return this->force_rounding(q_mid({ x, y })); -} - -double FILibRounding::int_down(double x) -{ - this->downward(); - return this->to_int(x); -} - -double FILibRounding::int_up(double x) -{ - this->upward(); - return this->to_int(x); -} - -} // namespace ipc::rigid diff --git a/src/interval/filib_rounding.hpp b/src/interval/filib_rounding.hpp deleted file mode 100644 index e43d2e30..00000000 --- a/src/interval/filib_rounding.hpp +++ /dev/null @@ -1,73 +0,0 @@ -#pragma once - -#include - -namespace ipc::rigid { - -// A wrapper for filibs rounding -struct FILibRounding : boost::numeric::interval_lib::rounding_control { - // default constructor, destructor - // FILibRounding() {} - // ~FILibRounding() {} - void init() {} - // mathematical operations - double add_down(double x, double y); // [-∞;+∞][-∞;+∞] - double add_up(double x, double y); // [-∞;+∞][-∞;+∞] - double sub_down(double x, double y); // [-∞;+∞][-∞;+∞] - double sub_up(double x, double y); // [-∞;+∞][-∞;+∞] - double mul_down(double x, double y); // [-∞;+∞][-∞;+∞] - double mul_up(double x, double y); // [-∞;+∞][-∞;+∞] - double div_down(double x, double y); // [-∞;+∞]([-∞;+∞]-{0}) - double div_up(double x, double y); // [-∞;+∞]([-∞;+∞]-{0}) - double sqrt_down(double x); // ]0;+∞] - double sqrt_up(double x); // ]0;+∞] - double exp_down(double x); // [-∞;+∞] - double exp_up(double x); // [-∞;+∞] - // double log_down(double x); // ]0;+∞] - // double log_up(double x); // ]0;+∞] - double cos_down(double x); // [0;2π] - double cos_up(double x); // [0;2π] - // double tan_down(double x); // ]-π/2;π/2[ - // double tan_up(double x); // ]-π/2;π/2[ - // double asin_down(double x); // [-1;1] - // double asin_up(double x); // [-1;1] - // double acos_down(double x); // [-1;1] - // double acos_up(double x); // [-1;1] - // double atan_down(double x); // [-∞;+∞] - // double atan_up(double x); // [-∞;+∞] - // double sinh_down(double x); // [-∞;+∞] - // double sinh_up(double x); // [-∞;+∞] - // double cosh_down(double x); // [-∞;+∞] - // double cosh_up(double x); // [-∞;+∞] - // double tanh_down(double x); // [-∞;+∞] - // double tanh_up(double x); // [-∞;+∞] - // double asinh_down(double x); // [-∞;+∞] - // double asinh_up(double x); // [-∞;+∞] - // double acosh_down(double x); // [1;+∞] - // double acosh_up(double x); // [1;+∞] - // double atanh_down(double x); // [-1;1] - // double atanh_up(double x); // [-1;1] - - /// @brief median of two numbers rounded to closest number - double median(double x, double y); // [-∞;+∞][-∞;+∞] - - /// @brief round down to integer - double int_down(double x); // [-∞;+∞] - /// @brief round up to integer - double int_up(double x); // [-∞;+∞] - - /// @brief convert to the closest double (rounding down) - template double conv_down(const T& x) - { - this->downward(); - return this->force_rounding(x); - } - /// @brief convert to the closest double (rounding up) - template double conv_up(const T& x) - { - this->upward(); - return this->force_rounding(x); - } -}; - -} // namespace ipc::rigid diff --git a/src/interval/interval.cpp b/src/interval/interval.cpp new file mode 100644 index 00000000..ea55d8fd --- /dev/null +++ b/src/interval/interval.cpp @@ -0,0 +1,353 @@ +#include "interval.hpp" + +#include +#include +#include + +namespace ipc::rigid { + +Interval::Interval() { *this = empty(); } + +Interval::Interval(double x) +{ + INF = x; + SUP = x; +} + +Interval::Interval(double x, double y) +{ + INF = x; + SUP = y; +} + +Interval Interval::empty() +{ + return Interval( + std::numeric_limits::infinity(), + -std::numeric_limits::infinity()); +} + +/* ------------------------------------------------------------------- */ +/* --- IO (input/output) --- */ +/* ------------------------------------------------------------------- */ + +std::istream& operator>>(std::istream& is, Interval& a) +{ + double help, ioconst; + + ioconst = (1e17 - 1) * 1e27; + + is >> help; + if ((help < -ioconst) || (help > ioconst)) + a.INF = q_pred(q_pred(help)); + else + a.INF = q_pred(help); + + is >> help; + if ((help < -ioconst) || (help > ioconst)) + a.SUP = q_succ(q_succ(help)); + else + a.SUP = q_succ(help); + + return is; +} + +std::ostream& operator<<(std::ostream& os, const Interval& a) +{ + Interval help; + + help.INF = q_pred(q_pred(a.INF)); + help.SUP = q_succ(q_succ(a.SUP)); + + std::ios_base::fmtflags aktform = std::cout.flags(); + os << "[" << std::setprecision(15) << std::setw(23) + << std::setiosflags(std::ios::scientific); + os << help.INF; + std::cout.flags(aktform); + os << "," << std::setprecision(15) << std::setw(23) + << std::setiosflags(std::ios::scientific); + os << help.SUP; + std::cout.flags(aktform); + os << " ]"; + + return os; +} + +/* ------------------------------------------------------------------- */ +/* --- interval arithmetic (basic operations) --- */ +/* ------------------------------------------------------------------- */ + +Interval operator+(const Interval& a, const Interval& b) +{ + return add_ii(a, b); +} + +Interval operator+(const Interval& a, double b) { return add_id(a, b); } + +Interval operator+(double a, const Interval& b) { return add_di(a, b); } + +Interval operator+(const Interval& a) { return a; } + +Interval& Interval::operator+=(const Interval& rhs) +{ + *this = add_ii(*this, rhs); + return *this; +} + +Interval& Interval::operator+=(const double& rhs) +{ + *this = add_id(*this, rhs); + return *this; +} + +Interval operator-(const Interval& a, const Interval& b) +{ + return sub_ii(a, b); +} + +Interval operator-(const Interval& a, double b) { return sub_id(a, b); } + +Interval operator-(double a, const Interval& b) { return sub_di(a, b); } + +Interval operator-(const Interval& a) { return Interval(-a.SUP, -a.INF); } + +Interval& Interval::operator-=(const Interval& rhs) +{ + *this = sub_ii(*this, rhs); + return *this; +} + +Interval& Interval::operator-=(const double rhs) +{ + *this = sub_id(*this, rhs); + return *this; +} + +Interval operator*(const Interval& a, const Interval& b) +{ + return mul_ii(a, b); +} + +Interval operator*(const Interval& a, double b) { return mul_id(a, b); } + +Interval operator*(double a, const Interval& b) { return mul_di(a, b); } + +Interval& Interval::operator*=(const Interval& rhs) +{ + *this = mul_ii(*this, rhs); + return *this; +} + +Interval& Interval::operator*=(const double rhs) +{ + *this = mul_id(*this, rhs); + return *this; +} + +Interval operator/(const Interval& a, const Interval& b) +{ + return div_ii(a, b); +} + +Interval operator/(const Interval& a, double b) { return div_id(a, b); } + +Interval operator/(double a, const Interval& b) { return div_di(a, b); } + +Interval& Interval::operator/=(const Interval& rhs) +{ + *this = div_ii(*this, rhs); + return *this; +} + +Interval& Interval::operator/=(const double& rhs) +{ + *this = div_id(*this, rhs); + return *this; +} + +/* ------------------------------------------------------------------- */ +/* --- Interval arithmetic (logical operations) --- */ +/* ------------------------------------------------------------------- */ + +bool operator==(const Interval& a, const Interval& b) { return ieq_ii(a, b); } + +bool operator==(const Interval& a, double b) { return ieq_ii(a, Interval(b)); } + +bool operator!=(const Interval& a, const Interval& b) +{ + return !(a.INF == b.INF && a.SUP == b.SUP); +} + +bool operator<(const Interval& a, const Interval& b) { return in_ii(a, b); } + +bool operator<(const double a, const Interval& b) +{ + return b.INF < a && a < b.SUP; +} + +bool operator<=(const Interval& a, const Interval& b) +{ + return b.INF <= a.INF && a.SUP <= b.SUP; +} + +bool operator<=(double a, const Interval& b) { return in_di(a, b); } + +bool operator>(const Interval& a, const Interval& b) +{ + return b.INF > a.INF && a.SUP > b.SUP; +} +bool operator>(const Interval& a, double b) { return a.INF < b && b < a.SUP; } + +bool operator>=(const Interval& a, const Interval& b) +{ + return b.INF >= a.INF && a.SUP >= b.SUP; +} + +bool operator>=(const Interval& a, double b) +{ + return a.INF <= b && b <= a.SUP; +} + +Interval operator|(const Interval& a, const Interval& b) { return hull(a, b); } +Interval& Interval::operator|=(const Interval& b) +{ + *this = *this | b; + return *this; +} + +Interval operator&(const Interval& a, const Interval& b) +{ + return intsec(a, b); +} +Interval& Interval::operator&=(const Interval& b) +{ + *this = *this & b; + return *this; +} + +bool in(double a, const Interval& b) { return in_di(a, b); } + +bool in(const Interval& a, const Interval& b) { return in_ii(a, b); } + +bool Interval::is_empty() const { return this->INF > this->SUP; } + +bool Interval::zero_in() const { return this->INF <= 0 && this->SUP >= 0; } + +bool Interval::overlaps(const Interval& b) const +{ + return (this->INF <= b.SUP) && (b.INF <= this->SUP); +} + +/* ------------------------------------------------------------------- */ +/* --- utilities, mid, diam, ... --- */ +/* ------------------------------------------------------------------- */ + +double Interval::mid() const { return q_mid(*this); } + +std::pair Interval::bisect() const +{ + double mid = this->mid(); + return std::make_pair(Interval(this->INF, mid), Interval(mid, this->SUP)); +} + +bool disjoint(const Interval& a, const Interval& b) { return dis_ii(a, b); } + +double Interval::diam() const { return q_diam(*this); } + +double Interval::drel() const +{ + if ((SUP <= -q_minr) || (q_minr <= INF)) { + if (INF > 0) + return diam() / INF; + else + return diam() / (-SUP); + } else { + return diam(); + } +} + +Interval Interval::blow(double eps) const +{ + Interval y; + y = (1.0 + eps) * (*this) - eps * (*this); + return (Interval(q_pred(y.INF), q_succ(y.SUP))); +} + +// min max function, same as what BOOST does +Interval max(const Interval& x, const Interval& y) +{ + return Interval(std::max(x.INF, y.INF), std::max(x.SUP, y.SUP)); +} +Interval max(const Interval& x, double y) +{ + return Interval(std::max(x.INF, y), std::max(x.SUP, y)); +} + +Interval max(double x, const Interval& y) +{ + return Interval(std::max(x, y.INF), std::max(x, y.SUP)); +} + +Interval min(const Interval& x, const Interval& y) +{ + return Interval(std::min(x.INF, y.INF), std::min(x.SUP, y.SUP)); +} +Interval min(const Interval& x, double y) +{ + return Interval(std::min(x.INF, y), std::min(x.SUP, y)); +} + +Interval min(double x, const Interval& y) +{ + return Interval(std::min(x, y.INF), std::min(x, y.SUP)); +} + +/* ------------------------------------------------------------------- */ +/* --- Interval arithmetic (elementary functions) --- */ +/* ------------------------------------------------------------------- */ + +Interval exp(const Interval& a) { return j_exp(a); } +Interval expm(const Interval& a) { return j_expm(a); } +Interval sinh(const Interval& a) { return j_sinh(a); } +Interval cosh(const Interval& a) { return j_cosh(a); } +Interval coth(const Interval& a) { return j_coth(a); } +Interval tanh(const Interval& a) { return j_tanh(a); } +Interval log(const Interval& a) { return j_log(a); } +Interval ln(const Interval& a) { return j_log(a); } +Interval lg1p(const Interval& a) { return j_lg1p(a); } +Interval sqrt(const Interval& a) { return j_sqrt(a); } +Interval sqr(const Interval& a) { return j_sqr(a); } +Interval asnh(const Interval& a) { return j_asnh(a); } +Interval asinh(const Interval& a) { return j_asnh(a); } +Interval acsh(const Interval& a) { return j_acsh(a); } +Interval acosh(const Interval& a) { return j_acsh(a); } +Interval acth(const Interval& a) { return j_acth(a); } +Interval acoth(const Interval& a) { return j_acth(a); } +Interval atnh(const Interval& a) { return j_atnh(a); } +Interval atanh(const Interval& a) { return j_atnh(a); } +Interval asin(const Interval& a) { return j_asin(a); } +Interval acos(const Interval& a) { return j_acos(a); } +Interval acot(const Interval& a) { return j_acot(a); } +Interval atan(const Interval& a) { return j_atan(a); } +Interval sin(const Interval& a) { return j_sin(a); } +Interval cos(const Interval& a) { return j_cos(a); } +Interval cot(const Interval& a) { return j_cot(a); } +Interval tan(const Interval& a) { return j_tan(a); } +Interval exp2(const Interval& a) { return j_exp2(a); } +Interval ex10(const Interval& a) { return j_ex10(a); } +Interval log2(const Interval& a) { return j_log2(a); } +Interval lg10(const Interval& a) { return j_lg10(a); } +Interval erf(const Interval& a) { return j_erf(a); } +Interval erfc(const Interval& a) { return j_erfc(a); } + +Interval abs(const Interval& a) +{ + if (a.INF >= 0) + return a; + else if (a.SUP <= 0) + return -a; + else + return Interval(0, std::max(-a.INF, a.SUP)); +} + +} // namespace ipc::rigid \ No newline at end of file diff --git a/src/interval/interval.hpp b/src/interval/interval.hpp index 2134c7b8..9462a11e 100644 --- a/src/interval/interval.hpp +++ b/src/interval/interval.hpp @@ -1,66 +1,183 @@ // An interval object. #pragma once +#include + +#include + #include -#include +namespace ipc::rigid { -#define USE_FILIB_INTERVALS -#ifdef USE_FILIB_INTERVALS -#include -#endif +class Interval { +public: + Interval(); + Interval(double x); + Interval(double x, double y); -#include + static Interval empty(); -namespace ipc::rigid { + /* ------------------------------------------------------------------- */ + /* --- IO (input/output) --- */ + /* ------------------------------------------------------------------- */ -namespace interval_options { - typedef boost::numeric::interval_lib::checking_catch_nan - CheckingPolicy; -} // namespace interval_options - -#ifdef USE_FILIB_INTERVALS - -// Use filib rounding arithmetic -typedef boost::numeric::interval< - double, - boost::numeric::interval_lib::policies< - boost::numeric::interval_lib::save_state, - interval_options::CheckingPolicy>> - Interval; - -#elif defined(__APPLE__) - -// clang-format off -#warning "Rounding modes seem to be broken with trigonometric functions on macOS, unable to compute exact interval arithmetic!" -// clang-format on -typedef boost::numeric::interval< - double, - boost::numeric::interval_lib::policies< - boost::numeric::interval_lib::save_state< - boost::numeric::interval_lib::rounded_transc_exact>, - interval_options::CheckingPolicy>> - Interval; - -#else - -// Use proper rounding arithmetic -typedef boost::numeric::interval< - double, - boost::numeric::interval_lib::policies< - boost::numeric::interval_lib::save_state< - boost::numeric::interval_lib::rounded_transc_std>, - interval_options::CheckingPolicy>> - Interval; - -#endif // USE_FILIB_INTERVALS + friend std::istream& operator>>(std::istream& is, Interval& a); + friend std::ostream& operator<<(std::ostream& os, const Interval& a); + + /* ------------------------------------------------------------------- */ + /* --- interval arithmetic (basic operations) --- */ + /* ------------------------------------------------------------------- */ + + friend Interval operator+(const Interval& a, const Interval& b); + friend Interval operator+(const Interval& a, const double b); + friend Interval operator+(const double a, const Interval& b); + friend Interval operator+(const Interval& a); + + Interval& operator+=(const Interval& rhs); + Interval& operator+=(const double& rhs); + + friend Interval operator-(const Interval& a, const Interval& b); + friend Interval operator-(const Interval& a, const double b); + friend Interval operator-(const double a, const Interval& b); + friend Interval operator-(const Interval& a); + + Interval& operator-=(const Interval& rhs); + Interval& operator-=(const double rhs); + + friend Interval operator*(const Interval& a, const Interval& b); + friend Interval operator*(const Interval& a, double b); + friend Interval operator*(double a, const Interval& b); + + Interval& operator*=(const Interval& rhs); + Interval& operator*=(const double rhs); + + friend Interval operator/(const Interval& a, const Interval& b); + friend Interval operator/(const Interval& a, const double b); + friend Interval operator/(const double a, const Interval& b); + + Interval& operator/=(const Interval& rhs); + Interval& operator/=(const double& rhs); + + /* ------------------------------------------------------------------- */ + /* --- Interval arithmetic (logical operations) --- */ + /* ------------------------------------------------------------------- */ + + friend bool operator==(const Interval& a, const Interval& b); + friend bool operator==(const Interval& a, const double b); + + friend bool operator!=(const Interval& a, const Interval& b); + + friend bool operator<(const Interval& a, const Interval& b); + friend bool operator<(const double a, const Interval& b); + friend bool operator<=(const Interval& a, const Interval& b); + friend bool operator<=(const double a, const Interval& b); + + friend bool operator>(const Interval& a, const Interval& b); + friend bool operator>(const Interval& a, const double b); + friend bool operator>=(const Interval& a, const Interval& b); + friend bool operator>=(const Interval& a, const double b); + + friend Interval operator|(const Interval& a, const Interval& b); + friend Interval operator&(const Interval& a, const Interval& b); + + Interval& operator|=(const Interval& b); + Interval& operator&=(const Interval& b); + + friend bool in(double a, const Interval& b); + friend bool in(const Interval& a, const Interval& b); + + bool is_empty() const; + bool zero_in() const; + + bool overlaps(const Interval& b) const; + + /* ------------------------------------------------------------------- */ + /* --- utilities, mid, diam, ... --- */ + /* ------------------------------------------------------------------- */ + + double mid() const; + std::pair bisect() const; + friend bool disjoint(const Interval& a, const Interval& b); + double diam() const; + double drel() const; + double width() const { return diam(); } + Interval blow(double eps) const; + + // min max function, same as what BOOST does + friend Interval max(const Interval& x, const Interval& y); + friend Interval max(const Interval& x, double y); + friend Interval max(double x, const Interval& y); + + friend Interval min(const Interval& x, const Interval& y); + friend Interval min(const Interval& x, double y); + friend Interval min(double x, const Interval& y); + + /* ------------------------------------------------------------------- */ + /* --- Interval arithmetic (elementary functions) --- */ + /* ------------------------------------------------------------------- */ + + friend Interval exp(const Interval& a); + friend Interval expm(const Interval& a); + friend Interval sinh(const Interval& a); + friend Interval cosh(const Interval& a); + friend Interval coth(const Interval& a); + friend Interval tanh(const Interval& a); + friend Interval log(const Interval& a); + friend Interval ln(const Interval& a); + friend Interval lg1p(const Interval& a); + friend Interval sqrt(const Interval& a); + friend Interval sqr(const Interval& a); + friend Interval asnh(const Interval& a); + friend Interval asinh(const Interval& a); + friend Interval acsh(const Interval& a); + friend Interval acosh(const Interval& a); + friend Interval acth(const Interval& a); + friend Interval acoth(const Interval& a); + friend Interval atnh(const Interval& a); + friend Interval atanh(const Interval& a); + friend Interval asin(const Interval& a); + friend Interval acos(const Interval& a); + friend Interval acot(const Interval& a); + friend Interval atan(const Interval& a); + friend Interval sin(const Interval& a); + friend Interval cos(const Interval& a); + friend Interval cot(const Interval& a); + friend Interval tan(const Interval& a); + friend Interval exp2(const Interval& a); + friend Interval ex10(const Interval& a); + friend Interval log2(const Interval& a); + friend Interval lg10(const Interval& a); + friend Interval erf(const Interval& a); + friend Interval erfc(const Interval& a); + + friend Interval abs(const Interval& a); + + const double& lower() const { return INF; } + const double& upper() const { return SUP; } + + double& lower() { return INF; } + double& upper() { return SUP; } + +protected: + // Helper functions to convert from pure FILib interval to my Interval + Interval(interval i) + { + INF = i.INF; + SUP = i.SUP; + } + + // Helper functions to convert from my Interval to pure FILib interval + operator interval() const { return { INF, SUP }; } + + double INF, SUP; +}; template inline Eigen::VectorXd width(const Eigen::MatrixBase& x) { Eigen::VectorXd w(x.size()); for (int i = 0; i < x.size(); i++) { - w(i) = width(x(i)); + w(i) = x(i).width(); } return w; } @@ -81,7 +198,7 @@ inline bool zero_in(const Eigen::MatrixBase& x) { // Check if the origin is in the n-dimensional interval for (int i = 0; i < x.size(); i++) { - if (!boost::numeric::zero_in(x(i))) { + if (!x(i).zero_in()) { return false; } } @@ -115,19 +232,19 @@ struct ScalarBinaryOpTraits { typedef ipc::rigid::Interval ReturnType; }; -#if EIGEN_MAJOR_VERSION >= 3 -namespace internal { - template - struct is_convertible> { - enum { value = is_convertible::value }; - }; - - template - struct is_convertible< - boost::numeric::interval, - boost::numeric::interval> { - enum { value = true }; - }; -} // namespace internal -#endif +// #if EIGEN_MAJOR_VERSION >= 3 +// namespace internal { +// template +// struct is_convertible> { +// enum { value = is_convertible::value }; +// }; + +// template +// struct is_convertible< +// boost::numeric::interval, +// boost::numeric::interval> { +// enum { value = true }; +// }; +// } // namespace internal +// #endif } // namespace Eigen diff --git a/src/interval/interval_root_finder.cpp b/src/interval/interval_root_finder.cpp index d0d67c1e..c1f082b4 100644 --- a/src/interval/interval_root_finder.cpp +++ b/src/interval/interval_root_finder.cpp @@ -81,11 +81,11 @@ void log_octree( return; } - std::pair t_split = bisect(x0(0)); + std::pair t_split = x0(0).bisect(); for (Interval t : { t_split.first, t_split.second }) { - std::pair alpha_split = bisect(x0(1)); + std::pair alpha_split = x0(1).bisect(); for (Interval alpha : { alpha_split.first, alpha_split.second }) { - std::pair beta_split = bisect(x0(2)); + std::pair beta_split = x0(2).bisect(); for (Interval beta : { beta_split.first, beta_split.second }) { Vector3I x(t, alpha, beta); log_octree(f, x, levels - 1); @@ -178,26 +178,13 @@ bool interval_root_finder( } assert(split_i >= 0 && split_i <= x.size()); - std::pair halves = bisect(x(split_i)); + std::pair halves = x(split_i).bisect(); // Push the second half on first so it is examined after the first half x(split_i) = halves.second; xs.push(x); x(split_i) = halves.first; xs.push(x); } - // TODO: This is needs to be updated when max_iterations is renabled - // if (!xs.empty()) { - // // Return the earliest possible time of impact - // x = xs.top().first; - // xs.pop(); - // while (!xs.empty()) { - // if (xs.top().first[0].lower() < x[0].lower()) { - // x = xs.top().first; - // } - // xs.pop(); - // } - // return true; // A conservative answer - // } x = earliest_root; return found_root; } diff --git a/src/problems/distance_barrier_rb_problem.cpp b/src/problems/distance_barrier_rb_problem.cpp index 72469c18..fd3b48ae 100644 --- a/src/problems/distance_barrier_rb_problem.cpp +++ b/src/problems/distance_barrier_rb_problem.cpp @@ -603,8 +603,8 @@ bool DistanceBarrierRBProblem::take_step(const Eigen::VectorXd& x) * (Qdot_prev + h / 4.0 * ( - // Tau * Jinv + - rb.Qddot)); + // Tau * Jinv + + rb.Qddot)); rb.Qddot = 4 * (Q - Q_tilde) / (h * h); //+ Tau * Jinv; break; } diff --git a/src/problems/rigid_body_collision_constraint.cpp b/src/problems/rigid_body_collision_constraint.cpp index 1263cca9..35984e1c 100644 --- a/src/problems/rigid_body_collision_constraint.cpp +++ b/src/problems/rigid_body_collision_constraint.cpp @@ -50,7 +50,7 @@ RigidBodyEdgeEdgeConstraint::RigidBodyEdgeEdgeConstraint( RigidBodyFaceVertexConstraint::RigidBodyFaceVertexConstraint( const RigidBodyAssembler& bodies, - long face_index, + long face_index, long vertex_index) { const auto& face = bodies.m_faces.row(face_index); diff --git a/src/problems/rigid_body_collision_constraint.hpp b/src/problems/rigid_body_collision_constraint.hpp index f891eca1..98f106eb 100644 --- a/src/problems/rigid_body_collision_constraint.hpp +++ b/src/problems/rigid_body_collision_constraint.hpp @@ -26,20 +26,20 @@ struct RigidBodyVertexVertexConstraint { const RigidBodyAssembler& bodies, const VertexVertexConstraint& vv_constraint) : RigidBodyVertexVertexConstraint( - bodies, - vv_constraint.vertex0_index, - vv_constraint.vertex1_index, - vv_constraint.multiplicity) + bodies, + vv_constraint.vertex0_index, + vv_constraint.vertex1_index, + vv_constraint.multiplicity) { } RigidBodyVertexVertexConstraint( const RigidBodyAssembler& bodies, const VertexVertexFrictionConstraint& vv_constraint) : RigidBodyVertexVertexConstraint( - bodies, - vv_constraint.vertex0_index, - vv_constraint.vertex1_index, - vv_constraint.multiplicity) + bodies, + vv_constraint.vertex0_index, + vv_constraint.vertex1_index, + vv_constraint.multiplicity) { } @@ -69,20 +69,20 @@ struct RigidBodyEdgeVertexConstraint { const RigidBodyAssembler& bodies, const EdgeVertexConstraint& ev_constraint) : RigidBodyEdgeVertexConstraint( - bodies, - ev_constraint.edge_index, - ev_constraint.vertex_index, - ev_constraint.multiplicity) + bodies, + ev_constraint.edge_index, + ev_constraint.vertex_index, + ev_constraint.multiplicity) { } RigidBodyEdgeVertexConstraint( const RigidBodyAssembler& bodies, const EdgeVertexFrictionConstraint& ev_constraint) : RigidBodyEdgeVertexConstraint( - bodies, - ev_constraint.edge_index, - ev_constraint.vertex_index, - ev_constraint.multiplicity) + bodies, + ev_constraint.edge_index, + ev_constraint.vertex_index, + ev_constraint.multiplicity) { } @@ -118,17 +118,17 @@ struct RigidBodyEdgeEdgeConstraint { const RigidBodyAssembler& bodies, const EdgeEdgeConstraint& ee_constraint) : RigidBodyEdgeEdgeConstraint( - bodies, - ee_constraint.edge0_index, - ee_constraint.edge1_index, - ee_constraint.eps_x) + bodies, + ee_constraint.edge0_index, + ee_constraint.edge1_index, + ee_constraint.eps_x) { } RigidBodyEdgeEdgeConstraint( const RigidBodyAssembler& bodies, const EdgeEdgeFrictionConstraint& ee_constraint) : RigidBodyEdgeEdgeConstraint( - bodies, ee_constraint.edge0_index, ee_constraint.edge1_index, -1) + bodies, ee_constraint.edge0_index, ee_constraint.edge1_index, -1) { } @@ -154,21 +154,21 @@ struct RigidBodyFaceVertexConstraint { const long multiplicity = 1; RigidBodyFaceVertexConstraint( - const RigidBodyAssembler& bodies, - long face_index, + const RigidBodyAssembler& bodies, + long face_index, long vertex_index); RigidBodyFaceVertexConstraint( const RigidBodyAssembler& bodies, const FaceVertexConstraint& fv_constraint) : RigidBodyFaceVertexConstraint( - bodies, fv_constraint.face_index, fv_constraint.vertex_index) + bodies, fv_constraint.face_index, fv_constraint.vertex_index) { } RigidBodyFaceVertexConstraint( const RigidBodyAssembler& bodies, const FaceVertexFrictionConstraint& fv_constraint) : RigidBodyFaceVertexConstraint( - bodies, fv_constraint.face_index, fv_constraint.vertex_index) + bodies, fv_constraint.face_index, fv_constraint.vertex_index) { } @@ -208,8 +208,8 @@ vertex_local_body_ids(const Constraints& constraints, size_t ci) template std::array body_ids( - const RigidBodyAssembler& bodies, - const Constraints& constraints, + const RigidBodyAssembler& bodies, + const Constraints& constraints, size_t ci) { if (ci < constraints.vv_constraints.size()) { diff --git a/src/solvers/newton_solver.cpp b/src/solvers/newton_solver.cpp index af860722..a2796e14 100644 --- a/src/solvers/newton_solver.cpp +++ b/src/solvers/newton_solver.cpp @@ -508,8 +508,6 @@ bool NewtonSolver::compute_direction( linear_solver.factorize(hessian); if (linear_solver.info() == Eigen::Success) { - // TODO: Do we have a better initial guess for iterative - // solvers? direction = Eigen::VectorXd::Zero(gradient.size()); direction = linear_solver.solve(-gradient); if (linear_solver.info() == Eigen::Success) { diff --git a/src/tools/obj_sequence.cpp b/src/tools/obj_sequence.cpp index 812a2e51..89770de4 100644 --- a/src/tools/obj_sequence.cpp +++ b/src/tools/obj_sequence.cpp @@ -28,7 +28,7 @@ int main(int argc, char* argv[]) app.add_option("--log,--loglevel", loglevel, "log level") ->default_val(loglevel) ->transform(CLI::CheckedTransformer( - SPDLOG_LEVEL_NAMES_TO_LEVELS, CLI::ignore_case)); + SPDLOG_LEVEL_NAMES_TO_LEVELS, CLI::ignore_case)); try { app.parse(argc, argv); diff --git a/src/utils/clamp.hpp b/src/utils/clamp.hpp index 1a080f46..d12ebc6d 100644 --- a/src/utils/clamp.hpp +++ b/src/utils/clamp.hpp @@ -11,7 +11,7 @@ template inline T clamp_to_01(const T& x) template <> inline Interval clamp_to_01(const Interval& x) { - return boost::numeric::intersect(x, Interval(0, 1)); + return x & Interval(0, 1); } } // namespace ipc::rigid diff --git a/src/utils/is_zero.hpp b/src/utils/is_zero.hpp index d0bc906e..99ab675e 100644 --- a/src/utils/is_zero.hpp +++ b/src/utils/is_zero.hpp @@ -7,13 +7,10 @@ namespace ipc::rigid { template inline bool is_zero(T x) { return x == 0.0; } -template <> inline bool is_zero(Interval x) -{ - return boost::numeric::zero_in(x); -} +template <> inline bool is_zero(Interval x) { return x.zero_in(); } template -inline bool is_zero(Vector X) +inline bool is_zero(const Vector& X) { // Check that all components contain zero // WARNING: This is conservative, but no exact diff --git a/src/utils/sinc.cpp b/src/utils/sinc.cpp index 0ad4e6c3..a2453e79 100644 --- a/src/utils/sinc.cpp +++ b/src/utils/sinc.cpp @@ -79,22 +79,19 @@ Interval sinc(const Interval& x) } // sinc is monotonically decreasing, so flip a and b. // TODO: Set rounding modes here to avoid round off error. - y = hull( - y, - Interval( - // https://www.wolframalpha.com/input/?i=min+sin%28x%29%2Fx+between+4+and+5 - std::max( - _sinc_interval_taylor(x_monotonic.upper()).lower(), - -0.271724), // <-- A conservative lower bound for sinc(x) - std::min( - _sinc_interval_taylor(x_monotonic.lower()).upper(), 1.0))); + y |= Interval( + // https://www.wolframalpha.com/input/?i=min+sin%28x%29%2Fx+between+4+and+5 + std::max( + _sinc_interval_taylor(x_monotonic.upper()).lower(), + -0.271724), // <-- A conservative lower bound for sinc(x) + std::min(_sinc_interval_taylor(x_monotonic.lower()).upper(), 1.0)); } // Case 2 (Not necessarily monotonic): - if (!empty(x_gt_monotonic)) { + if (!x_gt_monotonic.is_empty()) { // x_gt_monotonic is larger than one, so the division should be well // behaved. - y = hull(y, sin(x_gt_monotonic) / x_gt_monotonic); + y |= sin(x_gt_monotonic) / x_gt_monotonic; } return y;