From 77440332524a70d4b6df230fc18aa5314e76aebd Mon Sep 17 00:00:00 2001 From: idan shenfeld Date: Mon, 6 Mar 2023 11:12:45 -0500 Subject: [PATCH 1/2] Add SDF body --- core/externals/CMakeLists.txt | 4 + core/externals/sdfgen/CMakeLists.txt | 40 + core/externals/sdfgen/README.md | 25 + core/externals/sdfgen/array1.h | 798 +++++++++++ core/externals/sdfgen/array2.h | 284 ++++ core/externals/sdfgen/array3.h | 276 ++++ core/externals/sdfgen/config.h | 11 + core/externals/sdfgen/config.h.in | 11 + core/externals/sdfgen/hashgrid.h | 141 ++ core/externals/sdfgen/hashtable.h | 252 ++++ core/externals/sdfgen/main.cpp | 183 +++ core/externals/sdfgen/makelevelset3.cpp | 228 +++ core/externals/sdfgen/makelevelset3.h | 20 + core/externals/sdfgen/util.h | 474 +++++++ core/externals/sdfgen/vec.h | 495 +++++++ core/output_build.txt | 336 +++++ core/projects/opengl_viewer/src/viewer.cpp | 4 +- core/projects/redmax/Body/Body.cpp | 23 + core/projects/redmax/Body/Body.h | 5 + core/projects/redmax/Body/BodyCuboid.cpp | 2 + core/projects/redmax/Body/BodyMeshObj.cpp | 40 +- core/projects/redmax/Body/BodyMeshObj.h | 10 +- core/projects/redmax/Body/BodySDFObj.cpp | 738 ++++++++++ core/projects/redmax/Body/BodySDFObj.h | 92 ++ core/projects/redmax/CMakeLists.txt | 2 + .../CollisionDetection/CollisionDetection.cpp | 36 + .../CollisionDetection/CollisionDetection.h | 4 + .../redmax/Force/ForceGeneralSDFContact.cpp | 1245 +++++++++++++++++ .../redmax/Force/ForceGeneralSDFContact.h | 56 + core/projects/redmax/Simulation.cpp | 87 ++ core/projects/redmax/Simulation.h | 3 + .../redmax/Simulation_Constructor.cpp | 47 +- 32 files changed, 5957 insertions(+), 15 deletions(-) create mode 100644 core/externals/sdfgen/CMakeLists.txt create mode 100644 core/externals/sdfgen/README.md create mode 100644 core/externals/sdfgen/array1.h create mode 100644 core/externals/sdfgen/array2.h create mode 100644 core/externals/sdfgen/array3.h create mode 100644 core/externals/sdfgen/config.h create mode 100644 core/externals/sdfgen/config.h.in create mode 100644 core/externals/sdfgen/hashgrid.h create mode 100644 core/externals/sdfgen/hashtable.h create mode 100644 core/externals/sdfgen/main.cpp create mode 100644 core/externals/sdfgen/makelevelset3.cpp create mode 100644 core/externals/sdfgen/makelevelset3.h create mode 100644 core/externals/sdfgen/util.h create mode 100644 core/externals/sdfgen/vec.h create mode 100644 core/output_build.txt create mode 100644 core/projects/redmax/Body/BodySDFObj.cpp create mode 100644 core/projects/redmax/Body/BodySDFObj.h create mode 100644 core/projects/redmax/Force/ForceGeneralSDFContact.cpp create mode 100644 core/projects/redmax/Force/ForceGeneralSDFContact.h diff --git a/core/externals/CMakeLists.txt b/core/externals/CMakeLists.txt index 9f70016..4fbe082 100644 --- a/core/externals/CMakeLists.txt +++ b/core/externals/CMakeLists.txt @@ -15,6 +15,7 @@ set(EXTERNAL_HEADER "${CMAKE_CURRENT_LIST_DIR}/stb" "${CMAKE_CURRENT_LIST_DIR}/pugixml/src" "${CMAKE_CURRENT_LIST_DIR}/tiny_obj_loader" + "${CMAKE_CURRENT_LIST_DIR}/sdfgen" ) include_directories(${EXTERNAL_HEADER}) @@ -28,6 +29,9 @@ add_subdirectory(pugixml) # pybind11 add_subdirectory(pybind11) +# sdfgen +add_subdirectory(sdfgen) + # viewer related if(MSVC AND NOT "${MSVC_VERSION}" LESS 1400) add_definitions( "/MP" ) diff --git a/core/externals/sdfgen/CMakeLists.txt b/core/externals/sdfgen/CMakeLists.txt new file mode 100644 index 0000000..c9e00e0 --- /dev/null +++ b/core/externals/sdfgen/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 2.6) +Project("sdfgen") + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries + +# SET(CMAKE_BUILD_TYPE Coverage) +SET(CMAKE_BUILD_TYPE Release) + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_BINARY_DIR}/bin) + +#These flags might not work on every system, especially the release flags, comment out as needed +set(CMAKE_CXX_FLAGS "-O3") +set(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") +set(CMAKE_CXX_FLAGS_RELEASE "-O3") + +#checks if VTK is available +find_package(VTK QUIET) +if(VTK_FOUND) + set(HAVE_VTK 1) +endif() + +#Generates config.h replacing expressions in config.h.in with actual values +configure_file( + "${CMAKE_CURRENT_SOURCE_DIR}/config.h.in" + "${CMAKE_CURRENT_SOURCE_DIR}/config.h" +) + +# add_executable(${PROJECT_NAME} main.cpp makelevelset3.cpp) +add_library(${PROJECT_NAME} makelevelset3.h makelevelset3.cpp) + +if(VTK_FOUND) + include_directories(${VTK_INCLUDE_DIRS}) + target_link_libraries(${PROJECT_NAME} ${VTK_LIBRARIES}) +endif() \ No newline at end of file diff --git a/core/externals/sdfgen/README.md b/core/externals/sdfgen/README.md new file mode 100644 index 0000000..ed85c98 --- /dev/null +++ b/core/externals/sdfgen/README.md @@ -0,0 +1,25 @@ +# SDFGen +A simple commandline utility to generate grid-based signed distance field (level set) generator from triangle meshes, using code from Robert Bridson's website. + + +The MIT License (MIT) + +Copyright (c) 2015, Christopher Batty + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/core/externals/sdfgen/array1.h b/core/externals/sdfgen/array1.h new file mode 100644 index 0000000..8343bb6 --- /dev/null +++ b/core/externals/sdfgen/array1.h @@ -0,0 +1,798 @@ +#ifndef ARRAY1_H +#define ARRAY1_H + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sdfgen { + +// In this file: +// Array1: a dynamic 1D array for plain-old-data (not objects) +// WrapArray1: a 1D array wrapper around an existing array (perhaps objects, perhaps data) +// For the most part std::vector operations are supported, though for the Wrap version +// note that memory is never allocated/deleted and constructor/destructors are never called +// from within the class, thus only shallow copies can be made and some operations such as +// resize() and push_back() are limited. +// Note: for the most part assertions are done with assert(), not exceptions... + +// gross template hacking to determine if a type is integral or not +struct Array1True {}; +struct Array1False {}; +template struct Array1IsIntegral{ typedef Array1False type; }; // default: no (specializations to yes follow) +template<> struct Array1IsIntegral{ typedef Array1True type; }; +template<> struct Array1IsIntegral{ typedef Array1True type; }; +template<> struct Array1IsIntegral{ typedef Array1True type; }; +template<> struct Array1IsIntegral{ typedef Array1True type; }; +template<> struct Array1IsIntegral{ typedef Array1True type; }; +template<> struct Array1IsIntegral{ typedef Array1True type; }; +template<> struct Array1IsIntegral{ typedef Array1True type; }; +template<> struct Array1IsIntegral{ typedef Array1True type; }; +template<> struct Array1IsIntegral{ typedef Array1True type; }; +template<> struct Array1IsIntegral{ typedef Array1True type; }; +template<> struct Array1IsIntegral{ typedef Array1True type; }; +template<> struct Array1IsIntegral{ typedef Array1True type; }; + +//============================================================================ +template +struct Array1 +{ + // STL-friendly typedefs + + typedef T* iterator; + typedef const T* const_iterator; + typedef unsigned long size_type; + typedef long difference_type; + typedef T& reference; + typedef const T& const_reference; + typedef T value_type; + typedef T* pointer; + typedef const T* const_pointer; + typedef std::reverse_iterator reverse_iterator; + typedef std::reverse_iterator const_reverse_iterator; + + // the actual representation + + unsigned long n; + unsigned long max_n; + T* data; + + // STL vector's interface, with additions, but only valid when used with plain-old-data + + Array1(void) + : n(0), max_n(0), data(0) + {} + + // note: default initial values are zero + Array1(unsigned long n_) + : n(0), max_n(0), data(0) + { + if(n_>ULONG_MAX/sizeof(T)) throw std::bad_alloc(); + data=(T*)std::calloc(n_, sizeof(T)); + if(!data) throw std::bad_alloc(); + n=n_; + max_n=n_; + } + + Array1(unsigned long n_, const T& value) + : n(0), max_n(0), data(0) + { + if(n_>ULONG_MAX/sizeof(T)) throw std::bad_alloc(); + data=(T*)std::calloc(n_, sizeof(T)); + if(!data) throw std::bad_alloc(); + n=n_; + max_n=n_; + for(unsigned long i=0; iULONG_MAX/sizeof(T)) throw std::bad_alloc(); + data=(T*)std::calloc(max_n_, sizeof(T)); + if(!data) throw std::bad_alloc(); + n=n_; + max_n=max_n_; + for(unsigned long i=0; iULONG_MAX/sizeof(T)) throw std::bad_alloc(); + data=(T*)std::calloc(n_, sizeof(T)); + if(!data) throw std::bad_alloc(); + n=n_; + max_n=n_; + assert(data_); + std::memcpy(data, data_, n*sizeof(T)); + } + + Array1(unsigned long n_, const T* data_, unsigned long max_n_) + : n(0), max_n(0), data(0) + { + assert(n_<=max_n_); + if(max_n_>ULONG_MAX/sizeof(T)) throw std::bad_alloc(); + data=(T*)std::calloc(max_n_, sizeof(T)); + if(!data) throw std::bad_alloc(); + max_n=max_n_; + n=n_; + assert(data_); + std::memcpy(data, data_, n*sizeof(T)); + } + + Array1(const Array1 &x) + : n(0), max_n(0), data(0) + { + data=(T*)std::malloc(x.n*sizeof(T)); + if(!data) throw std::bad_alloc(); + n=x.n; + max_n=x.n; + std::memcpy(data, x.data, n*sizeof(T)); + } + + ~Array1(void) + { + std::free(data); +#ifndef NDEBUG + data=0; + n=max_n=0; +#endif + } + + const T& operator[](unsigned long i) const + { return data[i]; } + + T& operator[](unsigned long i) + { return data[i]; } + + // these are range-checked (in debug mode) versions of operator[], like at() + const T& operator()(unsigned long i) const + { + assert(i& operator=(const Array1& x) + { + if(max_n& x) const + { + if(n!=x.n) return false; + for(unsigned long i=0; i& x) const + { + if(n!=x.n) return true; + for(unsigned long i=0; i& x) const + { + for(unsigned long i=0; i(const Array1& x) const + { + for(unsigned long i=0; ix[i]) return true; + else if(x[i]>data[i]) return false; + } + return n>x.n; + } + + bool operator<=(const Array1& x) const + { + for(unsigned long i=0; i=(const Array1& x) const + { + for(unsigned long i=0; ix[i]) return true; + else if(x[i]>data[i]) return false; + } + return n>=x.n; + } + + void add_unique(const T& value) + { + for(unsigned long i=0; imax_n){ + if(num>ULONG_MAX/sizeof(T)) throw std::bad_alloc(); + std::free(data); + data=(T*)std::malloc(num*sizeof(T)); + if(!data) throw std::bad_alloc(); + max_n=num; + } + n=num; + std::memcpy(data, copydata, n*sizeof(T)); + } + + template + void assign(InputIterator first, InputIterator last) + { assign_(first, last, typename Array1IsIntegral::type()); } + + template + void assign_(InputIterator first, InputIterator last, Array1True check) + { fill(first, last); } + + template + void assign_(InputIterator first, InputIterator last, Array1False check) + { + unsigned long i=0; + InputIterator p=first; + for(; p!=last; ++p, ++i){ + if(i==max_n) grow(); + data[i]=*p; + } + n=i; + } + + const T& at(unsigned long i) const + { + assert(i0); + return data[n-1]; + } + + T& back(void) + { + assert(data && n>0); + return data[n-1]; + } + + const T* begin(void) const + { return data; } + + T* begin(void) + { return data; } + + unsigned long capacity(void) const + { return max_n; } + + void clear(void) + { + std::free(data); + data=0; + max_n=0; + n=0; + } + + bool empty(void) const + { return n==0; } + + const T* end(void) const + { return data+n; } + + T* end(void) + { return data+n; } + + void erase(unsigned long index) + { + assert(indexmax_n){ + if(num>ULONG_MAX/sizeof(T)) throw std::bad_alloc(); + std::free(data); + data=(T*)std::malloc(num*sizeof(T)); + if(!data) throw std::bad_alloc(); + max_n=num; + } + n=num; + for(unsigned long i=0; i0); + return *data; + } + + T& front(void) + { + assert(n>0); + return *data; + } + + void grow(void) + { + unsigned long new_size=(max_n*sizeof(T)index; --i) + data[i]=data[i-1]; + data[index]=entry; + } + + unsigned long max_size(void) const + { return ULONG_MAX/sizeof(T); } + + void pop_back(void) + { + assert(n>0); + --n; + } + + void push_back(const T& value) + { + if(n==max_n) grow(); + data[n++]=value; + } + + reverse_iterator rbegin(void) + { return reverse_iterator(end()); } + + const_reverse_iterator rbegin(void) const + { return const_reverse_iterator(end()); } + + reverse_iterator rend(void) + { return reverse_iterator(begin()); } + + const_reverse_iterator rend(void) const + { return const_reverse_iterator(begin()); } + + void reserve(unsigned long r) + { + if(r>ULONG_MAX/sizeof(T)) throw std::bad_alloc(); + T *new_data=(T*)std::realloc(data, r*sizeof(T)); + if(!new_data) throw std::bad_alloc(); + data=new_data; + max_n=r; + } + + void resize(unsigned long n_) + { + if(n_>max_n) reserve(n_); + n=n_; + } + + void resize(unsigned long n_, const T& value) + { + if(n_>max_n) reserve(n_); + if(n& x) + { + std::swap(n, x.n); + std::swap(max_n, x.max_n); + std::swap(data, x.data); + } + + // resize the array to avoid wasted space, without changing contents + // (Note: realloc, at least on some platforms, will not do the trick) + void trim(void) + { + if(n==max_n) return; + T *new_data=(T*)std::malloc(n*sizeof(T)); + if(!new_data) return; + std::memcpy(new_data, data, n*sizeof(T)); + std::free(data); + data=new_data; + max_n=n; + } +}; + +// some common arrays + +typedef Array1 Array1d; +typedef Array1 Array1f; +typedef Array1 Array1ll; +typedef Array1 Array1ull; +typedef Array1 Array1i; +typedef Array1 Array1ui; +typedef Array1 Array1s; +typedef Array1 Array1us; +typedef Array1 Array1c; +typedef Array1 Array1uc; + +//============================================================================ +template +struct WrapArray1 +{ + // STL-friendly typedefs + + typedef T* iterator; + typedef const T* const_iterator; + typedef unsigned long size_type; + typedef long difference_type; + typedef T& reference; + typedef const T& const_reference; + typedef T value_type; + typedef T* pointer; + typedef const T* const_pointer; + typedef std::reverse_iterator reverse_iterator; + typedef std::reverse_iterator const_reverse_iterator; + + // the actual representation + + unsigned long n; + unsigned long max_n; + T* data; + + // most of STL vector's interface, with a few changes + + WrapArray1(void) + : n(0), max_n(0), data(0) + {} + + WrapArray1(unsigned long n_, T* data_) + : n(n_), max_n(n_), data(data_) + { assert(data || max_n==0); } + + WrapArray1(unsigned long n_, T* data_, unsigned long max_n_) + : n(n_), max_n(max_n_), data(data_) + { + assert(n<=max_n); + assert(data || max_n==0); + } + + // Allow for simple shallow copies of existing arrays + // Note that if the underlying arrays change where their data is, the WrapArray may be screwed up + + WrapArray1(Array1& a) + : n(a.n), max_n(a.max_n), data(a.data) + {} + + WrapArray1(std::vector& a) + : n(a.size()), max_n(a.capacity()), data(&a[0]) + {} + + void init(unsigned long n_, T* data_, unsigned long max_n_) + { + assert(n_<=max_n_); + assert(data_ || max_n_==0); + n=n_; + max_n=max_n_; + data=data_; + } + + const T& operator[](unsigned long i) const + { return data[i]; } + + T& operator[](unsigned long i) + { return data[i]; } + + // these are range-checked (in debug mode) versions of operator[], like at() + const T& operator()(unsigned long i) const + { + assert(i& x) const + { + if(n!=x.n) return false; + for(unsigned long i=0; i& x) const + { + if(n!=x.n) return true; + for(unsigned long i=0; i& x) const + { + for(unsigned long i=0; i(const WrapArray1& x) const + { + for(unsigned long i=0; ix[i]) return true; + else if(x[i]>data[i]) return false; + } + return n>x.n; + } + + bool operator<=(const WrapArray1& x) const + { + for(unsigned long i=0; i=(const WrapArray1& x) const + { + for(unsigned long i=0; ix[i]) return true; + else if(x[i]>data[i]) return false; + } + return n>=x.n; + } + + void add_unique(const T& value) + { + for(unsigned long i=0; i + void assign(InputIterator first, InputIterator last) + { assign_(first, last, typename Array1IsIntegral::type()); } + + template + void assign_(InputIterator first, InputIterator last, Array1True check) + { fill(first, last); } + + template + void assign_(InputIterator first, InputIterator last, Array1False check) + { + unsigned long i=0; + InputIterator p=first; + for(; p!=last; ++p, ++i){ + assert(i0); + return data[n-1]; + } + + T& back(void) + { + assert(data && n>0); + return data[n-1]; + } + + const T* begin(void) const + { return data; } + + T* begin(void) + { return data; } + + unsigned long capacity(void) const + { return max_n; } + + void clear(void) + { n=0; } + + bool empty(void) const + { return n==0; } + + const T* end(void) const + { return data+n; } + + T* end(void) + { return data+n; } + + void erase(unsigned long index) + { + assert(index0); + return *data; + } + + T& front(void) + { + assert(n>0); + return *data; + } + + void insert(unsigned long index, const T& entry) + { + assert(index<=n); + push_back(back()); + for(unsigned long i=n-1; i>index; --i) + data[i]=data[i-1]; + data[index]=entry; + } + + unsigned long max_size(void) const + { return max_n; } + + void pop_back(void) + { + assert(n>0); + --n; + } + + void push_back(const T& value) + { + assert(n& x) + { + std::swap(n, x.n); + std::swap(max_n, x.max_n); + std::swap(data, x.data); + } +}; + +// some common arrays + +typedef WrapArray1 WrapArray1d; +typedef WrapArray1 WrapArray1f; +typedef WrapArray1 WrapArray1ll; +typedef WrapArray1 WrapArray1ull; +typedef WrapArray1 WrapArray1i; +typedef WrapArray1 WrapArray1ui; +typedef WrapArray1 WrapArray1s; +typedef WrapArray1 WrapArray1us; +typedef WrapArray1 WrapArray1c; +typedef WrapArray1 WrapArray1uc; + +} + +#endif diff --git a/core/externals/sdfgen/array2.h b/core/externals/sdfgen/array2.h new file mode 100644 index 0000000..d08edd7 --- /dev/null +++ b/core/externals/sdfgen/array2.h @@ -0,0 +1,284 @@ +#ifndef ARRAY2_H +#define ARRAY2_H + +#include "array1.h" +#include +#include +#include + +namespace sdfgen { + +template > +struct Array2 +{ + // STL-friendly typedefs + + typedef typename ArrayT::iterator iterator; + typedef typename ArrayT::const_iterator const_iterator; + typedef typename ArrayT::size_type size_type; + typedef long difference_type; + typedef T& reference; + typedef const T& const_reference; + typedef T value_type; + typedef T* pointer; + typedef const T* const_pointer; + typedef typename ArrayT::reverse_iterator reverse_iterator; + typedef typename ArrayT::const_reverse_iterator const_reverse_iterator; + + // the actual representation + + int ni, nj; + ArrayT a; + + // the interface + + Array2(void) + : ni(0), nj(0) + {} + + Array2(int ni_, int nj_) + : ni(ni_), nj(nj_), a(ni_*nj_) + { assert(ni_>=0 && nj>=0); } + + Array2(int ni_, int nj_, ArrayT& a_) + : ni(ni_), nj(nj_), a(a_) + { assert(ni_>=0 && nj>=0); } + + Array2(int ni_, int nj_, const T& value) + : ni(ni_), nj(nj_), a(ni_*nj_, value) + { assert(ni_>=0 && nj>=0); } + + Array2(int ni_, int nj_, const T& value, size_type max_n_) + : ni(ni_), nj(nj_), a(ni_*nj_, value, max_n_) + { assert(ni_>=0 && nj>=0); } + + Array2(int ni_, int nj_, T* data_) + : ni(ni_), nj(nj_), a(ni_*nj_, data_) + { assert(ni_>=0 && nj>=0); } + + Array2(int ni_, int nj_, T* data_, size_type max_n_) + : ni(ni_), nj(nj_), a(ni_*nj_, data_, max_n_) + { assert(ni_>=0 && nj>=0); } + + template + Array2(Array2& other) + : ni(other.ni), nj(other.nj), a(other.a) + {} + + ~Array2(void) + { +#ifndef NDEBUG + ni=nj=0; +#endif + } + + const T& operator()(int i, int j) const + { + assert(i>=0 && i=0 && j=0 && i=0 && j& x) const + { return ni==x.ni && nj==x.nj && a==x.a; } + + bool operator!=(const Array2& x) const + { return ni!=x.ni || nj!=x.nj || a!=x.a; } + + bool operator<(const Array2& x) const + { + if(nix.ni) return false; + if(njx.nj) return false; + return a(const Array2& x) const + { + if(ni>x.ni) return true; else if(nix.nj) return true; else if(njx.a; + } + + bool operator<=(const Array2& x) const + { + if(nix.ni) return false; + if(njx.nj) return false; + return a<=x.a; + } + + bool operator>=(const Array2& x) const + { + if(ni>x.ni) return true; else if(nix.nj) return true; else if(nj=x.a; + } + + void assign(const T& value) + { a.assign(value); } + + void assign(int ni_, int nj_, const T& value) + { + a.assign(ni_*nj_, value); + ni=ni_; + nj=nj_; + } + + void assign(int ni_, int nj_, const T* copydata) + { + a.assign(ni_*nj_, copydata); + ni=ni_; + nj=nj_; + } + + const T& at(int i, int j) const + { + assert(i>=0 && i=0 && j=0 && i=0 && j=0 && nj_>=0); + a.resize(ni_*nj_); + ni=ni_; + nj=nj_; + } + + void resize(int ni_, int nj_, const T& value) + { + assert(ni_>=0 && nj_>=0); + a.resize(ni_*nj_, value); + ni=ni_; + nj=nj_; + } + + void set_zero(void) + { a.set_zero(); } + + size_type size(void) const + { return a.size(); } + + void swap(Array2& x) + { + std::swap(ni, x.ni); + std::swap(nj, x.nj); + a.swap(x.a); + } + + void trim(void) + { a.trim(); } +}; + +// some common arrays + +typedef Array2 > Array2d; +typedef Array2 > Array2f; +typedef Array2 > Array2ll; +typedef Array2 > Array2ull; +typedef Array2 > Array2i; +typedef Array2 > Array2ui; +typedef Array2 > Array2s; +typedef Array2 > Array2us; +typedef Array2 > Array2c; +typedef Array2 > Array2uc; + +// and wrapped versions + +typedef Array2 > WrapArray2d; +typedef Array2 > WrapArray2f; +typedef Array2 > WrapArray2ll; +typedef Array2 > WrapArray2ull; +typedef Array2 > WrapArray2i; +typedef Array2 > WrapArray2ui; +typedef Array2 > WrapArray2s; +typedef Array2 > WrapArray2us; +typedef Array2 > WrapArray2c; +typedef Array2 > WrapArray2uc; + +} + +#endif diff --git a/core/externals/sdfgen/array3.h b/core/externals/sdfgen/array3.h new file mode 100644 index 0000000..6334d24 --- /dev/null +++ b/core/externals/sdfgen/array3.h @@ -0,0 +1,276 @@ +#ifndef ARRAY3_H +#define ARRAY3_H + +#include "array1.h" +#include +#include +#include + +namespace sdfgen { + +template > +struct Array3 +{ + // STL-friendly typedefs + + typedef typename ArrayT::iterator iterator; + typedef typename ArrayT::const_iterator const_iterator; + typedef typename ArrayT::size_type size_type; + typedef long difference_type; + typedef T& reference; + typedef const T& const_reference; + typedef T value_type; + typedef T* pointer; + typedef const T* const_pointer; + typedef typename ArrayT::reverse_iterator reverse_iterator; + typedef typename ArrayT::const_reverse_iterator const_reverse_iterator; + + // the actual representation + + int ni, nj, nk; + ArrayT a; + + // the interface + + Array3(void) + : ni(0), nj(0), nk(0) + {} + + Array3(int ni_, int nj_, int nk_) + : ni(ni_), nj(nj_), nk(nk_), a(ni_*nj_*nk_) + { assert(ni_>=0 && nj_>=0 && nk_>=0); } + + Array3(int ni_, int nj_, int nk_, ArrayT& a_) + : ni(ni_), nj(nj_), nk(nk_), a(a_) + { assert(ni_>=0 && nj_>=0 && nk_>=0); } + + Array3(int ni_, int nj_, int nk_, const T& value) + : ni(ni_), nj(nj_), nk(nk_), a(ni_*nj_*nk_, value) + { assert(ni_>=0 && nj_>=0 && nk_>=0); } + + Array3(int ni_, int nj_, int nk_, const T& value, size_type max_n_) + : ni(ni_), nj(nj_), nk(nk_), a(ni_*nj_*nk_, value, max_n_) + { assert(ni_>=0 && nj_>=0 && nk_>=0); } + + Array3(int ni_, int nj_, int nk_, T* data_) + : ni(ni_), nj(nj_), nk(nk_), a(ni_*nj_*nk_, data_) + { assert(ni_>=0 && nj_>=0 && nk_>=0); } + + Array3(int ni_, int nj_, int nk_, T* data_, size_type max_n_) + : ni(ni_), nj(nj_), nk(nk_), a(ni_*nj_*nk_, data_, max_n_) + { assert(ni_>=0 && nj_>=0 && nk_>=0); } + + ~Array3(void) + { +#ifndef NDEBUG + ni=nj=0; +#endif + } + + const T& operator()(int i, int j, int k) const + { + assert(i>=0 && i=0 && j=0 && k=0 && i=0 && j=0 && k& x) const + { return ni==x.ni && nj==x.nj && nk==x.nk && a==x.a; } + + bool operator!=(const Array3& x) const + { return ni!=x.ni || nj!=x.nj || nk!=x.nk || a!=x.a; } + + bool operator<(const Array3& x) const + { + if(nix.ni) return false; + if(njx.nj) return false; + if(nkx.nk) return false; + return a(const Array3& x) const + { + if(ni>x.ni) return true; else if(nix.nj) return true; else if(njx.nk) return true; else if(nkx.a; + } + + bool operator<=(const Array3& x) const + { + if(nix.ni) return false; + if(njx.nj) return false; + if(nkx.nk) return false; + return a<=x.a; + } + + bool operator>=(const Array3& x) const + { + if(ni>x.ni) return true; else if(nix.nj) return true; else if(njx.nk) return true; else if(nk=x.a; + } + + void assign(const T& value) + { a.assign(value); } + + void assign(int ni_, int nj_, int nk_, const T& value) + { + a.assign(ni_*nj_*nk_, value); + ni=ni_; + nj=nj_; + nk=nk_; + } + + void assign(int ni_, int nj_, int nk_, const T* copydata) + { + a.assign(ni_*nj_*nk_, copydata); + ni=ni_; + nj=nj_; + nk=nk_; + } + + const T& at(int i, int j, int k) const + { + assert(i>=0 && i=0 && j=0 && k=0 && i=0 && j=0 && k=0 && nj_>=0 && nk_>=0); + a.resize(ni_*nj_*nk_); + ni=ni_; + nj=nj_; + nk=nk_; + } + + void resize(int ni_, int nj_, int nk_, const T& value) + { + assert(ni_>=0 && nj_>=0 && nk_>=0); + a.resize(ni_*nj_*nk_, value); + ni=ni_; + nj=nj_; + nk=nk_; + } + + void set_zero(void) + { a.set_zero(); } + + size_type size(void) const + { return a.size(); } + + void swap(Array3& x) + { + std::swap(ni, x.ni); + std::swap(nj, x.nj); + std::swap(nk, x.nk); + a.swap(x.a); + } + + void trim(void) + { a.trim(); } +}; + +// some common arrays + +typedef Array3 > Array3d; +typedef Array3 > Array3f; +typedef Array3 > Array3ll; +typedef Array3 > Array3ull; +typedef Array3 > Array3i; +typedef Array3 > Array3ui; +typedef Array3 > Array3s; +typedef Array3 > Array3us; +typedef Array3 > Array3c; +typedef Array3 > Array3uc; + +} + +#endif diff --git a/core/externals/sdfgen/config.h b/core/externals/sdfgen/config.h new file mode 100644 index 0000000..b010ec1 --- /dev/null +++ b/core/externals/sdfgen/config.h @@ -0,0 +1,11 @@ +#ifndef SDFGEN_CONFIG_H +#define SDFGEN_CONFIG_H + +/* + * Configuration Header for SDFGEN + */ + +/// Configured libraries +/* #undef HAVE_VTK */ + +#endif //SDFGEN_CONFIG_H diff --git a/core/externals/sdfgen/config.h.in b/core/externals/sdfgen/config.h.in new file mode 100644 index 0000000..a120320 --- /dev/null +++ b/core/externals/sdfgen/config.h.in @@ -0,0 +1,11 @@ +#ifndef SDFGEN_CONFIG_H +#define SDFGEN_CONFIG_H + +/* + * Configuration Header for SDFGEN + */ + +/// Configured libraries +#cmakedefine HAVE_VTK + +#endif //SDFGEN_CONFIG_H diff --git a/core/externals/sdfgen/hashgrid.h b/core/externals/sdfgen/hashgrid.h new file mode 100644 index 0000000..7e5fc25 --- /dev/null +++ b/core/externals/sdfgen/hashgrid.h @@ -0,0 +1,141 @@ +#ifndef HASHGRID_H +#define HASHGRID_H + +#include "hashtable.h" +#include "vec.h" + +namespace sdfgen { + +//========================================================= first do 2D ============================ + +template +struct HashGrid2 +{ + double dx, overdx; // side-length of a grid cell and its reciprocal + HashTable grid; + + explicit HashGrid2(double dx_=1, int expected_size=512) + : dx(dx_), overdx(1/dx_), grid(expected_size) + {} + + // only do this with an empty grid + void set_grid_size(double dx_) + { assert(size()==0); dx=dx_; overdx=1/dx; } + + void add_point(const Vec2d &x, const DataType &datum) + { grid.add(round(x*overdx), datum); } + + void delete_point(const Vec2d &x, const DataType &datum) + { grid.delete_entry(round(x*overdx), datum); } + + void add_box(const Vec2d &xmin, const Vec2d &xmax, const DataType &datum) + { + Vec2i imin=round(xmin*overdx), imax=round(xmax*overdx); + for(int j=imin[1]; j<=imax[1]; ++j) for(int i=imin[0]; i<=imax[0]; ++i) + grid.add(Vec2i(i,j), datum); + } + + void delete_box(const Vec2d &xmin, const Vec2d &xmax, const DataType &datum) + { + Vec2i imin=round(xmin*overdx), imax=round(xmax*overdx); + for(int j=imin[1]; j<=imax[1]; ++j) for(int i=imin[0]; i<=imax[0]; ++i) + grid.delete_entry(Vec2i(i,j), datum); + } + + unsigned int size(void) const + { return grid.size(); } + + void clear(void) + { grid.clear(); } + + void reserve(unsigned int expected_size) + { grid.reserve(expected_size); } + + bool find_first_point(const Vec2d &x, DataType &datum) const + { return grid.get_entry(round(x*overdx), datum); } + + bool find_point(const Vec2d &x, std::vector &data_list) const + { + data_list.resize(0); + grid.append_all_entries(round(x*overdx), data_list); + return data_list.size()>0; + } + + bool find_box(const Vec2d &xmin, const Vec2d &xmax, std::vector &data_list) const + { + data_list.resize(0); + Vec2i imin=round(xmin*overdx), imax=round(xmax*overdx); + for(int j=imin[1]; j<=imax[1]; ++j) for(int i=imin[0]; i<=imax[0]; ++i) + grid.append_all_entries(Vec2i(i,j), data_list); + return data_list.size()>0; + } +}; + +//==================================== and now in 3D ================================================= + +template +struct HashGrid3 +{ + double dx, overdx; // side-length of a grid cell and its reciprocal + HashTable grid; + + explicit HashGrid3(double dx_=1, int expected_size=512) + : dx(dx_), overdx(1/dx_), grid(expected_size) + {} + + // only do this with an empty grid + void set_grid_size(double dx_) + { assert(size()==0); dx=dx_; overdx=1/dx; } + + void add_point(const Vec3d &x, const DataType &datum) + { grid.add(round(x*overdx), datum); } + + void delete_point(const Vec3d &x, const DataType &datum) + { grid.delete_entry(round(x*overdx), datum); } + + void add_box(const Vec3d &xmin, const Vec3d &xmax, const DataType &datum) + { + Vec3i imin=round(xmin*overdx), imax=round(xmax*overdx); + for(int k=imin[2]; k<=imax[2]; ++k) for(int j=imin[1]; j<=imax[1]; ++j) for(int i=imin[0]; i<=imax[0]; ++i) + grid.add(Vec3i(i,j,k), datum); + } + + void delete_box(const Vec3d &xmin, const Vec3d &xmax, const DataType &datum) + { + Vec3i imin=round(xmin*overdx), imax=round(xmax*overdx); + for(int k=imin[2]; k<=imax[2]; ++k) for(int j=imin[1]; j<=imax[1]; ++j) for(int i=imin[0]; i<=imax[0]; ++i) + grid.delete_entry(Vec3i(i,j,k), datum); + } + + unsigned int size(void) const + { return grid.size(); } + + void clear(void) + { grid.clear(); } + + void reserve(unsigned int expected_size) + { grid.reserve(expected_size); } + + bool find_first_point(const Vec3d &x, DataType &index) const + { return grid.get_entry(round(x*overdx), index); } + + bool find_point(const Vec3d &x, std::vector &data_list) const + { + data_list.resize(0); + grid.append_all_entries(round(x*overdx), data_list); + return data_list.size()>0; + } + + bool find_box(const Vec3d &xmin, const Vec3d &xmax, std::vector &data_list) const + { + data_list.resize(0); + Vec3i imin=round(xmin*overdx), imax=round(xmax*overdx); + for(int k=imin[2]; k<=imax[2]; ++k) for(int j=imin[1]; j<=imax[1]; ++j) for(int i=imin[0]; i<=imax[0]; ++i) + grid.append_all_entries(Vec3i(i, j, k), data_list); + return data_list.size()>0; + } +}; + +} + +#endif diff --git a/core/externals/sdfgen/hashtable.h b/core/externals/sdfgen/hashtable.h new file mode 100644 index 0000000..c81fb59 --- /dev/null +++ b/core/externals/sdfgen/hashtable.h @@ -0,0 +1,252 @@ +#ifndef HASHTABLE_H +#define HASHTABLE_H + +#include +#include +#include + +namespace sdfgen { + +template +struct HashEntry +{ + Key key; + Data data; + int next; +}; + +// a useful core hash function +inline unsigned int hash(unsigned int k) +{ return k*2654435769u; } + +// default hash function object +struct DefaultHashFunction +{ + template + unsigned int operator() (const Key &k) const { return hash(k); } +}; + +struct equal +{ + template + bool operator() (const T &a, const T &b) const { return a==b; } +}; + +template +struct HashTable +{ + unsigned int table_rank; + unsigned int table_bits; + std::vector table; + unsigned int num_entries; + std::vector > pool; + int free_list; + const HashFunction hash_function; + const KeyEqual key_equal; + + explicit HashTable(unsigned int expected_size=64) + : hash_function(HashFunction()), key_equal(KeyEqual()) + { init(expected_size); } + + explicit HashTable(const HashFunction &hf, unsigned int expected_size=64) + : hash_function(hf), key_equal(KeyEqual()) + { init(expected_size); } + + void init(unsigned int expected_size) + { + unsigned int i; + num_entries=0; + table_rank=4; + while(1u<& data_return) const + { + unsigned int t=hash_function(k)&table_bits; + int i=table[t]; + while(i!=-1){ + if(key_equal(k, pool[i].key)) data_return.push_back(pool[i].data); + i=pool[i].next; + } + } + + Data &operator() (const Key &k, const Data &missing_data) + { + unsigned int t=hash_function(k)&table_bits; + int i=table[t]; + while(i!=-1){ + if(key_equal(k, pool[i].key)) + return pool[i].data; + i=pool[i].next; + } + add(k, missing_data); // note - this could cause the table to be resized, and t made out-of-date + return pool[table[hash_function(k)&table_bits]].data; // we know that add() puts it here! + } + + const Data &operator() (const Key &k, const Data &missing_data) const + { + unsigned int t=hash_function(k)&table_bits; + int i=table[t]; + while(i!=-1){ + if(key_equal(k, pool[i].key)) + return pool[i].data; + i=pool[i].next; + } + return missing_data; + } + + void output_statistics() const + { + std::vector lengthcount(table.size()); + unsigned int t; + int total=0; + for(t=0; t0){ + std::cout<<"length "<0) + maxlength=t; + std::cout<<"longest list: "< + #include + #include + #include + #include +#endif + + +#include +#include +#include +#include + +using namespace sdfgen; + +int main(int argc, char* argv[]) { + + if(argc != 4) { + std::cout << "SDFGen - A utility for converting closed oriented triangle meshes into grid-based signed distance fields.\n"; + std::cout << "\nThe output file format is:"; + std::cout << " \n"; + std::cout << " \n"; + std::cout << "\n"; + std::cout << " [...]\n\n"; + + std::cout << "(ni,nj,nk) are the integer dimensions of the resulting distance field.\n"; + std::cout << "(origin_x,origin_y,origin_z) is the 3D position of the grid origin.\n"; + std::cout << " is the grid spacing.\n\n"; + std::cout << " are the signed distance data values, in ascending order of i, then j, then k.\n"; + + std::cout << "The output filename will match that of the input, with the OBJ suffix replaced with SDF.\n\n"; + + std::cout << "Usage: SDFGen \n\n"; + std::cout << "Where:\n"; + std::cout << "\t specifies a Wavefront OBJ (text) file representing a *triangle* mesh (no quad or poly meshes allowed). File must use the suffix \".obj\".\n"; + std::cout << "\t specifies the length of grid cell in the resulting distance field.\n"; + std::cout << "\t specifies the number of cells worth of padding between the object bound box and the boundary of the distance field grid. Minimum is 1.\n\n"; + + exit(-1); + } + + std::string filename(argv[1]); + if(filename.size() < 5 || filename.substr(filename.size()-4) != std::string(".obj")) { + std::cerr << "Error: Expected OBJ file with filename of the form .obj.\n"; + exit(-1); + } + + std::stringstream arg2(argv[2]); + float dx; + arg2 >> dx; + + std::stringstream arg3(argv[3]); + int padding; + arg3 >> padding; + + // if(padding < 1) padding = 1; + if (padding < 0) padding = 0; + //start with a massive inside out bound box. + Vec3f min_box(std::numeric_limits::max(),std::numeric_limits::max(),std::numeric_limits::max()), + max_box(-std::numeric_limits::max(),-std::numeric_limits::max(),-std::numeric_limits::max()); + + std::cout << "Reading data.\n"; + + std::ifstream infile(argv[1]); + if(!infile) { + std::cerr << "Failed to open. Terminating.\n"; + exit(-1); + } + + int ignored_lines = 0; + std::string line; + std::vector vertList; + std::vector faceList; + while(!infile.eof()) { + std::getline(infile, line); + + //.obj files sometimes contain vertex normals indicated by "vn" + if(line.substr(0,1) == std::string("v") && line.substr(0,2) != std::string("vn")){ + std::stringstream data(line); + char c; + Vec3f point; + data >> c >> point[0] >> point[1] >> point[2]; + vertList.push_back(point); + update_minmax(point, min_box, max_box); + } + else if(line.substr(0,1) == std::string("f")) { + std::stringstream data(line); + char c; + int v0,v1,v2; + data >> c >> v0 >> v1 >> v2; + faceList.push_back(Vec3ui(v0-1,v1-1,v2-1)); + } + else if( line.substr(0,2) == std::string("vn") ){ + std::cerr << "Obj-loader is not able to parse vertex normals, please strip them from the input file. \n"; + exit(-2); + } + else { + ++ignored_lines; + } + } + infile.close(); + + if(ignored_lines > 0) + std::cout << "Warning: " << ignored_lines << " lines were ignored since they did not contain faces or vertices.\n"; + + std::cout << "Read in " << vertList.size() << " vertices and " << faceList.size() << " faces." << std::endl; + + //Add padding around the box. + Vec3f unit(1,1,1); + min_box -= padding*dx*unit; + max_box += padding*dx*unit; + Vec3ui sizes = Vec3ui((max_box - min_box + Vec3f(0.5 * dx))/dx) + Vec3ui(1,1,1); + + std::cout << "Bound box size: (" << min_box << ") to (" << max_box << ") with dimensions " << sizes << "." << std::endl; + + std::cout << "Computing signed distance field.\n"; + Array3f phi_grid; + make_level_set3(faceList, vertList, min_box, dx, sizes[0], sizes[1], sizes[2], phi_grid); + + std::string outname; + + #ifdef HAVE_VTK + // If compiled with VTK, we can directly output a volumetric image format instead + //Very hackily strip off file suffix. + outname = filename.substr(0, filename.size()-4) + std::string(".vti"); + std::cout << "Writing results to: " << outname << "\n"; + vtkSmartPointer output_volume = vtkSmartPointer::New(); + + output_volume->SetDimensions(phi_grid.ni ,phi_grid.nj ,phi_grid.nk); + output_volume->SetOrigin( phi_grid.ni*dx/2, phi_grid.nj*dx/2,phi_grid.nk*dx/2); + output_volume->SetSpacing(dx,dx,dx); + + vtkSmartPointer distance = vtkSmartPointer::New(); + + distance->SetNumberOfTuples(phi_grid.a.size()); + + output_volume->GetPointData()->AddArray(distance); + distance->SetName("Distance"); + + for(unsigned int i = 0; i < phi_grid.a.size(); ++i) { + distance->SetValue(i, phi_grid.a[i]); + } + + vtkSmartPointer writer = + vtkSmartPointer::New(); + writer->SetFileName(outname.c_str()); + + #if VTK_MAJOR_VERSION <= 5 + writer->SetInput(output_volume); + #else + writer->SetInputData(output_volume); + #endif + writer->Write(); + + #else + // if VTK support is missing, default back to the original ascii file-dump. + //Very hackily strip off file suffix. + outname = filename.substr(0, filename.size()-4) + std::string(".sdf"); + std::cout << "Writing results to: " << outname << "\n"; + + std::ofstream outfile( outname.c_str()); + outfile << phi_grid.ni << " " << phi_grid.nj << " " << phi_grid.nk << std::endl; + outfile << min_box[0] << " " << min_box[1] << " " << min_box[2] << std::endl; + outfile << dx << std::endl; + for(unsigned int i = 0; i < phi_grid.a.size(); ++i) { + outfile << phi_grid.a[i] << std::endl; + } + outfile.close(); + #endif + + std::cout << "Processing complete.\n"; + +return 0; +} diff --git a/core/externals/sdfgen/makelevelset3.cpp b/core/externals/sdfgen/makelevelset3.cpp new file mode 100644 index 0000000..e092b05 --- /dev/null +++ b/core/externals/sdfgen/makelevelset3.cpp @@ -0,0 +1,228 @@ +#include "makelevelset3.h" + +namespace sdfgen { + +// find the closest point of x0 is from segment x1-x2 +static Vec3f point_segment_closest_point(const Vec3f &x0, const Vec3f &x1, const Vec3f &x2) +{ + Vec3f dx(x2-x1); + double m2=mag2(dx); + // find parameter value of closest point on segment + float s12=(float)(dot(x2-x0, dx)/m2); + if(s12<0){ + s12=0; + }else if(s12>1){ + s12=1; + } + return s12*x1+(1-s12)*x2; +} + +// find distance x0 is from segment x1-x2 +static float point_segment_distance(const Vec3f &x0, const Vec3f &x1, const Vec3f &x2) +{ + return dist(x0, point_segment_closest_point(x0, x1, x2)); +} + +// find distance x0 is from triangle x1-x2-x3 +static float point_triangle_distance(const Vec3f &x0, const Vec3f &x1, const Vec3f &x2, const Vec3f &x3) +{ + // first find barycentric coordinates of closest point on infinite plane + Vec3f x13(x1-x3), x23(x2-x3), x03(x0-x3); + float m13=mag2(x13), m23=mag2(x23), d=dot(x13,x23); + float invdet=1.f/max(m13*m23-d*d,1e-30f); + float a=dot(x13,x03), b=dot(x23,x03); + // the barycentric coordinates themselves + float w23=invdet*(m23*a-d*b); + float w31=invdet*(m13*b-d*a); + float w12=1-w23-w31; + if(w23>=0 && w31>=0 && w12>=0){ // if we're inside the triangle + return dist(x0, w23*x1+w31*x2+w12*x3); + }else{ // we have to clamp to one of the edges + if(w23>0) // this rules out edge 2-3 for us + return min(point_segment_distance(x0,x1,x2), point_segment_distance(x0,x1,x3)); + else if(w31>0) // this rules out edge 1-3 + return min(point_segment_distance(x0,x1,x2), point_segment_distance(x0,x2,x3)); + else // w12 must be >0, ruling out edge 1-2 + return min(point_segment_distance(x0,x1,x3), point_segment_distance(x0,x2,x3)); + } +} + +// find closest point x0 is from triangle x1-x2-x3 +static Vec3f point_triangle_closest_point(const Vec3f &x0, const Vec3f &x1, const Vec3f &x2, const Vec3f &x3) +{ + // first find barycentric coordinates of closest point on infinite plane + Vec3f x13(x1-x3), x23(x2-x3), x03(x0-x3); + float m13=mag2(x13), m23=mag2(x23), d=dot(x13,x23); + float invdet=1.f/max(m13*m23-d*d,1e-30f); + float a=dot(x13,x03), b=dot(x23,x03); + // the barycentric coordinates themselves + float w23=invdet*(m23*a-d*b); + float w31=invdet*(m13*b-d*a); + float w12=1-w23-w31; + if(w23>=0 && w31>=0 && w12>=0){ // if we're inside the triangle + return w23*x1+w31*x2+w12*x3; + }else{ // we have to clamp to one of the edges + if(w23>0) { // this rules out edge 2-3 for us + Vec3f p12=point_segment_closest_point(x0,x1,x2), p13=point_segment_closest_point(x0,x1,x3); + float d12=dist(x0,p12), d13=dist(x0,p13); + if(d120) { // this rules out edge 1-3 + Vec3f p12=point_segment_closest_point(x0,x1,x2), p23=point_segment_closest_point(x0,x2,x3); + float d12=dist(x0,p12), d23=dist(x0,p23); + if(d120, ruling out edge 1-2 + Vec3f p13=point_segment_closest_point(x0,x1,x3), p23=point_segment_closest_point(x0,x2,x3); + float d13=dist(x0,p13), d23=dist(x0,p23); + if(d13 &tri, const std::vector &x, + Array3f &phi, Array3i &closest_tri, + const Vec3f &gx, int i0, int j0, int k0, int i1, int j1, int k1) +{ + if(closest_tri(i1,j1,k1)>=0){ + unsigned int p, q, r; assign(tri[closest_tri(i1,j1,k1)], p, q, r); + float d=point_triangle_distance(gx, x[p], x[q], x[r]); + if(d &tri, const std::vector &x, + Array3f &phi, Array3i &closest_tri, const Vec3f &origin, float dx, float dy, float dz, + int di, int dj, int dk) +{ + int i0, i1; + if(di>0){ i0=1; i1=phi.ni; } + else{ i0=phi.ni-2; i1=-1; } + int j0, j1; + if(dj>0){ j0=1; j1=phi.nj; } + else{ j0=phi.nj-2; j1=-1; } + int k0, k1; + if(dk>0){ k0=1; k1=phi.nk; } + else{ k0=phi.nk-2; k1=-1; } + for(int k=k0; k!=k1; k+=dk) for(int j=j0; j!=j1; j+=dj) for(int i=i0; i!=i1; i+=di){ + Vec3f gx(i*dx+origin[0], j*dy+origin[1], k*dz+origin[2]); + check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i-di, j, k); + check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i, j-dj, k); + check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i-di, j-dj, k); + check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i, j, k-dk); + check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i-di, j, k-dk); + check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i, j-dj, k-dk); + check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i-di, j-dj, k-dk); + } +} + +// calculate twice signed area of triangle (0,0)-(x1,y1)-(x2,y2) +// return an SOS-determined sign (-1, +1, or 0 only if it's a truly degenerate triangle) +static int orientation(double x1, double y1, double x2, double y2, double &twice_signed_area) +{ + twice_signed_area=y1*x2-x1*y2; + if(twice_signed_area>0) return 1; + else if(twice_signed_area<0) return -1; + else if(y2>y1) return 1; + else if(y2x2) return 1; + else if(x1 &tri, const std::vector &x, + const Vec3f &origin, float dx, float dy, float dz, int ni, int nj, int nk, + Array3f &phi, const int exact_band) +{ + phi.resize(ni, nj, nk); + phi.assign((ni+nj+nk)*(dx+dy+dz)); // upper bound on distance + Array3i closest_tri(ni, nj, nk, -1); + Array3i intersection_count(ni, nj, nk, 0); // intersection_count(i,j,k) is # of tri intersections in (i-1,i]x{j}x{k} + // we begin by initializing distances near the mesh, and figuring out intersection counts + Vec3f ijkmin, ijkmax; + for(unsigned int t=0; t &tri, const std::vector &x, + const Vec3f &origin, float dx, float dy, float dz, int nx, int ny, int nz, + Array3f &phi, const int exact_band=1); + +} + +#endif diff --git a/core/externals/sdfgen/util.h b/core/externals/sdfgen/util.h new file mode 100644 index 0000000..8fb9b23 --- /dev/null +++ b/core/externals/sdfgen/util.h @@ -0,0 +1,474 @@ +#ifndef UTIL_H +#define UTIL_H + +#include +#include +#include +#include + +namespace sdfgen { + +#ifndef M_PI +const double M_PI = 3.1415926535897932384626433832795; +#endif + +#ifdef WIN32 +#undef min +#undef max +#endif + +using std::min; +using std::max; +using std::swap; + +template +inline T sqr(const T& x) +{ return x*x; } + +template +inline T cube(const T& x) +{ return x*x*x; } + +template +inline T min(T a1, T a2, T a3) +{ return min(a1, min(a2, a3)); } + +template +inline T min(T a1, T a2, T a3, T a4) +{ return min(min(a1, a2), min(a3, a4)); } + +template +inline T min(T a1, T a2, T a3, T a4, T a5) +{ return min(min(a1, a2), min(a3, a4), a5); } + +template +inline T min(T a1, T a2, T a3, T a4, T a5, T a6) +{ return min(min(a1, a2), min(a3, a4), min(a5, a6)); } + +template +inline T max(T a1, T a2, T a3) +{ return max(a1, max(a2, a3)); } + +template +inline T max(T a1, T a2, T a3, T a4) +{ return max(max(a1, a2), max(a3, a4)); } + +template +inline T max(T a1, T a2, T a3, T a4, T a5) +{ return max(max(a1, a2), max(a3, a4), a5); } + +template +inline T max(T a1, T a2, T a3, T a4, T a5, T a6) +{ return max(max(a1, a2), max(a3, a4), max(a5, a6)); } + +template +inline void minmax(T a1, T a2, T& amin, T& amax) +{ + if(a1 +inline void minmax(T a1, T a2, T a3, T& amin, T& amax) +{ + if(a1 +inline void minmax(T a1, T a2, T a3, T a4, T& amin, T& amax) +{ + if(a1 +inline void minmax(T a1, T a2, T a3, T a4, T a5, T& amin, T& amax) +{ + //@@@ the logic could be shortcircuited a lot! + amin=min(a1,a2,a3,a4,a5); + amax=max(a1,a2,a3,a4,a5); +} + +template +inline void minmax(T a1, T a2, T a3, T a4, T a5, T a6, T& amin, T& amax) +{ + //@@@ the logic could be shortcircuited a lot! + amin=min(a1,a2,a3,a4,a5,a6); + amax=max(a1,a2,a3,a4,a5,a6); +} + +template +inline void update_minmax(T a1, T& amin, T& amax) +{ + if(a1amax) amax=a1; +} + +template +inline void sort(T &a, T &b, T &c) +{ + T temp; + if(a +inline T clamp(T a, T lower, T upper) +{ + if(aupper) return upper; + else return a; +} + +// only makes sense with T=float or double +template +inline T smooth_step(T r) +{ + if(r<0) return 0; + else if(r>1) return 1; + return r*r*r*(10+r*(-15+r*6)); +} + +// only makes sense with T=float or double +template +inline T smooth_step(T r, T r_lower, T r_upper, T value_lower, T value_upper) +{ return value_lower + smooth_step((r-r_lower)/(r_upper-r_lower)) * (value_upper-value_lower); } + +// only makes sense with T=float or double +template +inline T ramp(T r) +{ return smooth_step((r+1)/2)*2-1; } + +#ifdef WIN32 +inline int lround(double x) +{ + if(x>0) + return (x-floor(x)<0.5) ? (int)floor(x) : (int)ceil(x); + else + return (x-floor(x)<=0.5) ? (int)floor(x) : (int)ceil(x); +} + +inline double remainder(double x, double y) +{ + return x-std::floor(x/y+0.5)*y; +} +#endif + +inline unsigned int round_up_to_power_of_two(unsigned int n) +{ + int exponent=0; + --n; + while(n){ + ++exponent; + n>>=1; + } + return 1<1){ + ++exponent; + n>>=1; + } + return 1<>16); + i*=2654435769u; + i^=(i>>16); + i*=2654435769u; + return i; +} + +// the inverse of randhash +inline unsigned int unhash(unsigned int h) +{ + h*=340573321u; + h^=(h>>16); + h*=340573321u; + h^=(h>>16); + h*=340573321u; + h^=0xA3C59AC3u; + return h; +} + +// returns repeatable stateless pseudo-random number in [0,1] +inline double randhashd(unsigned int seed) +{ return randhash(seed)/(double)UINT_MAX; } +inline float randhashf(unsigned int seed) +{ return randhash(seed)/(float)UINT_MAX; } + +// returns repeatable stateless pseudo-random number in [a,b] +inline double randhashd(unsigned int seed, double a, double b) +{ return (b-a)*randhash(seed)/(double)UINT_MAX + a; } +inline float randhashf(unsigned int seed, float a, float b) +{ return ( (b-a)*randhash(seed)/(float)UINT_MAX + a); } + +inline int intlog2(int x) +{ + int exp=-1; + while(x){ + x>>=1; + ++exp; + } + return exp; +} + +template +inline void get_barycentric(T x, int& i, T& f, int i_low, int i_high) +{ + T s=std::floor(x); + i=(int)s; + if(ii_high-2){ + i=i_high-2; + f=1; + }else + f=(T)(x-s); +} + +template +inline S lerp(const S& value0, const S& value1, T f) +{ return (1-f)*value0 + f*value1; } + +template +inline S bilerp(const S& v00, const S& v10, + const S& v01, const S& v11, + T fx, T fy) +{ + return lerp(lerp(v00, v10, fx), + lerp(v01, v11, fx), + fy); +} + +template +inline S trilerp(const S& v000, const S& v100, + const S& v010, const S& v110, + const S& v001, const S& v101, + const S& v011, const S& v111, + T fx, T fy, T fz) +{ + return lerp(bilerp(v000, v100, v010, v110, fx, fy), + bilerp(v001, v101, v011, v111, fx, fy), + fz); +} + +template +inline S quadlerp(const S& v0000, const S& v1000, + const S& v0100, const S& v1100, + const S& v0010, const S& v1010, + const S& v0110, const S& v1110, + const S& v0001, const S& v1001, + const S& v0101, const S& v1101, + const S& v0011, const S& v1011, + const S& v0111, const S& v1111, + T fx, T fy, T fz, T ft) +{ + return lerp(trilerp(v0000, v1000, v0100, v1100, v0010, v1010, v0110, v1110, fx, fy, fz), + trilerp(v0001, v1001, v0101, v1101, v0011, v1011, v0111, v1111, fx, fy, fz), + ft); +} + +// f should be between 0 and 1, with f=0.5 corresponding to balanced weighting between w0 and w2 +template +inline void quadratic_bspline_weights(T f, T& w0, T& w1, T& w2) +{ + w0=T(0.5)*sqr(f-1); + w1=T(0.75)-sqr(f-T(0.5));; + w2=T(0.5)*sqr(f); +} + +// f should be between 0 and 1 +template +inline void cubic_interp_weights(T f, T& wneg1, T& w0, T& w1, T& w2) +{ + T f2(f*f), f3(f2*f); + wneg1=-T(1./3)*f+T(1./2)*f2-T(1./6)*f3; + w0=1-f2+T(1./2)*(f3-f); + w1=f+T(1./2)*(f2-f3); + w2=T(1./6)*(f3-f); +} + +template +inline S cubic_interp(const S& value_neg1, const S& value0, const S& value1, const S& value2, T f) +{ + T wneg1, w0, w1, w2; + cubic_interp_weights(f, wneg1, w0, w1, w2); + return wneg1*value_neg1 + w0*value0 + w1*value1 + w2*value2; +} + +template +void zero(std::vector& v) +{ for(int i=(int)v.size()-1; i>=0; --i) v[i]=0; } + +template +T abs_max(const std::vector& v) +{ + T m=0; + for(int i=(int)v.size()-1; i>=0; --i){ + if(std::fabs(v[i])>m) + m=std::fabs(v[i]); + } + return m; +} + +template +bool contains(const std::vector& a, T e) +{ + for(unsigned int i=0; i +void add_unique(std::vector& a, T e) +{ + for(unsigned int i=0; i +void insert(std::vector& a, unsigned int index, T e) +{ + a.push_back(a.back()); + for(unsigned int i=(unsigned int)a.size()-1; i>index; --i) + a[i]=a[i-1]; + a[index]=e; +} + +template +void erase(std::vector& a, unsigned int index) +{ + for(unsigned int i=index; i +void erase_swap(std::vector& a, unsigned int index) +{ + for(unsigned int i=index; i +void erase_unordered(std::vector& a, unsigned int index) +{ + a[index]=a.back(); + a.pop_back(); +} + +template +void erase_unordered_swap(std::vector& a, unsigned int index) +{ + swap(a[index], a.back()); + a.pop_back(); +} + +template +void find_and_erase_unordered(std::vector& a, const T& doomed_element) +{ + for(unsigned int i=0; i +void replace_once(std::vector& a, const T& old_element, const T& new_element) +{ + for(unsigned int i=0; i +void write_matlab(std::ostream& output, const std::vector& a, const char *variable_name, bool column_vector=true, int significant_digits=18) +{ + output< +#include +#include +#include "util.h" + +// Defines a thin wrapper around fixed size C-style arrays, using template parameters, +// which is useful for dealing with vectors of different dimensions. +// For example, float[3] is equivalent to Vec<3,float>. +// Entries in the vector are accessed with the overloaded [] operator, so +// for example if x is a Vec<3,float>, then the middle entry is x[1]. +// For convenience, there are a number of typedefs for abbreviation: +// Vec<3,float> -> Vec3f +// Vec<2,int> -> Vec2i +// and so on. +// Arithmetic operators are appropriately overloaded, and functions are defined +// for additional operations (such as dot-products, norms, cross-products, etc.) + +namespace sdfgen { + +template +struct Vec +{ + T v[N]; + + Vec(void) + {} + + explicit Vec(T value_for_all) + { for(unsigned int i=0; i + explicit Vec(const S *source) + { for(unsigned int i=0; i + explicit Vec(const Vec& source) + { for(unsigned int i=0; i(T v0, T v1) + { + assert(N==2); + v[0]=v0; v[1]=v1; + } + + Vec(T v0, T v1, T v2) + { + assert(N==3); + v[0]=v0; v[1]=v1; v[2]=v2; + } + + Vec(T v0, T v1, T v2, T v3) + { + assert(N==4); + v[0]=v0; v[1]=v1; v[2]=v2; v[3]=v3; + } + + Vec(T v0, T v1, T v2, T v3, T v4) + { + assert(N==5); + v[0]=v0; v[1]=v1; v[2]=v2; v[3]=v3; v[4]=v4; + } + + Vec(T v0, T v1, T v2, T v3, T v4, T v5) + { + assert(N==6); + v[0]=v0; v[1]=v1; v[2]=v2; v[3]=v3; v[4]=v4; v[5]=v5; + } + + T &operator[](int index) + { + assert(0<=index && (unsigned int)index operator+=(const Vec &w) + { + for(unsigned int i=0; i operator+(const Vec &w) const + { + Vec sum(*this); + sum+=w; + return sum; + } + + Vec operator-=(const Vec &w) + { + for(unsigned int i=0; i operator-(void) const // unary minus + { + Vec negative; + for(unsigned int i=0; i operator-(const Vec &w) const // (binary) subtraction + { + Vec diff(*this); + diff-=w; + return diff; + } + + Vec operator*=(T a) + { + for(unsigned int i=0; i operator*(T a) const + { + Vec w(*this); + w*=a; + return w; + } + + Vec operator*=(const Vec &w) + { + for(unsigned int i=0; i operator*(const Vec &w) const + { + Vec componentwise_product; + for(unsigned int i=0; i operator/=(T a) + { + for(unsigned int i=0; i operator/(T a) const + { + Vec w(*this); + w/=a; + return w; + } + + Vec operator/=(const Vec &w) + { + for(unsigned int i=0; i operator/(const Vec &w) const + { + Vec componentwise_division; + for(unsigned int i=0; i Vec2d; +typedef Vec<2,float> Vec2f; +typedef Vec<2,int> Vec2i; +typedef Vec<2,unsigned int> Vec2ui; +typedef Vec<2,short> Vec2s; +typedef Vec<2,unsigned short> Vec2us; +typedef Vec<2,char> Vec2c; +typedef Vec<2,unsigned char> Vec2uc; + +typedef Vec<3,double> Vec3d; +typedef Vec<3,float> Vec3f; +typedef Vec<3,int> Vec3i; +typedef Vec<3,unsigned int> Vec3ui; +typedef Vec<3,short> Vec3s; +typedef Vec<3,unsigned short> Vec3us; +typedef Vec<3,char> Vec3c; +typedef Vec<3,unsigned char> Vec3uc; + +typedef Vec<4,double> Vec4d; +typedef Vec<4,float> Vec4f; +typedef Vec<4,int> Vec4i; +typedef Vec<4,unsigned int> Vec4ui; +typedef Vec<4,short> Vec4s; +typedef Vec<4,unsigned short> Vec4us; +typedef Vec<4,char> Vec4c; +typedef Vec<4,unsigned char> Vec4uc; + +typedef Vec<6,double> Vec6d; +typedef Vec<6,float> Vec6f; +typedef Vec<6,unsigned int> Vec6ui; +typedef Vec<6,int> Vec6i; +typedef Vec<6,short> Vec6s; +typedef Vec<6,unsigned short> Vec6us; +typedef Vec<6,char> Vec6c; +typedef Vec<6,unsigned char> Vec6uc; + + +template +T mag2(const Vec &a) +{ + T l=sqr(a.v[0]); + for(unsigned int i=1; i +T mag(const Vec &a) +{ return sqrt(mag2(a)); } + +template +inline T dist2(const Vec &a, const Vec &b) +{ + T d=sqr(a.v[0]-b.v[0]); + for(unsigned int i=1; i +inline T dist(const Vec &a, const Vec &b) +{ return std::sqrt(dist2(a,b)); } + +template +inline void normalize(Vec &a) +{ a/=mag(a); } + +template +inline Vec normalized(const Vec &a) +{ return a/mag(a); } + +template +inline T infnorm(const Vec &a) +{ + T d=std::fabs(a.v[0]); + for(unsigned int i=1; i +void zero(Vec &a) +{ + for(unsigned int i=0; i +std::ostream &operator<<(std::ostream &out, const Vec &v) +{ + out< +std::istream &operator>>(std::istream &in, Vec &v) +{ + in>>v.v[0]; + for(unsigned int i=1; i>v.v[i]; + return in; +} + +template +inline bool operator==(const Vec &a, const Vec &b) +{ + bool t = (a.v[0] == b.v[0]); + unsigned int i=1; + while(i +inline bool operator!=(const Vec &a, const Vec &b) +{ + bool t = (a.v[0] != b.v[0]); + unsigned int i=1; + while(i +inline Vec operator*(T a, const Vec &v) +{ + Vec w(v); + w*=a; + return w; +} + +template +inline T min(const Vec &a) +{ + T m=a.v[0]; + for(unsigned int i=1; i +inline Vec min_union(const Vec &a, const Vec &b) +{ + Vec m; + for(unsigned int i=0; i +inline Vec max_union(const Vec &a, const Vec &b) +{ + Vec m; + for(unsigned int i=0; i b.v[i]) ? m.v[i]=a.v[i] : m.v[i]=b.v[i]; + return m; +} + +template +inline T max(const Vec &a) +{ + T m=a.v[0]; + for(unsigned int i=1; im) m=a.v[i]; + return m; +} + +template +inline T dot(const Vec &a, const Vec &b) +{ + T d=a.v[0]*b.v[0]; + for(unsigned int i=1; i +inline Vec<2,T> rotate(const Vec<2,T>& a, float angle) +{ + T c = cos(angle); + T s = sin(angle); + return Vec<2,T>(c*a[0] - s*a[1],s*a[0] + c*a[1]); // counter-clockwise rotation +} + +template +inline Vec<2,T> perp(const Vec<2,T> &a) +{ return Vec<2,T>(-a.v[1], a.v[0]); } // counter-clockwise rotation by 90 degrees + +template +inline T cross(const Vec<2,T> &a, const Vec<2,T> &b) +{ return a.v[0]*b.v[1]-a.v[1]*b.v[0]; } + +template +inline Vec<3,T> cross(const Vec<3,T> &a, const Vec<3,T> &b) +{ return Vec<3,T>(a.v[1]*b.v[2]-a.v[2]*b.v[1], a.v[2]*b.v[0]-a.v[0]*b.v[2], a.v[0]*b.v[1]-a.v[1]*b.v[0]); } + +template +inline T triple(const Vec<3,T> &a, const Vec<3,T> &b, const Vec<3,T> &c) +{ return a.v[0]*(b.v[1]*c.v[2]-b.v[2]*c.v[1]) + +a.v[1]*(b.v[2]*c.v[0]-b.v[0]*c.v[2]) + +a.v[2]*(b.v[0]*c.v[1]-b.v[1]*c.v[0]); } + +template +inline unsigned int hash(const Vec &a) +{ + unsigned int h=a.v[0]; + for(unsigned int i=1; i +inline void assign(const Vec &a, T &a0, T &a1) +{ + assert(N==2); + a0=a.v[0]; a1=a.v[1]; +} + +template +inline void assign(const Vec &a, T &a0, T &a1, T &a2) +{ + assert(N==3); + a0=a.v[0]; a1=a.v[1]; a2=a.v[2]; +} + +template +inline void assign(const Vec &a, T &a0, T &a1, T &a2, T &a3) +{ + assert(N==4); + a0=a.v[0]; a1=a.v[1]; a2=a.v[2]; a3=a.v[3]; +} + +template +inline void assign(const Vec &a, T &a0, T &a1, T &a2, T &a3, T &a4, T &a5) +{ + assert(N==6); + a0=a.v[0]; a1=a.v[1]; a2=a.v[2]; a3=a.v[3]; a4=a.v[4]; a5=a.v[5]; +} + +template +inline Vec round(const Vec &a) +{ + Vec rounded; + for(unsigned int i=0; i +inline Vec floor(const Vec &a) +{ + Vec rounded; + for(unsigned int i=0; i +inline Vec ceil(const Vec &a) +{ + Vec rounded; + for(unsigned int i=0; i +inline Vec fabs(const Vec &a) +{ + Vec result; + for(unsigned int i=0; i +inline void minmax(const Vec &x0, const Vec &x1, Vec &xmin, Vec &xmax) +{ + for(unsigned int i=0; i +inline void minmax(const Vec &x0, const Vec &x1, const Vec &x2, Vec &xmin, Vec &xmax) +{ + for(unsigned int i=0; i +inline void minmax(const Vec &x0, const Vec &x1, const Vec &x2, const Vec &x3, + Vec &xmin, Vec &xmax) +{ + for(unsigned int i=0; i +inline void minmax(const Vec &x0, const Vec &x1, const Vec &x2, const Vec &x3, const Vec &x4, + Vec &xmin, Vec &xmax) +{ + for(unsigned int i=0; i +inline void minmax(const Vec &x0, const Vec &x1, const Vec &x2, const Vec &x3, const Vec &x4, + const Vec &x5, Vec &xmin, Vec &xmax) +{ + for(unsigned int i=0; i +inline void update_minmax(const Vec &x, Vec &xmin, Vec &xmax) +{ + for(unsigned int i=0; i build/bdist.linux-x86_64/egg +creating stub loader for redmax_py.cpython-38-x86_64-linux-gnu.so +byte-compiling build/bdist.linux-x86_64/egg/redmax_py.py to redmax_py.cpython-38.pyc +creating build/bdist.linux-x86_64/egg/EGG-INFO +copying redmax_py.egg-info/PKG-INFO -> build/bdist.linux-x86_64/egg/EGG-INFO +copying redmax_py.egg-info/SOURCES.txt -> build/bdist.linux-x86_64/egg/EGG-INFO +copying redmax_py.egg-info/dependency_links.txt -> build/bdist.linux-x86_64/egg/EGG-INFO +copying redmax_py.egg-info/not-zip-safe -> build/bdist.linux-x86_64/egg/EGG-INFO +copying redmax_py.egg-info/top_level.txt -> build/bdist.linux-x86_64/egg/EGG-INFO +writing build/bdist.linux-x86_64/egg/EGG-INFO/native_libs.txt +creating 'dist/redmax_py-0.0.1-py3.8-linux-x86_64.egg' and adding 'build/bdist.linux-x86_64/egg' to it +removing 'build/bdist.linux-x86_64/egg' (and everything under it) +Processing redmax_py-0.0.1-py3.8-linux-x86_64.egg +removing '/home/idanshen/anaconda3/envs/tactile_control_env/lib/python3.8/site-packages/redmax_py-0.0.1-py3.8-linux-x86_64.egg' (and everything under it) +creating /home/idanshen/anaconda3/envs/tactile_control_env/lib/python3.8/site-packages/redmax_py-0.0.1-py3.8-linux-x86_64.egg +Extracting redmax_py-0.0.1-py3.8-linux-x86_64.egg to /home/idanshen/anaconda3/envs/tactile_control_env/lib/python3.8/site-packages +redmax-py 0.0.1 is already the active version in easy-install.pth + +Installed /home/idanshen/anaconda3/envs/tactile_control_env/lib/python3.8/site-packages/redmax_py-0.0.1-py3.8-linux-x86_64.egg +Processing dependencies for redmax-py==0.0.1 +Finished processing dependencies for redmax-py==0.0.1 diff --git a/core/projects/opengl_viewer/src/viewer.cpp b/core/projects/opengl_viewer/src/viewer.cpp index 8f28579..5c49008 100644 --- a/core/projects/opengl_viewer/src/viewer.cpp +++ b/core/projects/opengl_viewer/src/viewer.cpp @@ -648,8 +648,8 @@ void Viewer::MouseWheelScrollCallback(const float y_offset) { } void Viewer::MouseButtonCallback(const int button, const int action) { - if (button == GLFW_MOUSE_BUTTON_MIDDLE) { - mouse_wheel_pressed_ = (action == GLFW_PRESS || action == GLFW_REPEAT); + if (button == GLFW_MOUSE_BUTTON_LEFT || button == GLFW_MOUSE_BUTTON_MIDDLE) { + mouse_wheel_pressed_ = (action == GLFW_PRESS || action == GLFW_REPEAT); } // Forward it to ImGuiWrapper. diff --git a/core/projects/redmax/Body/Body.cpp b/core/projects/redmax/Body/Body.cpp index 0afc39d..76a8c55 100644 --- a/core/projects/redmax/Body/Body.cpp +++ b/core/projects/redmax/Body/Body.cpp @@ -459,4 +459,27 @@ Vector3 Body::velocity_in_world(Vector3 pos) const { return vel; } +std::pair Body::get_AABB() { + std::pair aabb; + double p[2][3]; + p[0][0] = _bounding_box.first[0], p[0][1] = _bounding_box.first[1], p[0][2] = _bounding_box.first[2]; + p[1][0] = _bounding_box.second[0], p[1][1] = _bounding_box.second[1], p[1][2] = _bounding_box.second[2]; + for (int x = 0;x < 2;++x) + for (int y = 0;y < 2;++y) + for (int z = 0;z < 2;++z) { + Vector3 vert(p[x][0], p[y][1], p[z][2]); + Vector3 vert_world = position_in_world(vert); + if (x == 0 && y == 0 && z == 0) { + aabb.first = vert_world; + aabb.second = vert_world; + } else { + for (int axis = 0;axis < 3;++axis) { + aabb.first[axis] = min(aabb.first[axis], vert_world[axis]); + aabb.second[axis] = max(aabb.second[axis], vert_world[axis]); + } + } + } + return aabb; +} + } \ No newline at end of file diff --git a/core/projects/redmax/Body/Body.h b/core/projects/redmax/Body/Body.h index 7d4224e..79b485f 100644 --- a/core/projects/redmax/Body/Body.h +++ b/core/projects/redmax/Body/Body.h @@ -33,6 +33,9 @@ class Body { // contacts std::vector _contact_points; + std::pair _bounding_box; // bounding box of the body + + // index vector _index; // index for phi in maximal coordinates @@ -128,6 +131,8 @@ class Body { // update parameters virtual void update_density(dtype density) {}; virtual void update_size(VectorX body_size) {}; + + std::pair get_AABB(); protected: BodyAnimator* _animator; diff --git a/core/projects/redmax/Body/BodyCuboid.cpp b/core/projects/redmax/Body/BodyCuboid.cpp index 78b29e2..75ebc6b 100644 --- a/core/projects/redmax/Body/BodyCuboid.cpp +++ b/core/projects/redmax/Body/BodyCuboid.cpp @@ -183,6 +183,8 @@ void BodyCuboid::collision( tdot = (I - n * n.transpose()) * vw; } +// refer to https://www.overleaf.com/project/5cd0a78e0e41fe0f8632a42a +// section 1.4: General Penalty-based Contact void BodyCuboid::collision( Vector3 xw, Vector3 xw_dot, /* input */ dtype &d, Vector3 &n, /* output */ diff --git a/core/projects/redmax/Body/BodyMeshObj.cpp b/core/projects/redmax/Body/BodyMeshObj.cpp index 92723c0..9813081 100644 --- a/core/projects/redmax/Body/BodyMeshObj.cpp +++ b/core/projects/redmax/Body/BodyMeshObj.cpp @@ -17,10 +17,7 @@ BodyMeshObj::BodyMeshObj( _filename = filename; - load_mesh(_filename); - - _V = _V.array().colwise() * scale.array(); - + load_mesh(_filename, scale); process_mesh(); if (transform_type == BODY_TO_JOINT) { // this option is not recommended since the mesh will go through unknown transform during initialization to diagonolize the inertia tensor @@ -38,11 +35,11 @@ BodyMeshObj::BodyMeshObj( } else { std::cerr << "[BodyMeshObj::BodyMeshObj] Undefined transform_type: " << transform_type << std::endl; } - + precompute_bounding_box(); precompute_contact_points(); } -void BodyMeshObj::load_mesh(std::string filename) { +void BodyMeshObj::load_mesh(std::string filename, Vector3 scale) { std::vector obj_shape; std::vector obj_material; tinyobj::attrib_t attrib; @@ -52,9 +49,9 @@ void BodyMeshObj::load_mesh(std::string filename) { int num_vertices = (int)attrib.vertices.size() / 3; _V.resize(3, num_vertices); for (int i = 0;i < num_vertices;i++) { - _V.col(i) = Vector3(attrib.vertices[i * 3], - attrib.vertices[i * 3 + 1], - attrib.vertices[i * 3 + 2]); + _V.col(i) = Vector3(attrib.vertices[i * 3] * scale[0], + attrib.vertices[i * 3 + 1] * scale[1], + attrib.vertices[i * 3 + 2] * scale[2]); } int num_elements = (int)obj_shape[0].mesh.indices.size() / 3; @@ -173,6 +170,11 @@ void BodyMeshObj::compute_mass_property( -offd(1), -offd(0), diag(0) + diag(1)).finished(); } +void BodyMeshObj::precompute_bounding_box() { + _bounding_box.first = _V.rowwise().minCoeff(); + _bounding_box.second = _V.rowwise().maxCoeff(); +} + void BodyMeshObj::precompute_contact_points() { // simple random sample on the surface vertices // TODO: more sophisticated sampling @@ -224,4 +226,24 @@ void BodyMeshObj::update_density(dtype density) { process_mesh(); } +bool BodyMeshObj::filter_single(Vector3 xi) { + if ((xi.array() < _bounding_box.first.array()).any()) { + return false; + } + if ((xi.array() > _bounding_box.second.array()).any()) { + return false; + } + return true; +} + +std::vector BodyMeshObj::filter(Matrix3X xi) { + std::vector filter_indices; + for (int i = 0;i < xi.cols();++i) { + if (filter_single(xi.col(i))) { + filter_indices.push_back(i); + } + } + return filter_indices; +} + } \ No newline at end of file diff --git a/core/projects/redmax/Body/BodyMeshObj.h b/core/projects/redmax/Body/BodyMeshObj.h index 471a8fd..8ea1d18 100644 --- a/core/projects/redmax/Body/BodyMeshObj.h +++ b/core/projects/redmax/Body/BodyMeshObj.h @@ -13,7 +13,7 @@ class BodyMeshObj : public Body { Matrix3Xi _F; // face elements SE3 _E_oi; // transform from body frame to obj frame SE3 _E_io; // transform from obj frame to body frame - SE3 _E_0i; // transform from body frame to world frame + // SE3 _E_0i; // transform from body frame to world frame enum TransformType { BODY_TO_JOINT, @@ -36,9 +36,13 @@ class BodyMeshObj : public Body { std::vector& animator_list); void update_density(dtype density); + + bool filter_single(Vector3 xi); + + std::vector filter(Matrix3X xi); private: - void load_mesh(std::string filename); + void load_mesh(std::string filename, Vector3 scale); void process_mesh(); @@ -46,6 +50,8 @@ class BodyMeshObj : public Body { dtype &mass, Vector3 &COM, /*output*/ Matrix3 &I); + void precompute_bounding_box(); + void precompute_contact_points(); }; diff --git a/core/projects/redmax/Body/BodySDFObj.cpp b/core/projects/redmax/Body/BodySDFObj.cpp new file mode 100644 index 0000000..ba12e63 --- /dev/null +++ b/core/projects/redmax/Body/BodySDFObj.cpp @@ -0,0 +1,738 @@ +#include "Body/BodySDFObj.h" +#include "makelevelset3.h" + +namespace redmax { + +BodySDFObj::BodySDFObj( + Simulation* sim, Joint* joint, + std::string filename, + Matrix3 R, Vector3 p, + dtype dx, int res, dtype col_th, + TransformType transform_type, + dtype density, + Vector3 scale, + bool load_sdf, + bool save_sdf) + : BodyMeshObj(sim, joint, filename, R, p, transform_type, density, scale) { + + bool sdf_loaded = false; + if (load_sdf) { + sdf_loaded = load_SDF(); + } + if (!sdf_loaded) { + precompute_SDF(dx, res); + if (save_sdf) { + save_SDF(); + } + } + + // std::cerr << "Before computing SDF" << std::endl; + // precompute_SDF(dx); + _col_th = col_th; + std::cerr << "Finish computing SDF" << std::endl; + std::cerr << "SDF size: " << _SDF.ni << "," << _SDF.nj << "," << _SDF.nk << std::endl; + std::cerr << "SDF min box: (" << _min_box[0] << "," << _min_box[1] << "," << _min_box[2] << "), max box: (" << _max_box[0] << "," << _max_box[1] << "," << _max_box[2] << ")" << std::endl; +} + +void BodySDFObj::precompute_SDF(dtype dx, int res) { + + //start with a massive inside out bound box. + sdfgen::Vec3f min_box(std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max()), + max_box(-std::numeric_limits::max(), -std::numeric_limits::max(), -std::numeric_limits::max()); + + std::vector vertList; + std::vector faceList; + + int num_vertices = _V.cols(); + for (int i = 0;i < num_vertices;i++) { + sdfgen::Vec3f point(_V(0, i), _V(1, i), _V(2, i)); + vertList.push_back(point); + sdfgen::update_minmax(point, min_box, max_box); + } + + int num_faces = _F.cols(); + for (int i = 0;i < num_faces;i++) { + faceList.push_back(sdfgen::Vec3ui(_F(0, i), _F(1, i), _F(2, i))); + } + + int min_size = res; + Vector3 bbox_size = _bounding_box.second - _bounding_box.first; + _dx = std::min(dx, bbox_size(0) / min_size); + _dy = std::min(dx, bbox_size(1) / min_size); + _dz = std::min(dx, bbox_size(2) / min_size); + + // add padding around the box + sdfgen::Vec3f unit((float)_dx, (float)_dy, (float)_dz); + int padding = 1; + min_box -= (float)padding * unit; + max_box += (float)padding * unit; + sdfgen::Vec3ui sizes = sdfgen::Vec3ui((max_box - min_box + sdfgen::Vec3f(0.5 * _dx, 0.5 * _dy, 0.5 * _dz)) / sdfgen::Vec3f(_dx, _dy, _dz)) + sdfgen::Vec3ui(1, 1, 1); + + // make SDF + sdfgen::Array3f SDF; + sdfgen::make_level_set3(faceList, vertList, min_box, (float)_dx, (float)_dy, (float)_dz, sizes[0], sizes[1], sizes[2], SDF); + + // type conversion + _min_box = Vector3(min_box[0], min_box[1], min_box[2]); + _max_box = Vector3(max_box[0], max_box[1], max_box[2]); + int ni = SDF.ni, nj = SDF.nj, nk = SDF.nk; + _SDF.resize(ni, nj, nk); + for (int i = 0; i < ni; ++i) for (int j = 0; j < nj; ++j) for (int k = 0; k < nk; ++k) { + _SDF(i, j, k) = (dtype)SDF(i, j, k); + } +} + +bool BodySDFObj::load_SDF() { + std::string inname = _filename.substr(0, _filename.size()-4) + std::string(".sdf"); + std::ifstream infile(inname.c_str()); + bool success = infile.good(); + if (success) { + int ni, nj, nk; + infile >> ni >> nj >> nk; + infile >> _min_box[0] >> _min_box[1] >> _min_box[2]; + infile >> _max_box[0] >> _max_box[1] >> _max_box[2]; + infile >> _dx >> _dy >> _dz; + _SDF.resize(ni, nj, nk); + for(unsigned int i = 0; i < _SDF.a.size(); ++i) { + infile >> _SDF.a[i]; + } + } + infile.close(); + return success; +} + +void BodySDFObj::save_SDF() { + std::string outname = _filename.substr(0, _filename.size()-4) + std::string(".sdf"); + std::ofstream outfile(outname.c_str()); + outfile << _SDF.ni << " " << _SDF.nj << " " << _SDF.nk << std::endl; + outfile << _min_box[0] << " " << _min_box[1] << " " << _min_box[2] << std::endl; + outfile << _max_box[0] << " " << _max_box[1] << " " << _max_box[2] << std::endl; + outfile << _dx << " " << _dy << " " << _dz << std::endl; + for(unsigned int i = 0; i < _SDF.a.size(); ++i) { + outfile << _SDF.a[i] << std::endl; + } + outfile.close(); +} + +void BodySDFObj::clear_saved_SDF() { + std::string name = _filename.substr(0, _filename.size()-4) + std::string(".sdf"); + remove(name.c_str()); +} + +dtype BodySDFObj::query_SDF(Vector3 x, bool outside_accurate) { + + bool inside = true; + for (int i = 0;i < 3;i++) { + inside = inside && x(i) >= _min_box[i] && x(i) <= _max_box[i]; + } + + if (inside) { + // compute grid location and interpolation coefficients + dtype ci = (x(0) - _min_box[0]) / _dx, cj = (x(1) - _min_box[1]) / _dy, ck = (x(2) - _min_box[2]) / _dz; + int i = std::floor(ci), j = std::floor(cj), k = std::floor(ck); + ci = ci - i, cj = cj - j, ck = ck - k; + + // boundary handling + int ni = _SDF.ni, nj = _SDF.nj, nk = _SDF.nk; + int i_pos = (i == ni - 1) ? i : i + 1; + int j_pos = (j == nj - 1) ? j : j + 1; + int k_pos = (k == nk - 1) ? k : k + 1; + + // interpolate along i-axis + dtype vi_j0k0 = (1 - ci) * _SDF(i, j, k) + ci * _SDF(i_pos, j, k); + dtype vi_j1k0 = (1 - ci) * _SDF(i, j_pos, k) + ci * _SDF(i_pos, j_pos, k); + dtype vi_j0k1 = (1 - ci) * _SDF(i, j, k_pos) + ci * _SDF(i_pos, j, k_pos); + dtype vi_j1k1 = (1 - ci) * _SDF(i, j_pos, k_pos) + ci * _SDF(i_pos, j_pos, k_pos); + + // interpolate along j-axis + dtype vij_k0 = (1 - cj) * vi_j0k0 + cj * vi_j1k0; + dtype vij_k1 = (1 - cj) * vi_j0k1 + cj * vi_j1k1; + + // interpolate along k-axis + dtype v = (1 - ck) * vij_k0 + ck * vij_k1; + + return v; + + } else { + + if (outside_accurate) { + + // find closest boundary point + Vector3 x_b; + for (int i = 0; i < 3; ++i) x_b(i) = std::min(std::max(x(i), _min_box[i]), _max_box[i]); + + // approximate by boundary distance + boundary SDF + dtype v = (x - x_b).norm() + query_SDF(x_b); + return v; + + } else { + return (dtype)std::numeric_limits::max(); + } + } + +} + +Vector3 BodySDFObj::query_dSDF(Vector3 x) { + + dtype eps = 1e-5; + Vector3 dx = Vector3(eps, 0, 0), dy = Vector3(0, eps, 0), dz = Vector3(0, 0, eps); + Vector3 v; + v(0) = query_SDF(x + dx, true) - query_SDF(x - dx, true); + v(1) = query_SDF(x + dy, true) - query_SDF(x - dy, true); + v(2) = query_SDF(x + dz, true) - query_SDF(x - dz, true); + v /= (2 * eps); + + return v; +} + +Matrix3 BodySDFObj::query_ddSDF(Vector3 x) { + + dtype eps = 1e-5; + Vector3 dx = Vector3(eps, 0, 0), dy = Vector3(0, eps, 0), dz = Vector3(0, 0, eps); + Vector3 halfdx = dx / 2, halfdy = dy / 2, halfdz = dz / 2; + Matrix3 v = Matrix3::Zero(); + dtype x_SDF = query_SDF(x, true); + v(0, 0) = query_SDF(x + dx, true) + query_SDF(x - dx, true) - 2 * x_SDF; + v(1, 1) = query_SDF(x + dy, true) + query_SDF(x - dy, true) - 2 * x_SDF; + v(2, 2) = query_SDF(x + dz, true) + query_SDF(x - dz, true) - 2 * x_SDF; + v(0, 1) = v(1, 0) = query_SDF(x + halfdx + halfdy, true) + query_SDF(x - halfdx - halfdy, true) - query_SDF(x + halfdx - halfdy, true) - query_SDF(x - halfdx + halfdy, true); + v(0, 2) = v(2, 0) = query_SDF(x + halfdx + halfdz, true) + query_SDF(x - halfdx - halfdz, true) - query_SDF(x + halfdx - halfdz, true) - query_SDF(x - halfdx + halfdz, true); + v(1, 2) = v(2, 1) = query_SDF(x + halfdy + halfdz, true) + query_SDF(x - halfdy - halfdz, true) - query_SDF(x + halfdy - halfdz, true) - query_SDF(x - halfdy + halfdz, true); + v /= (eps * eps); + + return v; +} + +dtype BodySDFObj::distance(Vector3 xw) { + + Matrix3 R2 = _E_0i.topLeftCorner(3, 3); + Vector3 p2 = _E_0i.topRightCorner(3, 1); + Vector3 x = R2.transpose() * (xw - p2); + return query_SDF(x) + _col_th; +} + +Vector3 BodySDFObj::surface_contact_pt(Vector3 xw) { + + Matrix3 R2 = _E_0i.topLeftCorner(3, 3); + Vector3 p2 = _E_0i.topRightCorner(3, 1); + Vector3 x = R2.transpose() * (xw - p2); + + // compute d + dtype d = query_SDF(x) + _col_th; + + // compute e + Vector3 dd_dx = query_dSDF(x); + dtype dd_dx_norm = dd_dx.norm(); + Vector3 e; + if (std::abs(dd_dx_norm) < 1e-5) { + e = Vector3::Zero(); + } else { + e = dd_dx / dd_dx_norm; + } + + // compute xi2 + Vector3 xi2 = x - d * e; + return xi2; +} + +void BodySDFObj::collision( + Vector3 xw, Vector3 xw_dot, /* input */ + dtype &d, Vector3 &n, /* output */ + dtype &ddot, Vector3 &tdot, + Vector3 &xi2) { + + Matrix3 I = Matrix3::Identity(); + Matrix3 R2 = _E_0i.topLeftCorner(3, 3); + Vector3 p2 = _E_0i.topRightCorner(3, 1); + Vector3 w2_dot = _phi.head(3); + Vector3 v2_dot = _phi.tail(3); + Vector3 p2_dot = R2 * v2_dot; + Vector3 x = R2.transpose() * (xw - p2); + + // compute d + d = query_SDF(x) + _col_th; + + // compute e + Vector3 dd_dx = query_dSDF(x); + Vector3 e = dd_dx / dd_dx.norm(); + + // compute n + n = R2 * e; + + // compute xi2 + xi2 = x - d * e; + + // compute ddot + Vector3 xdot = math::skew(w2_dot).transpose() * x + R2.transpose() * xw_dot - v2_dot; + ddot = dd_dx.transpose() * xdot; + + // compute tdot + Vector3 xw2_dot = R2 * (w2_dot.cross(xi2) + v2_dot); + Vector3 vw = xw_dot - xw2_dot; + tdot = (I - n * n.transpose()) * vw; +} + +// refer to https://www.overleaf.com/project/5cd0a78e0e41fe0f8632a42a +// section 1.4: General Penalty-based Contact +void BodySDFObj::collision( + Vector3 xw, Vector3 xw_dot, /* input */ + dtype &d, Vector3 &n, /* output */ + dtype &ddot, Vector3 &tdot, + Vector3 &xi2, + RowVector3 &dd_dxw, RowVector6 &dd_dq2, /* derivatives for d */ + Matrix3 &dn_dxw, Matrix36 &dn_dq2, /* derivatives for n */ + RowVector3 &dddot_dxw, RowVector3 &dddot_dxwdot, /* derivatives for ddot */ + RowVector6 &dddot_dq2, RowVector6 &dddot_dphi2, + Matrix3 &dtdot_dxw, Matrix3 &dtdot_dxwdot, /* derivatives for tdot */ + Matrix36 &dtdot_dq2, Matrix36 &dtdot_dphi2, + Matrix3 &dxi2_dxw, Matrix36 &dxi2_dq2/* derivatives for xi2 */) { + + Matrix3 I = Matrix3::Identity(); + Matrix3 R2 = _E_0i.topLeftCorner(3, 3); + Vector3 p2 = _E_0i.topRightCorner(3, 1); + Vector3 w2_dot = _phi.head(3); + Vector3 v2_dot = _phi.tail(3); + Vector3 p2_dot = R2 * v2_dot; + Vector3 x = R2.transpose() * (xw - p2); + + /**************** values ****************/ + + d = query_SDF(x) + _col_th; + Vector3 dd_dx = query_dSDF(x); + Vector3 e = dd_dx / dd_dx.norm(); + n = R2 * e; + xi2 = x - d * e; + Vector3 xdot = math::skew(w2_dot).transpose() * x + R2.transpose() * xw_dot - v2_dot; + ddot = dd_dx.transpose() * xdot; + Vector3 xw2_dot = R2 * (w2_dot.cross(xi2) + v2_dot); + Vector3 vw = xw_dot - xw2_dot; + tdot = (I - n * n.transpose()) * vw; + + /**************** derivatives ****************/ + + Vector3 e1 = Vector3::UnitX(), e2 = Vector3::UnitY(), e3 = Vector3::UnitZ(); + + // x + Matrix3 dx_dxw = R2.transpose(); + Matrix3 dx_dw2 = math::skew(x); + Matrix3 dx_dv2 = -I; + + // d + dd_dxw = dd_dx.transpose() * dx_dxw; + dd_dq2.leftCols(3) = dd_dx.transpose() * dx_dw2; + dd_dq2.rightCols(3) = dd_dx.transpose() * dx_dv2; + + // n + Matrix3 de_ddd_dx = Matrix3::Zero(); + dtype dd_dx_norm = dd_dx.norm(); + for (int i = 0; i < 3; ++i) { + Vector3 unit_vec = Vector3::Zero(); + unit_vec(i) = 1; + de_ddd_dx.row(i) = (dd_dx_norm * unit_vec - dd_dx(i) * dd_dx / dd_dx_norm) / (dd_dx_norm * dd_dx_norm); + } + Matrix3 ddd_dx_dx = query_ddSDF(x); + Matrix3 de_dx = de_ddd_dx * ddd_dx_dx; + Matrix3 de_dxw = de_dx * dx_dxw; + Matrix3 de_dw2 = de_dx * dx_dw2; + Matrix3 de_dv2 = de_dx * dx_dv2; + dn_dxw = R2 * de_dxw; + dn_dq2.leftCols(3) = R2 * de_dw2; + dn_dq2.col(0) += R2 * math::skew(e1) * e; + dn_dq2.col(1) += R2 * math::skew(e2) * e; + dn_dq2.col(2) += R2 * math::skew(e3) * e; + dn_dq2.rightCols(3) = R2 * de_dv2; + + // xi2 + dxi2_dxw = dx_dxw - e * dd_dxw - d * de_dxw; + dxi2_dq2.leftCols(3) = dx_dw2 - e * dd_dq2.head(3) - d * de_dw2; + dxi2_dq2.rightCols(3) = dx_dv2 - e * dd_dq2.tail(3) - d * de_dv2; + + // ddot + dddot_dxw = (dd_dx.transpose() * math::skew(w2_dot).transpose() + xdot.transpose() * ddd_dx_dx) * R2.transpose(); + dddot_dxwdot = dd_dx.transpose() * R2.transpose(); + dddot_dq2.leftCols(3) = dd_dx.transpose() * (-math::skew(w2_dot) * math::skew(x) + math::skew(R2.transpose() * xw_dot)) + xdot.transpose() * ddd_dx_dx * dx_dw2; + dddot_dq2.rightCols(3) = dd_dx.transpose() * math::skew(w2_dot) + xdot.transpose() * ddd_dx_dx * dx_dv2; + dddot_dphi2.leftCols(3) = dd_dx.transpose() * math::skew(x); + dddot_dphi2.rightCols(3) = dd_dx.transpose() * (-I); + + // xw2_dot + Matrix3 dxw2dot_dxw = R2 * math::skew(w2_dot) * dxi2_dxw; + Matrix3 dxw2dot_dxwdot = Matrix3::Zero(); + Matrix36 dxw2dot_dq2; + dxw2dot_dq2.leftCols(3) = -R2 * math::skew(w2_dot.cross(xi2) + v2_dot) + R2 * math::skew(w2_dot) * dxi2_dq2.leftCols(3); + dxw2dot_dq2.rightCols(3) = R2 * math::skew(w2_dot) * dxi2_dq2.rightCols(3); + Matrix36 dxw2dot_dphi2; + dxw2dot_dphi2.col(0) = R2 * math::skew(e1) * xi2; + dxw2dot_dphi2.col(1) = R2 * math::skew(e2) * xi2; + dxw2dot_dphi2.col(2) = R2 * math::skew(e3) * xi2; + dxw2dot_dphi2.rightCols(3) = R2; + + // tdot + dtdot_dxw = -dxw2dot_dxw - n.transpose() * vw * dn_dxw - n * vw.transpose() * dn_dxw + n * n.transpose() * dxw2dot_dxw; + dtdot_dxwdot = (I - n * n.transpose()) * (I - dxw2dot_dxwdot); + dtdot_dq2.leftCols(3) = -dxw2dot_dq2.leftCols(3) - n.transpose() * vw * dn_dq2.leftCols(3) - n * (vw.transpose() * dn_dq2.leftCols(3) - n.transpose() * dxw2dot_dq2.leftCols(3)); + dtdot_dq2.rightCols(3) = -dxw2dot_dq2.rightCols(3) - n.transpose() * vw * dn_dq2.rightCols(3) - n * (vw.transpose() * dn_dq2.rightCols(3) - n.transpose() * dxw2dot_dq2.rightCols(3)); + dtdot_dphi2 = (I - n * n.transpose()) * (- dxw2dot_dphi2); +} + +Vector3i BodySDFObj::query_grid_location(Vector3 x) { + dtype ci = (x(0) - _min_box[0]) / _dx, cj = (x(1) - _min_box[1]) / _dy, ck = (x(2) - _min_box[2]) / _dz; + int i = std::floor(ci), j = std::floor(cj), k = std::floor(ck); + return Vector3i(i, j, k); +} + +void BodySDFObj::test_collision_derivatives() { + srand(1000); + // srand(time(0)); + dtype eps = 1e-7; + // generate random xw, xw_dot, E_2, phi_2 + Eigen::Quaternion quat_2(Vector4::Random()); + quat_2.normalize(); + Matrix4 E2 = Matrix4::Identity(); + E2.topLeftCorner(3, 3) = quat_2.toRotationMatrix(); + E2.topRightCorner(3, 1) = Vector3::Random(); + Vector6 phi2 = Vector6::Random(); + + Vector3 x, xw1; + Vector3 rnd_x; + Vector3 scale_x = _max_box - _min_box - 2 * Vector3::Ones() * _dx; + Vector3 min_x = _min_box + Vector3::Ones() * _dx; + + Vector3 xw1_dot = Vector3::Random(); + + while (true) { + + rnd_x = 0.5 * (Vector3::Random() + Vector3::Ones()); + + x = rnd_x.cwiseProduct(scale_x) + min_x; + xw1 = E2.topLeftCorner(3, 3) * x + E2.topRightCorner(3, 1); + + if (query_SDF(x) >= 0.) { + std::cerr << _min_box.transpose() << " " << _max_box.transpose() << std::endl; + std::cerr << x.transpose() << std::endl; + throw_error("Bug in test_collision_derivatives() in BodySDFObj.cpp"); + } + + Matrix3 R2 = E2.topLeftCorner(3, 3); + Vector3 p2 = E2.topRightCorner(3, 1); + + std::vector xw_pos; + xw_pos.push_back(xw1 + eps * xw1_dot); + xw_pos.push_back(xw1 + eps * Vector3::UnitX() * eps); + xw_pos.push_back(xw1 + eps * Vector3::UnitY() * eps); + xw_pos.push_back(xw1 + eps * Vector3::UnitZ() * eps); + + std::vector x_pos; + for (int i = 0; i < xw_pos.size(); ++i) { + x_pos.push_back(R2.transpose() * (xw_pos[i] - p2)); + } + + Vector3i loc = query_grid_location(x); + std::vector loc_delta; + std::vector deltas; + deltas.push_back(_dx / 2. * Vector3::UnitX()); + deltas.push_back(_dx / 2. * Vector3::UnitY()); + deltas.push_back(_dx / 2. * Vector3::UnitZ()); + for (int i = 0; i < deltas.size(); ++i) { + loc_delta.push_back(query_grid_location(x + deltas[i])); + } + + for (int i = 0; i < x_pos.size(); ++i) { + Vector3i loc_pos = query_grid_location(x_pos[i]); + if (!loc.isApprox(loc_pos)) { + std::cerr << "loc and loc_pos: " << loc.transpose() << ", " << loc_pos.transpose() << std::endl; + std::cerr << "position: " << x.transpose() << ", " << x_pos[i].transpose() << std::endl; + throw_error("grid location mismatch"); + } + for (int j = 0; j < deltas.size(); ++j) { + Vector3i loc_pos_delta = query_grid_location(x_pos[i] + deltas[j]); + if (!loc_delta[j].isApprox(loc_pos_delta)) { + std::cerr << j << "th loc and loc_pos delta: " << loc_delta[j].transpose() << ", " << loc_pos_delta.transpose() << std::endl; + std::cerr << "position: " << (x + deltas[j]).transpose() << ", " << (x_pos[i] + deltas[j]).transpose() << std::endl; + throw_error("grid location mismatch"); + } + } + } + + break; + } + + this->_E_0i = E2; + this->_phi = phi2; + + // test SDF derivatives + { + Matrix3 R2 = E2.topLeftCorner(3, 3); + Vector3 p2 = E2.topRightCorner(3, 1); + + dtype d_ori = query_SDF(x); + Vector3 dd_dx_ori = query_dSDF(x); + Vector3 dd_dx_fd; + for (int i = 0;i < 3;i++) { + Vector3 x_pos = x; + x_pos[i] += eps; + dtype d_pos = query_SDF(x_pos); + dd_dx_fd[i] = (d_pos - d_ori) / eps; + } + + print_error("dd_dx", dd_dx_ori, dd_dx_fd); + + Matrix3 ddd_dx_ori = query_ddSDF(x); + Matrix3 ddd_dx_fd; + for (int i = 0;i < 3;i++) { + Vector3 x_pos = x; + x_pos[i] += eps; + Vector3 dd_dx_pos = query_dSDF(x_pos); + ddd_dx_fd.col(i) = (dd_dx_pos - dd_dx_ori) / eps; + } + + print_error("ddd_dx", ddd_dx_ori, ddd_dx_fd); + } + + // test time derivatives + { + dtype d_ori, ddot_ori; + Vector3 n_ori, tdot_ori, xi2_ori; + collision(xw1, xw1_dot, d_ori, n_ori, ddot_ori, tdot_ori, xi2_ori); + + dtype ddot_fd; + + Vector3 xw1_pos = xw1 + eps * xw1_dot; + Vector6 dq = eps * phi2; + Matrix4 E2_pos = E2 * math::exp(dq); + this->_E_0i = E2_pos; + dtype d_pos, ddot_pos; + Vector3 n_pos, tdot_pos, xi2_pos; + collision(xw1_pos, xw1_dot, d_pos, n_pos, ddot_pos, tdot_pos, xi2_pos); + ddot_fd = (d_pos - d_ori) / eps; + this->_E_0i = E2; + + print_error("ddot", ddot_ori, ddot_fd); + } + + dtype d, ddot; + Vector3 n, tdot, xi2; + RowVector3 dd_dxw1, dddot_dxw1, dddot_dxw1dot; + RowVector6 dd_dq2, dddot_dq2, dddot_dphi2; + Matrix3 dn_dxw1, dtdot_dxw1, dtdot_dxw1dot, dxi2_dxw1; + Matrix36 dn_dq2, dtdot_dq2, dtdot_dphi2, dxi2_dq2; + collision(xw1, xw1_dot, d, n, ddot, tdot, xi2, + dd_dxw1, dd_dq2, + dn_dxw1, dn_dq2, + dddot_dxw1, dddot_dxw1dot, + dddot_dq2, dddot_dphi2, + dtdot_dxw1, dtdot_dxw1dot, + dtdot_dq2, dtdot_dphi2, + dxi2_dxw1, dxi2_dq2); + + // test dxw1 related + RowVector3 dd_dxw1_fd, dddot_dxw1_fd; + Matrix3 dn_dxw1_fd, dtdot_dxw1_fd, dxi2_dxw1_fd; + for (int i = 0;i < 3;i++) { + Vector3 xw1_pos = xw1; + xw1_pos[i] += eps; + dtype d_pos, ddot_pos; + Vector3 n_pos, tdot_pos, xi2_pos; + collision(xw1_pos, xw1_dot, d_pos, n_pos, ddot_pos, tdot_pos, xi2_pos); + dd_dxw1_fd[i] = (d_pos - d) / eps; + dddot_dxw1_fd[i] = (ddot_pos - ddot) / eps; + dn_dxw1_fd.col(i) = (n_pos - n) / eps; + dtdot_dxw1_fd.col(i) = (tdot_pos - tdot) / eps; + dxi2_dxw1_fd.col(i) = (xi2_pos - xi2) / eps; + } + print_error("dd_dxw1", dd_dxw1, dd_dxw1_fd); + print_error("dddot_dxw1", dddot_dxw1, dddot_dxw1_fd); + print_error("dn_dxw1", dn_dxw1, dn_dxw1_fd); + print_error("dtdot_dxw1", dtdot_dxw1, dtdot_dxw1_fd); + print_error("dxi2_dxw1", dxi2_dxw1, dxi2_dxw1_fd); + + // test dxw1dot related + RowVector3 dddot_dxw1dot_fd; + Matrix3 dtdot_dxw1dot_fd; + for (int i = 0;i < 3;i++) { + Vector3 xw1dot_pos = xw1_dot; + xw1dot_pos[i] += eps; + dtype d_pos, ddot_pos; + Vector3 n_pos, tdot_pos, xi2_pos; + collision(xw1, xw1dot_pos, d_pos, n_pos, ddot_pos, tdot_pos, xi2_pos); + dddot_dxw1dot_fd[i] = (ddot_pos - ddot) / eps; + dtdot_dxw1dot_fd.col(i) = (tdot_pos - tdot) / eps; + } + print_error("dddot_dxw1dot", dddot_dxw1dot, dddot_dxw1dot_fd); + print_error("dtdot_dxw1dot", dtdot_dxw1dot, dtdot_dxw1dot_fd); + + // test dq2 related + RowVector6 dd_dq2_fd, dddot_dq2_fd; + Matrix36 dn_dq2_fd, dtdot_dq2_fd, dxi2_dq2_fd; + for (int i = 0;i < 6;i++) { + Vector6 dq = Vector6::Zero(); + dq[i] = eps; + Matrix4 E2_pos = E2 * math::exp(dq); + dtype d_pos, ddot_pos; + Vector3 n_pos, tdot_pos, xi2_pos; + this->_E_0i = E2_pos; + collision(xw1, xw1_dot, d_pos, n_pos, ddot_pos, tdot_pos, xi2_pos); + dd_dq2_fd[i] = (d_pos - d) / eps; + dddot_dq2_fd[i] = (ddot_pos - ddot) / eps; + dn_dq2_fd.col(i) = (n_pos - n) / eps; + dtdot_dq2_fd.col(i) = (tdot_pos - tdot) / eps; + dxi2_dq2_fd.col(i) = (xi2_pos - xi2) / eps; + } + print_error("dd_dq2", dd_dq2, dd_dq2_fd); + print_error("dddot_dq2", dddot_dq2, dddot_dq2_fd); + print_error("dn_dw2", dn_dq2.leftCols(3), dn_dq2_fd.leftCols(3)); + print_error("dn_dv2", dn_dq2.rightCols(3), dn_dq2_fd.rightCols(3)); + print_error("dtdot_dw2", dtdot_dq2.leftCols(3), dtdot_dq2_fd.leftCols(3)); + print_error("dtdot_dv2", dtdot_dq2.rightCols(3), dtdot_dq2_fd.rightCols(3)); + print_error("dxi2_dq2", dxi2_dq2, dxi2_dq2_fd); + this->_E_0i = E2; + + // test dphi2 related + RowVector6 dddot_dphi2_fd; + Matrix36 dtdot_dphi2_fd; + for (int i = 0;i < 6;i++) { + Vector6 phi2_pos = phi2; + phi2_pos[i] += eps; + this->_phi = phi2_pos; + dtype d_pos, ddot_pos; + Vector3 n_pos, tdot_pos, xi2_pos; + collision(xw1, xw1_dot, d_pos, n_pos, ddot_pos, tdot_pos, xi2_pos); + dddot_dphi2_fd[i] = (ddot_pos - ddot) / eps; + dtdot_dphi2_fd.col(i) = (tdot_pos - tdot) / eps; + } + print_error("dddot_dw2dot", dddot_dphi2.head(3), dddot_dphi2_fd.head(3)); + print_error("dddot_dv2dot", dddot_dphi2.tail(3), dddot_dphi2_fd.tail(3)); + print_error("dtdot_dw2dot", dtdot_dphi2.leftCols(3), dtdot_dphi2_fd.leftCols(3)); + print_error("dtdot_dv2dot", dtdot_dphi2.rightCols(3), dtdot_dphi2_fd.rightCols(3)); + this->_phi = phi2; +} + +void BodySDFObj::test_collision_derivatives_runtime(Vector3 xw, Vector3 xw_dot) { + dtype eps = 1e-7; + + Matrix4 E2 = this->_E_0i; + Vector6 phi2 = this->_phi; + Vector3 xw1 = xw; + Vector3 xw1_dot = xw_dot; + Vector3 x = E2.topLeftCorner(3, 3).transpose() * (xw1 - E2.topRightCorner(3, 1)); + + dtype d, ddot; + Vector3 n, tdot, xi2; + RowVector3 dd_dxw1, dddot_dxw1, dddot_dxw1dot; + RowVector6 dd_dq2, dddot_dq2, dddot_dphi2; + Matrix3 dn_dxw1, dtdot_dxw1, dtdot_dxw1dot, dxi2_dxw1; + Matrix36 dn_dq2, dtdot_dq2, dtdot_dphi2, dxi2_dq2; + collision(xw1, xw1_dot, d, n, ddot, tdot, xi2, + dd_dxw1, dd_dq2, + dn_dxw1, dn_dq2, + dddot_dxw1, dddot_dxw1dot, + dddot_dq2, dddot_dphi2, + dtdot_dxw1, dtdot_dxw1dot, + dtdot_dq2, dtdot_dphi2, + dxi2_dxw1, dxi2_dq2); + + // test time derivatives + { + // dtype d_ori, ddot_ori; + // Vector3 n_ori, tdot_ori, xi2_ori; + // collision(xw1, xw1_dot, d_ori, n_ori, ddot_ori, tdot_ori, xi2_ori); + + dtype ddot_fd; + + Vector3 xw1_pos = xw1 + eps * xw1_dot; + Vector6 dq = eps * phi2; + Matrix4 E2_pos = E2 * math::exp(dq); + this->_E_0i = E2_pos; + dtype d_pos, ddot_pos; + Vector3 n_pos, tdot_pos, xi2_pos; + collision(xw1_pos, xw1_dot, d_pos, n_pos, ddot_pos, tdot_pos, xi2_pos); + + ddot_fd = (d_pos - d) / eps; + this->_E_0i = E2; + + print_error("Body SDF Collision Derivatives: ddot", ddot, ddot_fd); + } + + // test dxw1 related + RowVector3 dd_dxw1_fd, dddot_dxw1_fd; + Matrix3 dn_dxw1_fd, dtdot_dxw1_fd, dxi2_dxw1_fd; + for (int i = 0;i < 3;i++) { + Vector3 xw1_pos = xw1; + xw1_pos[i] += eps; + dtype d_pos, ddot_pos; + Vector3 n_pos, tdot_pos, xi2_pos; + collision(xw1_pos, xw1_dot, d_pos, n_pos, ddot_pos, tdot_pos, xi2_pos); + dd_dxw1_fd[i] = (d_pos - d) / eps; + dddot_dxw1_fd[i] = (ddot_pos - ddot) / eps; + dn_dxw1_fd.col(i) = (n_pos - n) / eps; + dtdot_dxw1_fd.col(i) = (tdot_pos - tdot) / eps; + dxi2_dxw1_fd.col(i) = (xi2_pos - xi2) / eps; + } + print_error("Body SDF Collision Derivatives: dd_dxw1", dd_dxw1, dd_dxw1_fd); + print_error("Body SDF Collision Derivatives: dddot_dxw1", dddot_dxw1, dddot_dxw1_fd); + print_error("Body SDF Collision Derivatives: dn_dxw1", dn_dxw1, dn_dxw1_fd); + print_error("Body SDF Collision Derivatives: dtdot_dxw1", dtdot_dxw1, dtdot_dxw1_fd); + print_error("Body SDF Collision Derivatives: dxi2_dxw1", dxi2_dxw1, dxi2_dxw1_fd); + + // test dxw1dot related + RowVector3 dddot_dxw1dot_fd; + Matrix3 dtdot_dxw1dot_fd; + for (int i = 0;i < 3;i++) { + Vector3 xw1dot_pos = xw1_dot; + xw1dot_pos[i] += eps; + dtype d_pos, ddot_pos; + Vector3 n_pos, tdot_pos, xi2_pos; + collision(xw1, xw1dot_pos, d_pos, n_pos, ddot_pos, tdot_pos, xi2_pos); + dddot_dxw1dot_fd[i] = (ddot_pos - ddot) / eps; + dtdot_dxw1dot_fd.col(i) = (tdot_pos - tdot) / eps; + } + print_error("Body SDF Collision Derivatives: dddot_dxw1dot", dddot_dxw1dot, dddot_dxw1dot_fd); + print_error("Body SDF Collision Derivatives: dtdot_dxw1dot", dtdot_dxw1dot, dtdot_dxw1dot_fd); + + // test dq2 related + RowVector6 dd_dq2_fd, dddot_dq2_fd; + Matrix36 dn_dq2_fd, dtdot_dq2_fd, dxi2_dq2_fd; + for (int i = 0;i < 6;i++) { + Vector6 dq = Vector6::Zero(); + dq[i] = eps; + Matrix4 E2_pos = E2 * math::exp(dq); + dtype d_pos, ddot_pos; + Vector3 n_pos, tdot_pos, xi2_pos; + this->_E_0i = E2_pos; + collision(xw1, xw1_dot, d_pos, n_pos, ddot_pos, tdot_pos, xi2_pos); + dd_dq2_fd[i] = (d_pos - d) / eps; + dddot_dq2_fd[i] = (ddot_pos - ddot) / eps; + dn_dq2_fd.col(i) = (n_pos - n) / eps; + dtdot_dq2_fd.col(i) = (tdot_pos - tdot) / eps; + dxi2_dq2_fd.col(i) = (xi2_pos - xi2) / eps; + } + print_error("Body SDF Collision Derivatives: dd_dq2", dd_dq2, dd_dq2_fd); + print_error("Body SDF Collision Derivatives: dddot_dq2", dddot_dq2, dddot_dq2_fd); + print_error("Body SDF Collision Derivatives: dn_dw2", dn_dq2.leftCols(3), dn_dq2_fd.leftCols(3)); + print_error("Body SDF Collision Derivatives: dn_dv2", dn_dq2.rightCols(3), dn_dq2_fd.rightCols(3)); + print_error("Body SDF Collision Derivatives: dtdot_dw2", dtdot_dq2.leftCols(3), dtdot_dq2_fd.leftCols(3)); + print_error("Body SDF Collision Derivatives: dtdot_dv2", dtdot_dq2.rightCols(3), dtdot_dq2_fd.rightCols(3)); + print_error("Body SDF Collision Derivatives: dxi2_dq2", dxi2_dq2, dxi2_dq2_fd); + this->_E_0i = E2; + + // test dphi2 related + RowVector6 dddot_dphi2_fd; + Matrix36 dtdot_dphi2_fd; + for (int i = 0;i < 6;i++) { + Vector6 phi2_pos = phi2; + phi2_pos[i] += eps; + this->_phi = phi2_pos; + dtype d_pos, ddot_pos; + Vector3 n_pos, tdot_pos, xi2_pos; + collision(xw1, xw1_dot, d_pos, n_pos, ddot_pos, tdot_pos, xi2_pos); + dddot_dphi2_fd[i] = (ddot_pos - ddot) / eps; + dtdot_dphi2_fd.col(i) = (tdot_pos - tdot) / eps; + } + print_error("Body SDF Collision Derivatives: dddot_dw2dot", dddot_dphi2.head(3), dddot_dphi2_fd.head(3)); + print_error("Body SDF Collision Derivatives: dddot_dv2dot", dddot_dphi2.tail(3), dddot_dphi2_fd.tail(3)); + print_error("Body SDF Collision Derivatives: dtdot_dw2dot", dtdot_dphi2.leftCols(3), dtdot_dphi2_fd.leftCols(3)); + print_error("Body SDF Collision Derivatives: dtdot_dv2dot", dtdot_dphi2.rightCols(3), dtdot_dphi2_fd.rightCols(3)); + this->_phi = phi2; +} + +} diff --git a/core/projects/redmax/Body/BodySDFObj.h b/core/projects/redmax/Body/BodySDFObj.h new file mode 100644 index 0000000..29f419d --- /dev/null +++ b/core/projects/redmax/Body/BodySDFObj.h @@ -0,0 +1,92 @@ +#pragma once +#include "Body/BodyMeshObj.h" +#include "array3.h" +#include "vec.h" + +namespace redmax { + +/** + * BodySDFObj define a body loaded from an .obj file (triangle surface mesh) and compute a SDF for it. + **/ +class BodySDFObj : public BodyMeshObj { +public: + Vector3 _min_box, _max_box; // bounding box of SDF grid + dtype _dx, _dy, _dz; // delta of SDF grid + dtype _col_th; // collision threshold + sdfgen::Array3 _SDF; // voxel grid of SDF + + BodySDFObj(Simulation* sim, Joint* joint, + std::string filename, + Matrix3 R, Vector3 p, + dtype dx, int res, dtype col_th, + TransformType transform_type = BODY_TO_JOINT, + dtype density = (dtype)1.0, + Vector3 scale = Vector3::Ones(), + bool load_sdf = false, + bool save_sdf = false); + + /** + * analytical distance function + * @param: + * xw: the location of the query position in world frame + * @return: contact distance of xw + **/ + dtype distance(Vector3 xw); + + /** + * get contact point on the surface + */ + Vector3 surface_contact_pt(Vector3 xw); + + /** + * analytical distance function return dist, normal, x2i, ddot, tdot + * @return + * dist: the distance of xw + * normal: the normalized contact normal + * xi2: the contact position on this body + * ddot: the magnitude of the velocity on normal direction + * tdot: the tangential velocity + **/ + void collision(Vector3 xw, Vector3 xw_dot, /* input */ + dtype &d, Vector3 &n, /* output */ + dtype &ddot, Vector3 &tdot, + Vector3 &xi2); + + /** + * analytical distance function return dist, normal, x2i, ddot, tdot and derivatives + * @return + * dist: the distance of xw + * normal: the normalized contact normal + * xi2: the contact position on this body + * ddot: the magnitude of the velocity on normal direction + * tdot: the tangential velocity + * derivatives + **/ + void collision(Vector3 xw, Vector3 xw_dot, /* input */ + dtype &d, Vector3 &n, /* output */ + dtype &ddot, Vector3 &tdot, + Vector3 &xi2, + RowVector3 &dd_dxw, RowVector6 &dd_dq2, /* derivatives for d */ + Matrix3 &dn_dxw, Matrix36 &dn_dq2, /* derivatives for n */ + RowVector3 &dddot_dxw, RowVector3 &dddot_dxwdot, /* derivatives for ddot */ + RowVector6 &dddot_dq2, RowVector6 &dddot_dphi2, + Matrix3 &dtdot_dxw, Matrix3 &dtdot_dxwdot, /* derivatives for tdot */ + Matrix36 &dtdot_dq2, Matrix36 &dtdot_dphi2, + Matrix3 &dxi2_dxw, Matrix36 &dxi2_dq2 /* derivatives for xi2 */); + + void test_collision_derivatives(); + void test_collision_derivatives_runtime(Vector3 xw, Vector3 xw_dot); + + void clear_saved_SDF(); + +private: + void precompute_SDF(dtype dx, int res); + bool load_SDF(); + void save_SDF(); + dtype query_SDF(Vector3 x, bool outside_accurate = true); + Vector3 query_dSDF(Vector3 x); + Matrix3 query_ddSDF(Vector3 x); + Vector3i query_grid_location(Vector3 x); +}; + +} diff --git a/core/projects/redmax/CMakeLists.txt b/core/projects/redmax/CMakeLists.txt index fd9cd9d..38d08e8 100644 --- a/core/projects/redmax/CMakeLists.txt +++ b/core/projects/redmax/CMakeLists.txt @@ -15,6 +15,7 @@ target_include_directories(redmax PRIVATE ${SOURCE_DIR}/) target_link_libraries(redmax PRIVATE opengl_viewer) target_link_libraries(redmax PRIVATE pugixml) target_link_libraries(redmax PRIVATE tiny_obj_loader) +target_link_libraries(redmax PRIVATE sdfgen) target_include_directories(redmax_py PRIVATE ${EXTERNAL_HEADER}) target_include_directories(redmax_py PRIVATE ${SOURCE_DIR}/) @@ -23,6 +24,7 @@ target_include_directories(redmax_py PRIVATE ${SOURCE_DIR}/) target_link_libraries(redmax_py PRIVATE opengl_viewer) target_link_libraries(redmax_py PRIVATE pugixml) target_link_libraries(redmax_py PRIVATE tiny_obj_loader) +target_link_libraries(redmax_py PRIVATE sdfgen) # Add libs. target_link_libraries(redmax PRIVATE imgui) diff --git a/core/projects/redmax/CollisionDetection/CollisionDetection.cpp b/core/projects/redmax/CollisionDetection/CollisionDetection.cpp index a449188..0079040 100644 --- a/core/projects/redmax/CollisionDetection/CollisionDetection.cpp +++ b/core/projects/redmax/CollisionDetection/CollisionDetection.cpp @@ -1,6 +1,7 @@ #include "Body/Body.h" #include "Body/BodyCuboid.h" #include "Body/BodyMeshObj.h" +#include "Body/BodySDFObj.h" #include "Body/BodySphere.h" #include "Body/BodyPrimitiveShape.h" #include "CollisionDetection/CollisionDetection.h" @@ -82,4 +83,39 @@ void collision_detection_general_primitive(const Body* contact_body, const Body* } } +// detect the collision between a general body and a SDF body +void collision_detection_general_SDF(const Body* contact_body, const BodySDFObj* SDF_body, std::vector& contacts) { + BodySDFObj* SDF_body_ = const_cast(SDF_body); + + // AABB bounding box check + if (dynamic_cast(const_cast(contact_body)) != nullptr) { + BodySDFObj* contact_sdf_body_ = dynamic_cast(const_cast(contact_body)); + std::pair aabb1 = contact_sdf_body_->get_AABB(); + std::pair aabb2 = SDF_body_->get_AABB(); + for (int axis = 0; axis < 3;axis ++) { + if (aabb1.second[axis] < aabb2.first[axis] - 1e-6 || aabb2.second[axis] < aabb1.first[axis] - 1e-6) { + return; + } + } + } + + std::vector contact_points = contact_body->get_contact_points(); + + // if (dynamic_cast(const_cast(contact_body)) != nullptr) { + // BodyCuboid* cuboid_body_ = dynamic_cast(const_cast(contact_body)); + // contact_points = cuboid_body_->get_general_contact_points(); + // } + + Matrix4 E_w1 = contact_body->_E_0i; + + for (int i = 0;i < contact_points.size();i++) { + Vector3 xw1 = E_w1.topLeftCorner(3, 3) * contact_points[i] + E_w1.topRightCorner(3, 1); + dtype d = SDF_body_->distance(xw1); + if (d < 0.) { + contacts.push_back(Contact(contact_points[i], xw1, d, Vector3::Zero(), i)); + } + } +} + + } \ No newline at end of file diff --git a/core/projects/redmax/CollisionDetection/CollisionDetection.h b/core/projects/redmax/CollisionDetection/CollisionDetection.h index fddef7e..3546a63 100644 --- a/core/projects/redmax/CollisionDetection/CollisionDetection.h +++ b/core/projects/redmax/CollisionDetection/CollisionDetection.h @@ -7,6 +7,7 @@ namespace redmax { class Body; class BodyCuboid; +class BodySDFObj; // detect the collision between ground and a cuboid body // return the list of the contact points represented in body frame. @@ -18,4 +19,7 @@ void collision_detection_cuboid_cuboid(const BodyCuboid* cuboid1, const BodyCubo // the general body should be able to give a list of contact points on the surface. // the primitive body is supposed to have an anlytical distance field void collision_detection_general_primitive(const Body* contact_body, const Body* primitive_body, std::vector& contacts); + +// detect the collision between a general body and a SDF body +void collision_detection_general_SDF(const Body* contact_body, const BodySDFObj* SDF_body, std::vector& contacts); } \ No newline at end of file diff --git a/core/projects/redmax/Force/ForceGeneralSDFContact.cpp b/core/projects/redmax/Force/ForceGeneralSDFContact.cpp new file mode 100644 index 0000000..86e125b --- /dev/null +++ b/core/projects/redmax/Force/ForceGeneralSDFContact.cpp @@ -0,0 +1,1245 @@ +#include "Force/ForceGeneralSDFContact.h" +#include "CollisionDetection/Contact.h" +#include "CollisionDetection/CollisionDetection.h" +#include "Body/Body.h" +#include "Body/BodySDFObj.h" +#include "Body/BodyCuboid.h" +#include "Simulation.h" +#include "Joint/Joint.h" + +namespace redmax { + +ForceGeneralSDFContact::ForceGeneralSDFContact( + Simulation* sim, + Body* contact_body, Body* SDF_body, + dtype kn, dtype kt, dtype mu, dtype damping) : Force(sim) { + _contact_body = contact_body; + _SDF_body = dynamic_cast(SDF_body); + if (_SDF_body == nullptr) { + throw_error("The second body in Contact should be SDF body."); + } + + _kn = kn; + _kt = kt; + _mu = mu; + _damping = damping; + _scale = 1.; +} + +void ForceGeneralSDFContact::set_stiffness(dtype kn, dtype kt) { + _kn = kn; + _kt = kt; +} + +void ForceGeneralSDFContact::set_friction(dtype mu) { + _mu = mu; +} + +void ForceGeneralSDFContact::set_damping(dtype damping) { + _damping = damping; +} + +void ForceGeneralSDFContact::set_scale(dtype scale) { + _scale = scale; +} + +void ForceGeneralSDFContact::computeForce(VectorX& fm, VectorX& fr, bool verbose) { + auto t0 = clock(); + + // detect the contact points between two bodies + std::vector contacts; + contacts.clear(); + + collision_detection_general_SDF(_contact_body, _SDF_body, contacts); + + auto t1 = clock(); + + VectorX fm_sub; + computeForce(contacts, fm_sub, verbose); + + fm.segment(_contact_body->_index[0], 6) += fm_sub.head(6); + fm.segment(_SDF_body->_index[0], 6) += fm_sub.tail(6); + + auto t2 = clock(); + + _time.add("collision detection", t1 - t0); + _time.add("compute force", t2 - t1); +} + +void ForceGeneralSDFContact::computeForceWithDerivative( + VectorX& fm, VectorX& fr, + MatrixX& Km, MatrixX& Dm, + MatrixX& Kr, MatrixX& Dr, + bool verbose) { + + auto t0 = clock(); + + // detect the contact points between two bodies + std::vector contacts; + contacts.clear(); + collision_detection_general_SDF(_contact_body, _SDF_body, contacts); + + auto t1 = clock(); + + VectorX fm_sub; + MatrixX Km_sub, Dm_sub; + computeForceWithDerivative(contacts, fm_sub, Km_sub, Dm_sub, verbose); + + fm.segment(_contact_body->_index[0], 6) += fm_sub.head(6); + fm.segment(_SDF_body->_index[0], 6) += fm_sub.tail(6); + + Km.block(_contact_body->_index[0], _contact_body->_index[0], 6, 6) += Km_sub.topLeftCorner(6, 6); + Km.block(_contact_body->_index[0], _SDF_body->_index[0], 6, 6) += Km_sub.topRightCorner(6, 6); + Km.block(_SDF_body->_index[0], _contact_body->_index[0], 6, 6) += Km_sub.bottomLeftCorner(6, 6); + Km.block(_SDF_body->_index[0], _SDF_body->_index[0], 6, 6) += Km_sub.bottomRightCorner(6, 6); + + Dm.block(_contact_body->_index[0], _contact_body->_index[0], 6, 6) += Dm_sub.topLeftCorner(6, 6); + Dm.block(_contact_body->_index[0], _SDF_body->_index[0], 6, 6) += Dm_sub.topRightCorner(6, 6); + Dm.block(_SDF_body->_index[0], _contact_body->_index[0], 6, 6) += Dm_sub.bottomLeftCorner(6, 6); + Dm.block(_SDF_body->_index[0], _SDF_body->_index[0], 6, 6) += Dm_sub.bottomRightCorner(6, 6); + + auto t2 = clock(); + + _time.add("collision detection", t1 - t0); + _time.add("compute force", t2 - t1); +} + +void ForceGeneralSDFContact::computeForceWithDerivative( + VectorX& fm, VectorX& fr, + MatrixX& Km, MatrixX& Dm, + MatrixX& Kr, MatrixX& Dr, + MatrixX& dfm_dp, MatrixX& dfr_dp, + bool verbose) { + + auto t0 = clock(); + + // detect the contact points between two bodies + std::vector contacts; + contacts.clear(); + collision_detection_general_SDF(_contact_body, _SDF_body, contacts); + + auto t1 = clock(); + + VectorX fm_sub; + MatrixX Km_sub, Dm_sub; + computeForceWithDerivative(contacts, fm_sub, Km_sub, Dm_sub, dfm_dp, verbose); + + fm.segment(_contact_body->_index[0], 6) += fm_sub.head(6); + fm.segment(_SDF_body->_index[0], 6) += fm_sub.tail(6); + + Km.block(_contact_body->_index[0], _contact_body->_index[0], 6, 6) += Km_sub.topLeftCorner(6, 6); + Km.block(_contact_body->_index[0], _SDF_body->_index[0], 6, 6) += Km_sub.topRightCorner(6, 6); + Km.block(_SDF_body->_index[0], _contact_body->_index[0], 6, 6) += Km_sub.bottomLeftCorner(6, 6); + Km.block(_SDF_body->_index[0], _SDF_body->_index[0], 6, 6) += Km_sub.bottomRightCorner(6, 6); + + Dm.block(_contact_body->_index[0], _contact_body->_index[0], 6, 6) += Dm_sub.topLeftCorner(6, 6); + Dm.block(_contact_body->_index[0], _SDF_body->_index[0], 6, 6) += Dm_sub.topRightCorner(6, 6); + Dm.block(_SDF_body->_index[0], _contact_body->_index[0], 6, 6) += Dm_sub.bottomLeftCorner(6, 6); + Dm.block(_SDF_body->_index[0], _SDF_body->_index[0], 6, 6) += Dm_sub.bottomRightCorner(6, 6); + + auto t2 = clock(); + + _time.add("collision detection", t1 - t0); + _time.add("compute force", t2 - t1); +} + +// new damping model (proportional to ddot * d) and friction related to damping force as well +void ForceGeneralSDFContact::computeForce(std::vector &contacts, VectorX& fm, bool verbose) { + Matrix3 R1 = _contact_body->_E_0i.topLeftCorner(3, 3); + Vector6 phi1 = _contact_body->_phi; + Matrix3 R2 = _SDF_body->_E_0i.topLeftCorner(3, 3); + Vector6 phi2 = _SDF_body->_phi; + + fm = VectorX::Zero(12); + + dtype kn = _kn * _scale * _contact_body->_contact_scale; + dtype kt = _kt * _scale * _contact_body->_contact_scale; + for (int i = 0;i < contacts.size();i++) { + Vector3 xi1 = contacts[i]._xi; + Vector3 xw1 = _contact_body->_E_0i.topLeftCorner(3, 3) * xi1 + _contact_body->_E_0i.topRightCorner(3, 1); + Vector3 xw1_dot = R1 * (math::skew(xi1).transpose() * phi1.head(3) + phi1.tail(3)); + + dtype d, ddot; + Vector3 n, tdot, xi2; + _SDF_body->collision(xw1, xw1_dot, d, n, ddot, tdot, xi2); + + if (verbose) { + std::cerr << "General SDF Contact: d = " << d << ", contact body = " << _contact_body->_name << ", SDF body = " << _SDF_body->_name << ", xi1 = " << xi1.transpose() << std::endl; + std::cerr << "normal = " << n.transpose() << std::endl; + std::cerr << "normal.norm = " << n.norm() << std::endl; + std::cerr << "d = " << d << std::endl; + std::cerr << "ddot = " << ddot << std::endl; + std::cerr << "xw1 = " << xw1.transpose() << std::endl; + std::cerr << "centor = " << _SDF_body->_E_0i.topRightCorner(3, 1).transpose() << std::endl; + } + + MatrixX Gamma1 = math::gamma(xi1); + MatrixX Gamma2 = math::gamma(xi2); + + MatrixX GTRT1 = Gamma1.transpose() * R1.transpose(); + Vector6 ni1 = GTRT1 * n; + MatrixX GTRT2 = Gamma2.transpose() * R2.transpose(); + Vector6 ni2 = GTRT2 * n; + + // contact force + Vector6 fc1 = (-kn * d + _damping * ddot * d) * ni1; + fm.head(6) += fc1; + Vector6 fc2 = (kn * d - _damping * ddot * d) * ni2; + fm.tail(6) += fc2; + + if (verbose && dynamic_cast(const_cast(_contact_body)) != nullptr) { + std::cerr << "contact_body = " << _contact_body->_name << ", SDF body = " << _SDF_body->_name << ", xi1 = " << xi1.transpose() << ", fc1 = " << fc1.transpose() << std::endl; + } + + if (verbose) { + std::cerr << "n = " << n.transpose() << ", fc2 = " << fc2.transpose() << std::endl; + } + // frictional force + dtype fc_norm = fc1.norm(); + dtype tdot_norm = tdot.norm(); + if (_mu > constants::eps) { + // general contact body + if (_mu * fc_norm >= kt * tdot_norm - constants::eps) { // static frictional force + Vector6 fs = -kt * GTRT1 * tdot; + fm.head(6) += fs; + } else { // dynamic frictional force + Vector6 fd = -_mu * fc_norm / tdot_norm * GTRT1 * tdot; + fm.head(6) += fd; + } + // SDF contact body + if (_mu * fc_norm >= kt * tdot_norm - constants::eps) { // static frictional force + Vector6 fs = kt * GTRT2 * tdot; + fm.tail(6) += fs; + } else { // dynanmic frictional force + Vector6 fd = _mu * fc_norm / tdot_norm * GTRT2 * tdot; + fm.tail(6) += fd; + } + } + } +} + +void ForceGeneralSDFContact::computeForceWithDerivative( + std::vector &contacts, + VectorX& fm, MatrixX& Km, MatrixX& Dm, + bool verbose) { + + fm = VectorX::Zero(12); + Km = MatrixX::Zero(12, 12); + Dm = MatrixX::Zero(12, 12); + + Matrix3 R1 = _contact_body->_E_0i.topLeftCorner(3, 3); + Vector6 phi1 = _contact_body->_phi; + Matrix3 R2 = _SDF_body->_E_0i.topLeftCorner(3, 3); + Vector6 phi2 = _SDF_body->_phi; + + dtype kn = _kn * _scale * _contact_body->_contact_scale; + dtype kt = _kt * _scale * _contact_body->_contact_scale; + for (int i = 0;i < contacts.size();i++) { + Vector3 xi1 = contacts[i]._xi; + Vector3 xw1 = _contact_body->_E_0i.topLeftCorner(3, 3) * xi1 + _contact_body->_E_0i.topRightCorner(3, 1); + Vector3 xw1_dot = R1 * (math::skew(xi1).transpose() * phi1.head(3) + phi1.tail(3)); + + Matrix36 dxw1_dq1; + dxw1_dq1.leftCols(3) = -R1 * math::skew(xi1); + dxw1_dq1.rightCols(3) = R1; + + Matrix36 dxw1dot_dq1; + dxw1dot_dq1.leftCols(3) = -R1 * math::skew(math::skew(xi1).transpose() * phi1.head(3) + phi1.tail(3)); + dxw1dot_dq1.rightCols(3).setZero(); + + Matrix36 dxw1dot_dphi1; + dxw1dot_dphi1.leftCols(3) = R1 * math::skew(xi1).transpose(); + dxw1dot_dphi1.rightCols(3) = R1; + + dtype d, ddot; + Vector3 n, tdot, xi2; + RowVector3 dd_dxw1, dddot_dxw1, dddot_dxw1dot; + RowVector6 dd_dq2, dddot_dq2, dddot_dphi2; + Matrix3 dn_dxw1, dtdot_dxw1, dtdot_dxw1dot, dxi2_dxw1; + Matrix36 dn_dq2, dtdot_dq2, dtdot_dphi2, dxi2_dq2; + _SDF_body->collision(xw1, xw1_dot, d, n, ddot, tdot, xi2, + dd_dxw1, dd_dq2, + dn_dxw1, dn_dq2, + dddot_dxw1, dddot_dxw1dot, + dddot_dq2, dddot_dphi2, + dtdot_dxw1, dtdot_dxw1dot, + dtdot_dq2, dtdot_dphi2, + dxi2_dxw1, dxi2_dq2); + + Matrix36 dxi2_dq1 = dxi2_dxw1 * dxw1_dq1; + + MatrixX Gamma1 = math::gamma(xi1); + MatrixX Gamma2 = math::gamma(xi2); + + // derivatives + JacobianMatrixVector dGamma2_dxi2(3, 6, 3); + dGamma2_dxi2.setZero(); + dGamma2_dxi2(0)(1, 2) = 1; dGamma2_dxi2(0)(2, 1) = -1; + dGamma2_dxi2(1)(0, 2) = -1; dGamma2_dxi2(1)(2, 0) = 1; + dGamma2_dxi2(2)(0, 1) = 1; dGamma2_dxi2(2)(1, 0) = -1; + + JacobianMatrixVector dGamma2_dq1(3, 6, 6); + dGamma2_dq1.setZero(); + for (int j = 0;j < 6;j++) + for (int k = 0;k < 3;k++) + dGamma2_dq1(j) += dGamma2_dxi2(k) * dxi2_dq1(k, j); + + JacobianMatrixVector dGamma2_dq2(3, 6, 6); + dGamma2_dq2.setZero(); + for (int j = 0;j < 6;j++) + for (int k = 0;k < 3;k++) + dGamma2_dq2(j) += dGamma2_dxi2(k) * dxi2_dq2(k, j); + + MatrixX GTRT1 = Gamma1.transpose() * R1.transpose(); + Vector6 ni1 = GTRT1 * n; + MatrixX GTRT2 = Gamma2.transpose() * R2.transpose(); + Vector6 ni2 = GTRT2 * n; + + // contact force + // contact force on general contact body + Vector6 fc1 = (-kn * d + _damping * ddot * d) * ni1; + + fm.head(6) += fc1; + + // derivatives + RowVector6 dd_dq1 = dd_dxw1 * dxw1_dq1; + RowVector6 dddot_dq1 = dddot_dxw1 * dxw1_dq1 + dddot_dxw1dot * dxw1dot_dq1; + Matrix36 dn_dq1 = dn_dxw1 * dxw1_dq1; + RowVector6 dddot_dphi1 = dddot_dxw1dot * dxw1dot_dphi1; + Matrix36 dtdot_dq1 = dtdot_dxw1 * dxw1_dq1 + dtdot_dxw1dot * dxw1dot_dq1; + Matrix36 dtdot_dphi1 = dtdot_dxw1dot * dxw1dot_dphi1; + + Matrix6 dfc1_dq1 = - ni1 * (kn * dd_dq1 - _damping * dddot_dq1 * d - _damping * ddot * dd_dq1) - + (kn * d - _damping * ddot * d) * GTRT1 * dn_dq1; + dfc1_dq1.leftCols(3) -= (kn * d - _damping * ddot * d) * Gamma1.transpose() * math::skew(R1.transpose() * n); + Km.topLeftCorner(6, 6) += dfc1_dq1; + + Matrix6 dfc1_dq2 = - ni1 * (kn * dd_dq2 - _damping * dddot_dq2 * d - _damping * ddot * dd_dq2) - + (kn * d - _damping * ddot * d) * GTRT1 * dn_dq2; + Km.topRightCorner(6, 6) += dfc1_dq2; + + Matrix6 dfc1_dphi1 = ni1 * _damping * dddot_dphi1 * d; + Dm.topLeftCorner(6, 6) += dfc1_dphi1; + + Matrix6 dfc1_dphi2 = ni1 * _damping * dddot_dphi2 * d; + Dm.topRightCorner(6, 6) += dfc1_dphi2; + + // contact force on SDF contact body + Vector6 fc2 = (kn * d - _damping * ddot * d) * ni2; + + fm.tail(6) += fc2; + + // derivatives + Matrix6 tmp1; + tmp1.setZero(); + for (int j = 0;j < 6;j++) + tmp1.col(j) = dGamma2_dq1(j).transpose() * (R2.transpose() * n); + Km.bottomLeftCorner(6, 6) += ni2 * (kn * dd_dq1 - _damping * dddot_dq1 * d - _damping * ddot * dd_dq1) + + (kn * d - _damping * ddot * d) * (tmp1 + GTRT2 * dn_dq1); + Matrix6 tmp2; + tmp2.setZero(); + for (int j = 0;j < 6;j++) + tmp2.col(j) = dGamma2_dq2(j).transpose() * (R2.transpose() * n); + Km.bottomRightCorner(6, 6) += ni2 * (kn * dd_dq2 - _damping * dddot_dq2 * d - _damping * ddot * dd_dq2) + + (kn * d - _damping * ddot * d) * (tmp2 + GTRT2 * dn_dq2); + Km.block(6, 6, 6, 3) += + (kn * d - _damping * ddot * d) * Gamma2.transpose() * math::skew(R2.transpose() * n); + + Dm.bottomLeftCorner(6, 6) -= ni2 * _damping * dddot_dphi1 * d; + Dm.bottomRightCorner(6, 6) -= ni2 * _damping * dddot_dphi2 * d; + + // frictional force + if (_mu > constants::eps) { + dtype fc_norm = fc1.norm(); + dtype tdot_norm = tdot.norm(); + RowVector6 dfc_norm_dq1 = (1. / fc_norm) * (fc1.transpose() * dfc1_dq1); + RowVector6 dfc_norm_dq2 = (1. / fc_norm) * (fc1.transpose() * dfc1_dq2); + RowVector6 dfc_norm_dphi1 = (1. / fc_norm) * (fc1.transpose() * dfc1_dphi1); + RowVector6 dfc_norm_dphi2 = (1. / fc_norm) * (fc1.transpose() * dfc1_dphi2); + RowVector6 dtdot_norm_dq1 = (1. / tdot_norm) * (tdot.transpose() * dtdot_dq1); + RowVector6 dtdot_norm_dq2 = (1. / tdot_norm) * (tdot.transpose() * dtdot_dq2); + RowVector6 dtdot_norm_dphi1 = (1. / tdot_norm) * (tdot.transpose() * dtdot_dphi1); + RowVector6 dtdot_norm_dphi2 = (1. / tdot_norm) * (tdot.transpose() * dtdot_dphi2); + + // frictional force on the general contact body + if (_mu * fc_norm >= kt * tdot_norm - constants::eps) { // static frictional force + // std::cerr << "static" << std::endl; + Vector6 fs = -kt * GTRT1 * tdot; + fm.head(6) += fs; + + // derivatives + Km.topLeftCorner(6, 6) -= kt * GTRT1 * dtdot_dq1; + Km.block(0, 0, 6, 3) -= kt * Gamma1.transpose() * math::skew(R1.transpose() * tdot); + Km.topRightCorner(6, 6) -= kt * GTRT1 * dtdot_dq2; + + Dm.topLeftCorner(6, 6) -= kt * GTRT1 * dtdot_dphi1; + Dm.topRightCorner(6, 6) -= kt * GTRT1 * dtdot_dphi2; + } else { // dynamic frictional force + // std::cerr << "dynamic" << std::endl; + Vector6 fd = -_mu * fc_norm / tdot_norm * GTRT1 * tdot; + fm.head(6) += fd; + + // derivatives + Km.topLeftCorner(6, 6) -= + _mu / tdot_norm * GTRT1 * (tdot * dfc_norm_dq1 - fc_norm / tdot_norm * tdot * dtdot_norm_dq1 + fc_norm * dtdot_dq1); + Km.block(0, 0, 6, 3) -= + _mu * Gamma1.transpose() * math::skew(fc_norm / tdot_norm * R1.transpose() * tdot); + Km.topRightCorner(6, 6) -= + _mu / tdot_norm * GTRT1 * (tdot * dfc_norm_dq2 - fc_norm / tdot_norm * tdot * dtdot_norm_dq2 + fc_norm * dtdot_dq2); + + Dm.topLeftCorner(6, 6) -= + _mu / tdot_norm * GTRT1 * (tdot * dfc_norm_dphi1 - fc_norm / tdot_norm * tdot * dtdot_norm_dphi1 + fc_norm * dtdot_dphi1); + Dm.topRightCorner(6, 6) -= + _mu / tdot_norm * GTRT1 * (tdot * dfc_norm_dphi2 - fc_norm / tdot_norm * tdot * dtdot_norm_dphi2 + fc_norm * dtdot_dphi2); + } + + // frictional force on the SDF contact body + if (_mu * fc_norm >= kt * tdot_norm - constants::eps) { // static frictional force + Vector6 fs = kt * GTRT2 * tdot; + fm.tail(6) += fs; + + // derivatives + Matrix6 tmp1; + tmp1.setZero(); + for (int j = 0;j < 6;j++) + tmp1.col(j) = dGamma2_dq1(j).transpose() * (R2.transpose() * tdot); + Km.bottomLeftCorner(6, 6) += kt * GTRT2 * dtdot_dq1 + kt * tmp1; + + Matrix6 tmp2; + tmp2.setZero(); + for (int j = 0;j < 6;j++) + tmp2.col(j) = dGamma2_dq2(j).transpose() * (R2.transpose() * tdot); + Km.bottomRightCorner(6, 6) += kt * GTRT2 * dtdot_dq2 + kt * tmp2; + Km.block(6, 6, 6, 3) += kt * Gamma2.transpose() * math::skew(R2.transpose() * tdot); + + Dm.bottomLeftCorner(6, 6) += kt * GTRT2 * dtdot_dphi1; + Dm.bottomRightCorner(6, 6) += kt * GTRT2 * dtdot_dphi2; + } else { // dynanmic frictional force + Vector6 fd = _mu * fc_norm / tdot_norm * GTRT2 * tdot; + fm.tail(6) += fd; + + // derivatives + Matrix6 tmp1; + tmp1.setZero(); + for (int j = 0;j < 6;j++) + tmp1.col(j) = dGamma2_dq1(j).transpose() * (R2.transpose() * tdot); + Km.bottomLeftCorner(6, 6) += + _mu / tdot_norm * GTRT2 * (tdot * dfc_norm_dq1 - tdot * fc_norm / tdot_norm * dtdot_norm_dq1 + fc_norm * dtdot_dq1) + + _mu * fc_norm / tdot_norm * tmp1; + + Matrix6 tmp2; + tmp2.setZero(); + for (int j = 0;j < 6;j++) + tmp2.col(j) = dGamma2_dq2(j).transpose() * (R2.transpose() * tdot); + Km.bottomRightCorner(6, 6) += + _mu / tdot_norm * GTRT2 * (tdot * dfc_norm_dq2 - tdot * fc_norm / tdot_norm * dtdot_norm_dq2 + fc_norm * dtdot_dq2) + + _mu * fc_norm / tdot_norm * tmp2; + Km.block(6, 6, 6, 3) += + _mu * Gamma2.transpose() * math::skew(fc_norm / tdot_norm * R2.transpose() * tdot); + + Dm.bottomLeftCorner(6, 6) += + _mu / tdot_norm * GTRT2 * (tdot * dfc_norm_dphi1 - tdot * fc_norm / tdot_norm * dtdot_norm_dphi1 + fc_norm * dtdot_dphi1); + Dm.bottomRightCorner(6, 6) += + _mu / tdot_norm * GTRT2 * (tdot * dfc_norm_dphi2 - tdot * fc_norm / tdot_norm * dtdot_norm_dphi2 + fc_norm * dtdot_dphi2); + } + } + } +} + +void ForceGeneralSDFContact::computeForceWithDerivative( + std::vector &contacts, + VectorX& fm, MatrixX& Km, MatrixX& Dm, MatrixX& dfm_dp, + bool verbose) { + + fm = VectorX::Zero(12); + Km = MatrixX::Zero(12, 12); + Dm = MatrixX::Zero(12, 12); + + Matrix3 R1 = _contact_body->_E_0i.topLeftCorner(3, 3); + Vector6 phi1 = _contact_body->_phi; + Vector3 w1_dot = phi1.head(3); + Vector3 v1_dot = phi1.tail(3); + Matrix3 R2 = _SDF_body->_E_0i.topLeftCorner(3, 3); + Vector6 phi2 = _SDF_body->_phi; + + // tools for design params 3 + Matrix3 dxw1_dp3 = R1, dxw1dot_dp3 = MatrixX::Zero(3, 3); + for (int k = 0;k < 3;k++) { + dxw1dot_dp3.col(k) = R1 * math::skew(-Vector3::Unit(k)) * w1_dot; + } + + if (verbose) + std::cerr << "num general contacts = " << contacts.size() << std::endl; + + dtype kn = _kn * _scale * _contact_body->_contact_scale; + dtype kt = _kt * _scale * _contact_body->_contact_scale; + for (int i = 0;i < contacts.size();i++) { + Vector3 xi1 = contacts[i]._xi; + Vector3 xw1 = _contact_body->_E_0i.topLeftCorner(3, 3) * xi1 + _contact_body->_E_0i.topRightCorner(3, 1); + Matrix3 xi1_brac = math::skew(xi1); + Vector3 xi1_dot = xi1_brac.transpose() * phi1.head(3) + phi1.tail(3); + Vector3 xw1_dot = R1 * xi1_dot; + int contact_id = contacts[i]._id; + + Matrix36 dxw1_dq1; + dxw1_dq1.leftCols(3) = -R1 * math::skew(xi1); + dxw1_dq1.rightCols(3) = R1; + + Matrix36 dxw1dot_dq1; + dxw1dot_dq1.leftCols(3) = -R1 * math::skew(math::skew(xi1).transpose() * phi1.head(3) + phi1.tail(3)); + dxw1dot_dq1.rightCols(3).setZero(); + + Matrix36 dxw1dot_dphi1; + dxw1dot_dphi1.leftCols(3) = R1 * math::skew(xi1).transpose(); + dxw1dot_dphi1.rightCols(3) = R1; + + dtype d, ddot; + Vector3 n, tdot, xi2; + RowVector3 dd_dxw1, dddot_dxw1, dddot_dxw1dot; + RowVector6 dd_dq2, dddot_dq2, dddot_dphi2; + Matrix3 dn_dxw1, dtdot_dxw1, dtdot_dxw1dot, dxi2_dxw1; + Matrix36 dn_dq2, dtdot_dq2, dtdot_dphi2, dxi2_dq2; + _SDF_body->collision(xw1, xw1_dot, d, n, ddot, tdot, xi2, + dd_dxw1, dd_dq2, + dn_dxw1, dn_dq2, + dddot_dxw1, dddot_dxw1dot, + dddot_dq2, dddot_dphi2, + dtdot_dxw1, dtdot_dxw1dot, + dtdot_dq2, dtdot_dphi2, + dxi2_dxw1, dxi2_dq2); + + Matrix36 dxi2_dq1 = dxi2_dxw1 * dxw1_dq1; + + MatrixX Gamma1 = math::gamma(xi1); + MatrixX Gamma2 = math::gamma(xi2); + + // derivatives + JacobianMatrixVector dGamma2_dxi2(3, 6, 3); + dGamma2_dxi2.setZero(); + dGamma2_dxi2(0)(1, 2) = 1; dGamma2_dxi2(0)(2, 1) = -1; + dGamma2_dxi2(1)(0, 2) = -1; dGamma2_dxi2(1)(2, 0) = 1; + dGamma2_dxi2(2)(0, 1) = 1; dGamma2_dxi2(2)(1, 0) = -1; + + JacobianMatrixVector dGamma2_dq1(3, 6, 6); + dGamma2_dq1.setZero(); + for (int j = 0;j < 6;j++) + for (int k = 0;k < 3;k++) + dGamma2_dq1(j) += dGamma2_dxi2(k) * dxi2_dq1(k, j); + + JacobianMatrixVector dGamma2_dq2(3, 6, 6); + dGamma2_dq2.setZero(); + for (int j = 0;j < 6;j++) + for (int k = 0;k < 3;k++) + dGamma2_dq2(j) += dGamma2_dxi2(k) * dxi2_dq2(k, j); + + MatrixX GTRT1 = Gamma1.transpose() * R1.transpose(); + Vector6 ni1 = GTRT1 * n; + Vector3 n1 = R1.transpose() * n; + MatrixX GTRT2 = Gamma2.transpose() * R2.transpose(); + Vector6 ni2 = GTRT2 * n; + Vector3 n2 = R2.transpose() * n; + + // contact force + // contact force on general contact body + Vector6 fc1 = (-kn * d + _damping * ddot * d) * ni1; + + fm.head(6) += fc1; + + // derivatives + // tools + RowVector6 dd_dq1 = dd_dxw1 * dxw1_dq1; + RowVector6 dddot_dq1 = dddot_dxw1 * dxw1_dq1 + dddot_dxw1dot * dxw1dot_dq1; + Matrix36 dn_dq1 = dn_dxw1 * dxw1_dq1; + RowVector6 dddot_dphi1 = dddot_dxw1dot * dxw1dot_dphi1; + Matrix36 dtdot_dq1 = dtdot_dxw1 * dxw1_dq1 + dtdot_dxw1dot * dxw1dot_dq1; + Matrix36 dtdot_dphi1 = dtdot_dxw1dot * dxw1dot_dphi1; + // tools for design derivatives + MatrixX dxw1_dp1 = MatrixX::Zero(3, 12), dxw1_dp2 = MatrixX::Zero(3, 12); + MatrixX dxw1dot_dp1 = MatrixX::Zero(3, 12), dxw1dot_dp2 = MatrixX::Zero(3, 12); + RowVectorX dd_dp1 = RowVectorX::Zero(_sim->_ndof_p1), dd_dp2 = RowVectorX::Zero(12), dd_dp3 = RowVectorX::Zero(3); + RowVectorX dddot_dp1 = RowVectorX::Zero(_sim->_ndof_p1), dddot_dp2 = RowVectorX::Zero(12), dddot_dp3 = RowVectorX::Zero(3); + MatrixX dn_dp1 = MatrixX::Zero(3, _sim->_ndof_p1), dn_dp2 = MatrixX::Zero(3, 12), dn_dp3 = MatrixX::Zero(3, 3); + MatrixX dxi2_dp1 = MatrixX::Zero(3, _sim->_ndof_p1), dxi2_dp2 = MatrixX::Zero(3, 12), dxi2_dp3 = MatrixX::Zero(3, 3); + MatrixX dtdot_dp1 = MatrixX::Zero(3, _sim->_ndof_p1), dtdot_dp2 = MatrixX::Zero(3, 12), dtdot_dp3 = MatrixX::Zero(3, 3); + + // dxw1_dp1 and dxw1dot_dp1 + // design params 1 + for (auto ancestor = _contact_body->_joint; ancestor != nullptr; ancestor = ancestor->_parent) { + if (ancestor->_design_params_1._active) { + for (int k = 0;k < ancestor->_design_params_1._ndof;k++) { + int idx = ancestor->_design_params_1._param_index(k); + Matrix3 dR_dp1 = _contact_body->_dE0i_dp1(idx).topLeftCorner(3, 3); + Vector3 dp_dp1 = _contact_body->_dE0i_dp1(idx).topRightCorner(3, 1); + Vector6 dphi_dp1 = _contact_body->_dphi_dp1.col(idx); + dxw1_dp1.col(k) = dR_dp1 * xi1 + dp_dp1; + dxw1dot_dp1.col(k) = dR_dp1 * xi1_dot + R1 * (xi1_brac.transpose() * dphi_dp1.head(3) + dphi_dp1.tail(3)); + } + int idx = ancestor->_design_params_1._param_index(0); + int ndof = ancestor->_design_params_1._ndof; + dd_dp1.segment(idx, ndof) = dd_dxw1 * dxw1_dp1; + dddot_dp1.segment(idx, ndof) = dddot_dxw1 * dxw1_dp1 + dddot_dxw1dot * dxw1dot_dp1; + dn_dp1.middleCols(idx, ndof) = dn_dxw1 * dxw1_dp1; + dxi2_dp1.middleCols(idx, ndof) = dxi2_dxw1 * dxw1_dp1; + dtdot_dp1.middleCols(idx, ndof) = dtdot_dxw1 * dxw1_dp1 + dtdot_dxw1dot * dxw1dot_dp1; + } + } + // design params 2 + if (_contact_body->_design_params_2._active) { + for (int k = 0;k < _contact_body->_design_params_2._ndof;k++) { + Matrix3 dR_dp2 = _contact_body->_dE0i_dp2(k).topLeftCorner(3, 3); + Vector3 dp_dp2 = _contact_body->_dE0i_dp2(k).topRightCorner(3, 1); + Vector6 dphi_dp2 = _contact_body->_dphi_dp2.col(k); + dxw1_dp2.col(k) = dR_dp2 * xi1 + dp_dp2; + dxw1dot_dp2.col(k) = dR_dp2 * xi1_dot + R1 * (xi1_brac.transpose() * dphi_dp2.head(3) + dphi_dp2.tail(3)); + } + dd_dp2 = dd_dxw1 * dxw1_dp2; + dddot_dp2 = dddot_dxw1 * dxw1_dp2 + dddot_dxw1dot * dxw1dot_dp2; + dn_dp2 = dn_dxw1 * dxw1_dp2; + dxi2_dp2 = dxi2_dxw1 * dxw1_dp2; + dtdot_dp2 = dtdot_dxw1 * dxw1_dp2 + dtdot_dxw1dot * dxw1dot_dp2; + } + // design params 3 + if (_contact_body->_design_params_3._active) { + dd_dp3 = dd_dxw1 * dxw1_dp3; + dddot_dp3 = dddot_dxw1 * dxw1_dp3 + dddot_dxw1dot * dxw1dot_dp3; + dn_dp3 = dn_dxw1 * dxw1_dp3; + dxi2_dp3 = dxi2_dxw1 * dxw1_dp3; + dtdot_dp3 = dtdot_dxw1 * dxw1_dp3 + dtdot_dxw1dot * dxw1dot_dp3; + } + JacobianMatrixVector dG1_dp3(3, 6, 3), dG2_dp1(3, 6, _sim->_ndof_p1), dG2_dp2(3, 6, 12), dG2_dp3(3, 6, 3); + for (int k = 0;k < 3;k++) { + dG1_dp3(k).leftCols(3) = math::skew(Vector3::Unit(k)).transpose(); + } + for (auto ancestor = _contact_body->_joint; ancestor != nullptr; ancestor = ancestor->_parent) { + if (ancestor->_design_params_1._active) { + for (int k = 0;k < ancestor->_design_params_1._ndof;k++) { + int idx = ancestor->_design_params_1._param_index(k); + for (int l = 0;l < 3;l++) { + dG2_dp1(idx) += dG1_dp3(l) * dxi2_dp1(l, idx); + } + } + } + } + if (_contact_body->_design_params_2._active) { + for (int k = 0;k < _contact_body->_design_params_2._ndof;k++) { + for (int l = 0;l < 3;l++) { + dG2_dp2(k) += dG1_dp3(l) * dxi2_dp2(l, k); + } + } + } + if (_contact_body->_design_params_3._active) { + for (int k = 0;k < 3;k++) + for (int l = 0;l < 3;l++) { + dG2_dp3(k) += dG1_dp3(l) * dxi2_dp3(l, k); + } + } + + // dfc1_dq1 + Matrix6 dfc1_dq1 = - ni1 * (kn * dd_dq1 - _damping * dddot_dq1 * d - _damping * ddot * dd_dq1) - + (kn * d - _damping * ddot * d) * GTRT1 * dn_dq1; + dfc1_dq1.leftCols(3) -= (kn * d - _damping * ddot * d) * Gamma1.transpose() * math::skew(R1.transpose() * n); + Km.topLeftCorner(6, 6) += dfc1_dq1; + + // dfc1_dq2 + Matrix6 dfc1_dq2 = - ni1 * (kn * dd_dq2 - _damping * dddot_dq2 * d - _damping * ddot * dd_dq2) - + (kn * d - _damping * ddot * d) * GTRT1 * dn_dq2; + Km.topRightCorner(6, 6) += dfc1_dq2; + + // dfc1_dphi1 + Matrix6 dfc1_dphi1 = ni1 * _damping * dddot_dphi1 * d; + Dm.topLeftCorner(6, 6) += dfc1_dphi1; + + // dfc1_dphi2 + Matrix6 dfc1_dphi2 = ni1 * _damping * dddot_dphi2 * d; + Dm.topRightCorner(6, 6) += dfc1_dphi2; + + // design derivatives + // dfc1_dp + MatrixX dfc1_dp1 = MatrixX::Zero(6, _sim->_ndof_p1), dfc1_dp2 = MatrixX::Zero(6, 12), dfc1_dp3 = MatrixX::Zero(6, 3), dfc1_dp6 = VectorX::Zero(6); + // design params 1 + for (auto ancestor = _contact_body->_joint; ancestor != nullptr; ancestor = ancestor->_parent) { + if (ancestor->_design_params_1._active) { + for (int k = 0;k < ancestor->_design_params_1._ndof;k++) { + int idx = ancestor->_design_params_1._param_index(k); + Matrix3 dR_dp1 = _contact_body->_dE0i_dp1(idx).topLeftCorner(3, 3); + dfc1_dp1.col(idx) = + -(kn * dd_dp1(idx) - _damping * dddot_dp1(idx) * d - _damping * ddot * dd_dp1(idx)) * ni1 + -(kn * d - _damping * ddot * d) * (Gamma1.transpose() * dR_dp1.transpose() * n + GTRT1 * dn_dp1.col(idx)); + dfm_dp.block(_contact_body->_index[0], idx, 6, 1) += dfc1_dp1.col(idx); + } + } + } + // design params 2 + if (_contact_body->_design_params_2._active) { + for (int k = 0;k < _contact_body->_design_params_2._ndof;k++) { + int idx = _contact_body->_design_params_2._param_index(k) + _sim->_ndof_p1; + Matrix3 dR_dp2 = _contact_body->_dE0i_dp2(k).topLeftCorner(3, 3); + dfc1_dp2.col(k) = + -(kn * dd_dp2(k) - _damping * dddot_dp2(k) * d - _damping * ddot * dd_dp2(k)) * ni1 + -(kn * d - _damping * ddot * d) * (Gamma1.transpose() * dR_dp2.transpose() * n + GTRT1 * dn_dp2.col(k)); + dfm_dp.block(_contact_body->_index[0], idx, 6, 1) += dfc1_dp2.col(k); + } + } + // design params 3 + if (_contact_body->_design_params_3._active) { + for (int k = 0;k < 3;k++) { + int idx = _contact_body->_design_params_3._param_index(contact_id * 3 + k) + _sim->_ndof_p1 + _sim->_ndof_p2; + dfc1_dp3.col(k) = + -(kn * dd_dp3(k) - _damping * dddot_dp3(k) * d - _damping * ddot * dd_dp3(k)) * ni1 + -(kn * d - _damping * ddot * d) * (dG1_dp3(k).transpose() * n1 + GTRT1 * dn_dp3.col(k)); + dfm_dp.block(_contact_body->_index[0], idx, 6, 1) += dfc1_dp3.col(k); + } + } + // design params 6 + int p6_offset = _sim->_ndof_p1 + _sim->_ndof_p2 + _sim->_ndof_p3 + _sim->_ndof_p4 + _sim->_ndof_p5; + if (_contact_body->_design_params_6._active) { + int idx = _contact_body->_design_params_6._param_index(0) + p6_offset; + dfc1_dp6 = -kn * d * ni1 / _contact_body->_contact_scale; + dfm_dp.block(_contact_body->_index[0], idx, 6, 1) += dfc1_dp6; + } + + // contact force on SDF contact body + Vector6 fc2 = (kn * d - _damping * ddot * d) * ni2; + + fm.tail(6) += fc2; + + // derivatives + Matrix6 tmp1; + tmp1.setZero(); + for (int j = 0;j < 6;j++) + tmp1.col(j) = dGamma2_dq1(j).transpose() * (R2.transpose() * n); + Km.bottomLeftCorner(6, 6) += ni2 * (kn * dd_dq1 - _damping * dddot_dq1 * d - _damping * ddot * dd_dq1) + + (kn * d - _damping * ddot * d) * (tmp1 + GTRT2 * dn_dq1); + Matrix6 tmp2; + tmp2.setZero(); + for (int j = 0;j < 6;j++) + tmp2.col(j) = dGamma2_dq2(j).transpose() * (R2.transpose() * n); + Km.bottomRightCorner(6, 6) += ni2 * (kn * dd_dq2 - _damping * dddot_dq2 * d - _damping * ddot * dd_dq2) + + (kn * d - _damping * ddot * d) * (tmp2 + GTRT2 * dn_dq2); + Km.block(6, 6, 6, 3) += + (kn * d - _damping * ddot * d) * Gamma2.transpose() * math::skew(R2.transpose() * n); + + Dm.bottomLeftCorner(6, 6) -= ni2 * _damping * dddot_dphi1 * d; + Dm.bottomRightCorner(6, 6) -= ni2 * _damping * dddot_dphi2 * d; + + // design derivatives + // dfc2_dp + // design params 1 + for (auto ancestor = _contact_body->_joint; ancestor != nullptr; ancestor = ancestor->_parent) { + if (ancestor->_design_params_1._active) { + int idx_start = ancestor->_design_params_1._param_index(0); + int ndof = ancestor->_design_params_1._ndof; + for (int k = 0;k < ndof;k++) { + int idx = idx_start + k; + dfm_dp.block(_SDF_body->_index[0], idx, 6, 1) += + (kn * dd_dp1(idx) - _damping * dddot_dp1(idx) * d - _damping * ddot * dd_dp1(idx)) * ni2 + + (kn * d - _damping * ddot * d) * (dG2_dp1(idx).transpose() * n2 + GTRT2 * dn_dp1.col(idx)); + } + } + } + // design params 2 + if (_contact_body->_design_params_2._active) { + int idx_start = _contact_body->_design_params_2._param_index(0); + int ndof = _contact_body->_design_params_2._ndof; + for (int k = 0;k < ndof;k++) { + int idx = idx_start + k + _sim->_ndof_p1; + dfm_dp.block(_SDF_body->_index[0], idx, 6, 1) += + (kn * dd_dp2(k) - _damping * dddot_dp2(k) * d - _damping * ddot * dd_dp2(k)) * ni2 + + (kn * d - _damping * ddot * d) * (dG2_dp2(k).transpose() * n2 + GTRT2 * dn_dp2.col(k)); + } + } + // design params 3 + if (_contact_body->_design_params_3._active) { + int idx_start = _contact_body->_design_params_3._param_index(contact_id * 3); + for (int k = 0;k < 3;k++) { + int idx = idx_start + k + _sim->_ndof_p1 + _sim->_ndof_p2; + dfm_dp.block(_SDF_body->_index[0], idx, 6, 1) += + (kn * dd_dp3(k) - _damping * dddot_dp3(k) * d - _damping * ddot * dd_dp3(k)) * ni2 + + (kn * d - _damping * ddot * d) * (dG2_dp3(k).transpose() * n2 + GTRT2 * dn_dp3.col(k)); + } + } + // design params 6 + if (_contact_body->_design_params_6._active) { + int idx = _contact_body->_design_params_6._param_index(0) + p6_offset; + dfm_dp.block(_SDF_body->_index[0], idx, 6, 1) += kn * d * ni2 / _contact_body->_contact_scale; + } + + // frictional force + if (_mu > constants::eps) { + dtype fc_norm = fc1.norm(); + dtype tdot_norm = tdot.norm(); + + // frictional force on the general contact body + if (_mu * fc_norm >= kt * tdot_norm - constants::eps) { // static frictional force + // general contact body + Vector6 fs1 = -kt * GTRT1 * tdot; + fm.head(6) += fs1; + + // derivatives + Km.topLeftCorner(6, 6) -= kt * GTRT1 * dtdot_dq1; + Km.block(0, 0, 6, 3) -= kt * Gamma1.transpose() * math::skew(R1.transpose() * tdot); + Km.topRightCorner(6, 6) -= kt * GTRT1 * dtdot_dq2; + + Dm.topLeftCorner(6, 6) -= kt * GTRT1 * dtdot_dphi1; + Dm.topRightCorner(6, 6) -= kt * GTRT1 * dtdot_dphi2; + + // design derivatives + // design params 1 + for (auto ancestor = _contact_body->_joint; ancestor != nullptr; ancestor = ancestor->_parent) { + if (ancestor->_design_params_1._active) { + for (int k = 0;k < ancestor->_design_params_1._ndof;k++) { + int idx = ancestor->_design_params_1._param_index(k); + Matrix3 dR_dp1 = _contact_body->_dE0i_dp1(idx).topLeftCorner(3, 3); + dfm_dp.block(_contact_body->_index[0], idx, 6, 1) += + -kt * Gamma1.transpose() * (dR_dp1.transpose() * tdot + R1.transpose() * dtdot_dp1.col(idx)); + } + } + } + // design params 2 + if (_contact_body->_design_params_2._active) { + for (int k = 0;k < _contact_body->_design_params_2._ndof;k++) { + int idx = _contact_body->_design_params_2._param_index(k) + _sim->_ndof_p1; + Matrix3 dR_dp2 = _contact_body->_dE0i_dp2(k).topLeftCorner(3, 3); + dfm_dp.block(_contact_body->_index[0], idx, 6, 1) += + -kt * Gamma1.transpose() * (dR_dp2.transpose() * tdot + R1.transpose() * dtdot_dp2.col(k)); + } + } + // design params 3 + if (_contact_body->_design_params_3._active) { + for (int k = 0;k < 3;k++) { + int idx = _contact_body->_design_params_3._param_index(contact_id * 3 + k) + _sim->_ndof_p1 + _sim->_ndof_p2; + dfm_dp.block(_contact_body->_index[0], idx, 6, 1) += + - kt * dG1_dp3(k).transpose() * R1.transpose() * tdot - kt * GTRT1 * dtdot_dp3.col(k); + } + } + // design params 6 + if (_contact_body->_design_params_6._active) { + int idx = _contact_body->_design_params_6._param_index(0) + p6_offset; + dfm_dp.block(_contact_body->_index[0], idx, 6, 1) += fs1 / _contact_body->_contact_scale; + } + + // SDF contact body + Vector6 fs2 = kt * GTRT2 * tdot; + fm.tail(6) += fs2; + + // derivatives + Matrix6 tmp1; + tmp1.setZero(); + for (int j = 0;j < 6;j++) + tmp1.col(j) = dGamma2_dq1(j).transpose() * (R2.transpose() * tdot); + Km.bottomLeftCorner(6, 6) += kt * GTRT2 * dtdot_dq1 + kt * tmp1; + + Matrix6 tmp2; + tmp2.setZero(); + for (int j = 0;j < 6;j++) + tmp2.col(j) = dGamma2_dq2(j).transpose() * (R2.transpose() * tdot); + Km.bottomRightCorner(6, 6) += kt * GTRT2 * dtdot_dq2 + kt * tmp2; + Km.block(6, 6, 6, 3) += kt * Gamma2.transpose() * math::skew(R2.transpose() * tdot); + + Dm.bottomLeftCorner(6, 6) += kt * GTRT2 * dtdot_dphi1; + Dm.bottomRightCorner(6, 6) += kt * GTRT2 * dtdot_dphi2; + + // design derivatives + // design params 1 + for (auto ancestor = _contact_body->_joint; ancestor != nullptr; ancestor = ancestor->_parent) { + if (ancestor->_design_params_1._active) { + for (int k = 0;k < ancestor->_design_params_1._ndof;k++) { + int idx = ancestor->_design_params_1._param_index(k); + dfm_dp.block(_SDF_body->_index[0], idx, 6, 1) += + kt * (dG2_dp1(idx).transpose() * R2.transpose() * tdot + GTRT2 * dtdot_dp1.col(idx)); + } + } + } + // design params 2 + if (_contact_body->_design_params_2._active) { + for (int k = 0;k < _contact_body->_design_params_2._ndof;k++) { + int idx = _contact_body->_design_params_2._param_index(k) + _sim->_ndof_p1; + dfm_dp.block(_SDF_body->_index[0], idx, 6, 1) += + kt * (dG2_dp2(k).transpose() * R2.transpose() * tdot + GTRT2 * dtdot_dp2.col(k)); + } + } + // design params 3 + if (_contact_body->_design_params_3._active) { + for (int k = 0;k < 3;k++) { + int idx = _contact_body->_design_params_3._param_index(contact_id * 3 + k) + _sim->_ndof_p1 + _sim->_ndof_p2; + dfm_dp.block(_SDF_body->_index[0], idx, 6, 1) += + kt * (dG2_dp3(k).transpose() * R2.transpose() * tdot + GTRT2 * dtdot_dp3.col(k)); + } + } + // design params 6 + if (_contact_body->_design_params_6._active) { + int idx = _contact_body->_design_params_6._param_index(0) + p6_offset; + dfm_dp.block(_SDF_body->_index[0], idx, 6, 1) += fs2 / _contact_body->_contact_scale; + } + } else { // dynamic frictional force + RowVector6 dfc_norm_dq1 = (1. / fc_norm) * (fc1.transpose() * dfc1_dq1); + RowVector6 dfc_norm_dq2 = (1. / fc_norm) * (fc1.transpose() * dfc1_dq2); + RowVector6 dfc_norm_dphi1 = (1. / fc_norm) * (fc1.transpose() * dfc1_dphi1); + RowVector6 dfc_norm_dphi2 = (1. / fc_norm) * (fc1.transpose() * dfc1_dphi2); + RowVector6 dtdot_norm_dq1 = (1. / tdot_norm) * (tdot.transpose() * dtdot_dq1); + RowVector6 dtdot_norm_dq2 = (1. / tdot_norm) * (tdot.transpose() * dtdot_dq2); + RowVector6 dtdot_norm_dphi1 = (1. / tdot_norm) * (tdot.transpose() * dtdot_dphi1); + RowVector6 dtdot_norm_dphi2 = (1. / tdot_norm) * (tdot.transpose() * dtdot_dphi2); + Vector3 a = tdot / tdot_norm; + Matrix3 A = (Matrix3::Identity() - a * a.transpose()) / tdot_norm; + RowVectorX dfc_norm_dp1 = RowVectorX::Zero(_sim->_ndof_p1), dfc_norm_dp2 = RowVectorX::Zero(12), dfc_norm_dp3 = RowVectorX::Zero(3); + MatrixX da_dp1 = MatrixX::Zero(3, _sim->_ndof_p1), da_dp2 = MatrixX::Zero(3, 12), da_dp3 = MatrixX::Zero(3, 3); + for (auto ancestor = _contact_body->_joint; ancestor != nullptr; ancestor = ancestor->_parent) { + if (ancestor->_design_params_1._active) { + int idx_start = ancestor->_design_params_1._param_index(0); + int ndof = ancestor->_design_params_1._ndof; + dfc_norm_dp1.segment(idx_start, ndof) = fc1.transpose() / fc_norm * dfc1_dp1.middleCols(idx_start, ndof); + da_dp1.middleCols(idx_start, ndof) = A * dtdot_dp1.middleCols(idx_start, ndof); + } + } + if (_contact_body->_design_params_2._active) { + dfc_norm_dp2 = fc1.transpose() / fc_norm * dfc1_dp2; + da_dp2 = A * dtdot_dp2; + } + if (_contact_body->_design_params_3._active) { + dfc_norm_dp3 = fc1.transpose() / fc_norm * dfc1_dp3; + da_dp3 = A * dtdot_dp3; + } + + // general contact body + Vector6 fd1 = -_mu * fc_norm / tdot_norm * GTRT1 * tdot; + fm.head(6) += fd1; + + // derivatives + Km.topLeftCorner(6, 6) -= + _mu / tdot_norm * GTRT1 * (tdot * dfc_norm_dq1 - fc_norm / tdot_norm * tdot * dtdot_norm_dq1 + fc_norm * dtdot_dq1); + Km.block(0, 0, 6, 3) -= + _mu * Gamma1.transpose() * math::skew(fc_norm / tdot_norm * R1.transpose() * tdot); + Km.topRightCorner(6, 6) -= + _mu / tdot_norm * GTRT1 * (tdot * dfc_norm_dq2 - fc_norm / tdot_norm * tdot * dtdot_norm_dq2 + fc_norm * dtdot_dq2); + + Dm.topLeftCorner(6, 6) -= + _mu / tdot_norm * GTRT1 * (tdot * dfc_norm_dphi1 - fc_norm / tdot_norm * tdot * dtdot_norm_dphi1 + fc_norm * dtdot_dphi1); + Dm.topRightCorner(6, 6) -= + _mu / tdot_norm * GTRT1 * (tdot * dfc_norm_dphi2 - fc_norm / tdot_norm * tdot * dtdot_norm_dphi2 + fc_norm * dtdot_dphi2); + + // design derivatives + // design params 1 + for (auto ancestor = _contact_body->_joint; ancestor != nullptr; ancestor = ancestor->_parent) { + if (ancestor->_design_params_1._active) { + for (int k = 0;k < ancestor->_design_params_1._ndof;k++) { + int idx = ancestor->_design_params_1._param_index(k); + Matrix3 dR_dp1 = _contact_body->_dE0i_dp1(idx).topLeftCorner(3, 3); + dfm_dp.block(_contact_body->_index[0], idx, 6, 1) += + -_mu * (Gamma1.transpose() * dR_dp1.transpose() * fc_norm * a + + GTRT1 * (dfc_norm_dp1(idx) * a + fc_norm * da_dp1.col(idx))); + } + } + } + // design params 2 + if (_contact_body->_design_params_2._active) { + for (int k = 0;k < _contact_body->_design_params_2._ndof;k++) { + int idx = _contact_body->_design_params_2._param_index(k) + _sim->_ndof_p1; + Matrix3 dR_dp2 = _contact_body->_dE0i_dp2(k).topLeftCorner(3, 3); + dfm_dp.block(_contact_body->_index[0], idx, 6, 1) += + -_mu * (Gamma1.transpose() * dR_dp2.transpose() * fc_norm * a + + GTRT1 * (dfc_norm_dp2(k) * a + fc_norm * da_dp2.col(k))); + } + } + // design params 3 + if (_contact_body->_design_params_3._active) { + for (int k = 0;k < 3;k++) { + int idx = _contact_body->_design_params_3._param_index(contact_id * 3 + k) + _sim->_ndof_p1 + _sim->_ndof_p2; + dfm_dp.block(_contact_body->_index[0], idx, 6, 1) += + -_mu * (dG1_dp3(k).transpose() * R1.transpose() * fc_norm * a) + -_mu * GTRT1 * (dfc_norm_dp3(k) * a + fc_norm * da_dp3.col(k)); + } + } + // design params 6 + if (_contact_body->_design_params_6._active) { + int idx = _contact_body->_design_params_6._param_index(0) + p6_offset; + dfm_dp.block(_contact_body->_index[0], idx, 6, 1) += fd1 / _contact_body->_contact_scale; + } + + // SDF contact body + Vector6 fd2 = _mu * fc_norm / tdot_norm * GTRT2 * tdot; + fm.tail(6) += fd2; + + // derivatives + Matrix6 tmp1; + tmp1.setZero(); + for (int j = 0;j < 6;j++) + tmp1.col(j) = dGamma2_dq1(j).transpose() * (R2.transpose() * tdot); + Km.bottomLeftCorner(6, 6) += + _mu / tdot_norm * GTRT2 * (tdot * dfc_norm_dq1 - tdot * fc_norm / tdot_norm * dtdot_norm_dq1 + fc_norm * dtdot_dq1) + + _mu * fc_norm / tdot_norm * tmp1; + + Matrix6 tmp2; + tmp2.setZero(); + for (int j = 0;j < 6;j++) + tmp2.col(j) = dGamma2_dq2(j).transpose() * (R2.transpose() * tdot); + Km.bottomRightCorner(6, 6) += + _mu / tdot_norm * GTRT2 * (tdot * dfc_norm_dq2 - tdot * fc_norm / tdot_norm * dtdot_norm_dq2 + fc_norm * dtdot_dq2) + + _mu * fc_norm / tdot_norm * tmp2; + Km.block(6, 6, 6, 3) += + _mu * Gamma2.transpose() * math::skew(fc_norm / tdot_norm * R2.transpose() * tdot); + + Dm.bottomLeftCorner(6, 6) += + _mu / tdot_norm * GTRT2 * (tdot * dfc_norm_dphi1 - tdot * fc_norm / tdot_norm * dtdot_norm_dphi1 + fc_norm * dtdot_dphi1); + Dm.bottomRightCorner(6, 6) += + _mu / tdot_norm * GTRT2 * (tdot * dfc_norm_dphi2 - tdot * fc_norm / tdot_norm * dtdot_norm_dphi2 + fc_norm * dtdot_dphi2); + + // design derivatives + // design params 1 + for (auto ancestor = _contact_body->_joint; ancestor != nullptr; ancestor = ancestor->_parent) { + if (ancestor->_design_params_1._active) { + for (int k = 0;k < ancestor->_design_params_1._ndof;k++) { + int idx = ancestor->_design_params_1._param_index(k); + dfm_dp.block(_SDF_body->_index[0], idx, 6, 1) += + _mu * (dG2_dp1(idx).transpose() * R2.transpose() * fc_norm * a + + GTRT2 * (dfc_norm_dp1(idx) * a + fc_norm * da_dp1.col(idx))); + } + } + } + // design params 2 + if (_contact_body->_design_params_2._active) { + for (int k = 0;k < _contact_body->_design_params_2._ndof;k++) { + int idx = _contact_body->_design_params_2._param_index(k) + _sim->_ndof_p1; + dfm_dp.block(_SDF_body->_index[0], idx, 6, 1) += + _mu * (dG2_dp2(k).transpose() * R2.transpose() * fc_norm * a + + GTRT2 * (dfc_norm_dp2(k) * a + fc_norm * da_dp2.col(k))); + } + } + // design params 3 + if (_contact_body->_design_params_3._active) { + for (int k = 0;k < 3;k++) { + int idx = _contact_body->_design_params_3._param_index(contact_id * 3 + k) + _sim->_ndof_p1 + _sim->_ndof_p2; + dfm_dp.block(_SDF_body->_index[0], idx, 6, 1) += + _mu * (dG2_dp3(k).transpose() * R2.transpose() * fc_norm * a + + GTRT2 * (dfc_norm_dp3(k) * a + fc_norm * da_dp3.col(k))); + } + } + // design params 6 + if (_contact_body->_design_params_6._active) { + int idx = _contact_body->_design_params_6._param_index(0) + p6_offset; + dfm_dp.block(_SDF_body->_index[0], idx, 6, 1) += fd2 / _contact_body->_contact_scale; + } + } + } + } +} + +void ForceGeneralSDFContact::test_derivatives() { + std::cerr << "**************************** General-SDF Collision Derivatives ***************************" << std::endl; + // srand(time(0)); + srand(1000); + + dtype eps = 1e-7; + // generate random xi1, E1, E2, phi1, phi2 + Vector3 xi1 = Vector3::Random() * 10.; + Eigen::Quaternion quat_1(Vector4::Random()); + quat_1.normalize(); + Matrix4 E1 = Matrix4::Identity(); + E1.topLeftCorner(3, 3) = quat_1.toRotationMatrix(); + E1.topRightCorner(3, 1) = Vector3::Random() * 10.; + Eigen::Quaternion quat_2(Vector4::Random()); + quat_2.normalize(); + Matrix4 E2 = Matrix4::Identity(); + E2.topLeftCorner(3, 3) = quat_2.toRotationMatrix(); + E2.topRightCorner(3, 1) = Vector3::Random() * 10.; + Vector6 phi1 = Vector6::Random() * 10.; + Vector6 phi2 = Vector6::Random() * 10.; + Vector3 xw1 = E1.topLeftCorner(3, 3) * xi1 + E1.topRightCorner(3, 1); + + this->_contact_body->_E_0i = E1; + this->_contact_body->_phi = phi1; + this->_SDF_body->_E_0i = E2; + this->_SDF_body->_phi = phi2; + + std::vector contacts; + contacts.push_back(Contact(xi1, xw1, 0.0, Vector3::Zero())); + + VectorX fm; + MatrixX Km, Dm; + computeForceWithDerivative(contacts, fm, Km, Dm, true); + + // Km + MatrixX Km_fd = MatrixX::Zero(12, 12); + for (int i = 0;i < 6;i++) { + Vector6 dq = Vector6::Zero(); + dq[i] = eps; + Matrix4 E1_pos = E1 * math::exp(dq); + this->_contact_body->_E_0i = E1_pos; + VectorX fm_pos; + computeForce(contacts, fm_pos); + Km_fd.col(i) = (fm_pos - fm) / eps; + } + this->_contact_body->_E_0i = E1; + + print_error("Km(q1) ", Km.leftCols(6), Km_fd.leftCols(6)); + + for (int i = 0;i < 6;i++) { + Vector6 dq = Vector6::Zero(); + dq[i] = eps; + Matrix4 E2_pos = E2 * math::exp(dq); + this->_SDF_body->_E_0i = E2_pos; + VectorX fm_pos; + computeForce(contacts, fm_pos); + Km_fd.col(6 + i) = (fm_pos - fm) / eps; + } + this->_SDF_body->_E_0i = E2; + print_error("Km(q2) ", Km.rightCols(6), Km_fd.rightCols(6)); + + // Dm + MatrixX Dm_fd = MatrixX::Zero(12, 12); + for (int i = 0;i < 6;i++) { + Vector6 phi1_pos = phi1; + phi1_pos[i] += eps; + this->_contact_body->_phi = phi1_pos; + VectorX fm_pos; + computeForce(contacts, fm_pos); + Dm_fd.col(i) = (fm_pos - fm) / eps; + } + this->_contact_body->_phi = phi1; + print_error("Dm(phi1) ", Dm.leftCols(6), Dm_fd.leftCols(6)); + + for (int i = 0;i < 6;i++) { + Vector6 phi2_pos = phi2; + phi2_pos[i] += eps; + this->_SDF_body->_phi = phi2_pos; + VectorX fm_pos; + computeForce(contacts, fm_pos); + Dm_fd.col(6 + i) = (fm_pos - fm) / eps; + } + this->_SDF_body->_phi = phi2; + print_error("Dm(phi2) ", Dm.rightCols(6), Dm_fd.rightCols(6)); +} + +void ForceGeneralSDFContact::test_derivatives_runtime() { + // detect the contact points between two bodies + std::vector contacts; + contacts.clear(); + collision_detection_general_SDF(_contact_body, _SDF_body, contacts); + + // test derivative for SDF body + for (int i = 0;i < contacts.size();i++) { + Vector3 xi1 = contacts[i]._xi; + Vector3 xw1 = _contact_body->_E_0i.topLeftCorner(3, 3) * xi1 + _contact_body->_E_0i.topRightCorner(3, 1); + Vector3 xw1_dot = _contact_body->_E_0i.topLeftCorner(3, 3) * (math::skew(xi1).transpose() * _contact_body->_phi.head(3) + _contact_body->_phi.tail(3)); + _SDF_body->test_collision_derivatives_runtime(xw1, xw1_dot); + } + + VectorX fm; + MatrixX Km, Dm; + computeForceWithDerivative(contacts, fm, Km, Dm); + + dtype eps = 1e-7; + // generate random xi1, E1, E2, phi1, phi2 + MatrixX E1 = this->_contact_body->_E_0i; + VectorX phi1 = this->_contact_body->_phi; + MatrixX E2 = this->_SDF_body->_E_0i; + VectorX phi2 = this->_SDF_body->_phi; + + // Km + MatrixX Km_fd = MatrixX::Zero(12, 12); + for (int i = 0;i < 6;i++) { + Vector6 dq = Vector6::Zero(); + dq[i] = eps; + Matrix4 E1_pos = E1 * math::exp(dq); + this->_contact_body->_E_0i = E1_pos; + VectorX fm_pos; + computeForce(contacts, fm_pos); + Km_fd.col(i) = (fm_pos - fm) / eps; + } + this->_contact_body->_E_0i = E1; + + print_error("General-SDF Collision Derivatives: Km(q1) ", Km.leftCols(6), Km_fd.leftCols(6)); + + for (int i = 0;i < 6;i++) { + Vector6 dq = Vector6::Zero(); + dq[i] = eps; + Matrix4 E2_pos = E2 * math::exp(dq); + this->_SDF_body->_E_0i = E2_pos; + VectorX fm_pos; + computeForce(contacts, fm_pos); + Km_fd.col(6 + i) = (fm_pos - fm) / eps; + } + this->_SDF_body->_E_0i = E2; + print_error("General-SDF Collision Derivatives: Km(q2) ", Km.rightCols(6), Km_fd.rightCols(6)); + + // Dm + MatrixX Dm_fd = MatrixX::Zero(12, 12); + for (int i = 0;i < 6;i++) { + Vector6 phi1_pos = phi1; + phi1_pos[i] += eps; + this->_contact_body->_phi = phi1_pos; + VectorX fm_pos; + computeForce(contacts, fm_pos); + Dm_fd.col(i) = (fm_pos - fm) / eps; + } + this->_contact_body->_phi = phi1; + print_error("General-SDF Collision Derivatives: Dm(phi1) ", Dm.leftCols(6), Dm_fd.leftCols(6)); + + for (int i = 0;i < 6;i++) { + Vector6 phi2_pos = phi2; + phi2_pos[i] += eps; + this->_SDF_body->_phi = phi2_pos; + VectorX fm_pos; + computeForce(contacts, fm_pos); + Dm_fd.col(6 + i) = (fm_pos - fm) / eps; + } + this->_SDF_body->_phi = phi2; + print_error("General-SDF Collision Derivatives: Dm(phi2) ", Dm.rightCols(6), Dm_fd.rightCols(6)); + + test_design_derivatives_runtime(); +} + +void ForceGeneralSDFContact::test_design_derivatives_runtime() { + // detect the contact points between two bodies + std::vector contacts; + contacts.clear(); + + collision_detection_general_SDF(_contact_body, _SDF_body, contacts); + + if (contacts.size() > 0) { + VectorX fm = VectorX::Zero(_sim->_ndof_m), fm_sub; + MatrixX Km_sub, Dm_sub; + MatrixX dfm_dp = MatrixX::Zero(_sim->_ndof_m, _sim->_ndof_p); + computeForceWithDerivative(contacts, fm_sub, Km_sub, Dm_sub, dfm_dp, false); + fm.segment(_contact_body->_index[0], 6) += fm_sub.head(6); + fm.segment(_SDF_body->_index[0], 6) += fm_sub.tail(6); + + VectorX design_params = _sim->get_design_params(); + + dtype eps = 1e-7; + for (int ii = 0;ii < 1;ii++) { + MatrixX dfm_dp_fd = MatrixX::Zero(_sim->_ndof_m, _sim->_ndof_p); + + for (int i = 0;i < _sim->_ndof_p;i++) { + VectorX design_params_pos = design_params; + design_params_pos(i) += eps; + std::vector contacts_pos = contacts; + + _sim->set_design_params(design_params_pos); + _sim->update_robot(false); + std::vector all_contact_points = _contact_body->get_contact_points(); + for (int j = 0;j < contacts.size();j++) + contacts_pos[j]._xi = all_contact_points[contacts_pos[j]._id]; + + VectorX fm_pos = VectorX::Zero(_sim->_ndof_m), fm_sub_pos; + MatrixX Km_pos, Dm_pos; + MatrixX dfm_dp_pos = MatrixX::Zero(_sim->_ndof_m, _sim->_ndof_p); + computeForceWithDerivative(contacts_pos, fm_sub_pos, Km_pos, Dm_pos, dfm_dp_pos, false); + fm_pos.segment(_contact_body->_index[0], 6) += fm_sub_pos.head(6); + fm_pos.segment(_SDF_body->_index[0], 6) += fm_sub_pos.tail(6); + dfm_dp_fd.col(i) = (fm_pos - fm) / eps; + } + // std::cerr << "eps = " << eps << std::endl; + print_error("General-SDF Collision Derivatives: dfm_dp1", dfm_dp.leftCols(_sim->_ndof_p1), dfm_dp_fd.leftCols(_sim->_ndof_p1)); + print_error("General-SDF Collision Derivatives: dfm_dp2", dfm_dp.middleCols(_sim->_ndof_p1, _sim->_ndof_p2), dfm_dp_fd.middleCols(_sim->_ndof_p1, _sim->_ndof_p2)); + print_error("General-SDF Collision Derivatives: dfm_dp3", dfm_dp.middleCols(_sim->_ndof_p1 + _sim->_ndof_p2, _sim->_ndof_p3), dfm_dp_fd.middleCols(_sim->_ndof_p1 + _sim->_ndof_p2, _sim->_ndof_p3)); + print_error("General-SDF Collision Derivatives: dfm_dp6", dfm_dp.middleCols(_sim->_ndof_p1 + _sim->_ndof_p2 + _sim->_ndof_p3 + _sim->_ndof_p4 + _sim->_ndof_p5, _sim->_ndof_p6), dfm_dp_fd.middleCols(_sim->_ndof_p1 + _sim->_ndof_p2 + _sim->_ndof_p3 + _sim->_ndof_p4 + _sim->_ndof_p5, _sim->_ndof_p6)); + eps /= 10.; + } + + _sim->set_design_params(design_params); + _sim->update_robot(true); + } +} + +} \ No newline at end of file diff --git a/core/projects/redmax/Force/ForceGeneralSDFContact.h b/core/projects/redmax/Force/ForceGeneralSDFContact.h new file mode 100644 index 0000000..302e6ac --- /dev/null +++ b/core/projects/redmax/Force/ForceGeneralSDFContact.h @@ -0,0 +1,56 @@ +#pragma once +#include "Common.h" +#include "Utils.h" +#include "Force/Force.h" +#include "CollisionDetection/Contact.h" + +namespace redmax { + +class Body; +class BodySDFObj; + +class ForceGeneralSDFContact : public Force { +public: + Body* _contact_body; // the general contact body should be able to give a list of contact points on the surface. + BodySDFObj* _SDF_body; // the SDF body is supposed to have a distance field + dtype _kn; // normal stiffness + dtype _kt; // tangential stiffness + dtype _mu; // coefficient of friction + dtype _damping; // damping of the contact force + dtype _scale; // a scale parameter to control the stiffness (for continuation method) + + ForceGeneralSDFContact( + Simulation* sim, + Body* contact_body, Body* SDF_body, + dtype kn = 1., dtype kt = 0., + dtype mu = 0., dtype damping = 0.); + + void set_stiffness(dtype kn, dtype kt); + void set_friction(dtype mu); + void set_damping(dtype damping); + void set_scale(dtype scale); + + void computeForce(VectorX& fm, VectorX& fr, bool verbose = false); + void computeForceWithDerivative( + VectorX& fm, VectorX& fr, + MatrixX& Km, MatrixX& Dm, + MatrixX& Kr, MatrixX& Dr, + bool verbose = false); + void computeForceWithDerivative( + VectorX& fm, VectorX& fr, + MatrixX& Km, MatrixX& Dm, + MatrixX& Kr, MatrixX& Dr, + MatrixX& dfm_dp, MatrixX& dfr_dp, + bool verbose = false); + + void test_derivatives(); + void test_derivatives_runtime(); + void test_design_derivatives_runtime(); + +protected: + void computeForce(std::vector &contacts, VectorX& fm, bool verbose = false); + void computeForceWithDerivative(std::vector &contacts, VectorX& fm, MatrixX& Km, MatrixX& Dm, bool verbose = false); + void computeForceWithDerivative(std::vector &contacts, VectorX& fm, MatrixX& Km, MatrixX& Dm, MatrixX& dfm_dp, bool verbose = false); +}; + +} \ No newline at end of file diff --git a/core/projects/redmax/Simulation.cpp b/core/projects/redmax/Simulation.cpp index cc24152..81089e4 100644 --- a/core/projects/redmax/Simulation.cpp +++ b/core/projects/redmax/Simulation.cpp @@ -1086,6 +1086,12 @@ void Simulation::forward(int num_steps, bool verbose, bool test_derivatives, boo } } else if (_options->_integrator == "SDIRK2") { integration_SDIRK2(q, qdot, _options->_h, q_next, qdot_next); + } else if (_options->_integrator == "Euler") { + integration_Euler(q, qdot, _options->_h, q_next, qdot_next); + } else if (_options->_integrator == "Midpoint") { + integration_Midpoint(q, qdot, _options->_h, q_next, qdot_next); + } else if (_options->_integrator == "RK4") { + integration_RK4(q, qdot, _options->_h, q_next, qdot_next); } else { std::cerr << "[Error] Integrator " << _options->_integrator << " has not been implemented." << std::endl; throw "error"; @@ -1322,6 +1328,87 @@ void Simulation::evaluate_g_with_derivatives_BDF1(const VectorX& q1, VectorX& g, } } +void Simulation::integration_Euler( + const VectorX q0, const VectorX qdot0, const dtype h, + VectorX& q1, VectorX& qdot1) { + _q0 = q0; + _qdot0 = qdot0; + _h = h; + MatrixX M; + VectorX f; + + set_state(q0, qdot0); + update_robot(); + computeMatrices(M, f); + + q1 = q0 + h * qdot0; + qdot1 = qdot0 + h * M.inverse() * f; +} + +void Simulation::integration_Midpoint( + const VectorX q0, const VectorX qdot0, const dtype h, + VectorX& q1, VectorX& qdot1) { + _q0 = q0; + _qdot0 = qdot0; + _h = h; + MatrixX M; + VectorX f; + VectorX q0_5, qdot0_5; + + set_state(q0, qdot0); + update_robot(); + computeMatrices(M, f); + + q0_5 = q0 + h / 2. * qdot0; + qdot0_5 = qdot0 + h / 2. * M.inverse() * f; + + set_state(q0_5, qdot0_5); + update_robot(); + computeMatrices(M, f); + + q1 = q0 + h * qdot0_5; + qdot1 = qdot0 + h * M.inverse() * f; +} + +void Simulation::integration_RK4( + const VectorX q0, const VectorX qdot0, const dtype h, + VectorX& q1, VectorX& qdot1) { + _q0 = q0; + _qdot0 = qdot0; + _h = h; + MatrixX M; + VectorX f; + VectorX kq1, kq2, kq3, kq4; + VectorX kqdot1, kqdot2, kqdot3, kqdot4; + + kq1 = qdot0; + set_state(q0, qdot0); + update_robot(); + computeMatrices(M, f); + kqdot1 = M.inverse() * f; + + kq2 = qdot0 + h / 2. * kqdot1; + set_state(q0 + h / 2. * kq1, kq2); + update_robot(); + computeMatrices(M, f); + kqdot2 = M.inverse() * f; + + kq3 = qdot0 + h / 2. * kqdot2; + set_state(q0 + h / 2. * kq2, kq3); + update_robot(); + computeMatrices(M, f); + kqdot3 = M.inverse() * f; + + kq4 = qdot0 + h * kqdot3; + set_state(q0 + h * kq3, kq4); + update_robot(); + computeMatrices(M, f); + kqdot4 = M.inverse() * f; + + q1 = q0 + h / 6. * (kq1 + 2 * kq2 + 2 * kq3 + kq4); + qdot1 = qdot0 + h / 6. * (kqdot1 + 2 * kqdot2 + 2 * kqdot3 + kqdot4); +} + void Simulation::integration_BDF1( const VectorX q0, const VectorX qdot0, const dtype h, VectorX& q1, VectorX& qdot1) { diff --git a/core/projects/redmax/Simulation.h b/core/projects/redmax/Simulation.h index 36f854a..089f63f 100644 --- a/core/projects/redmax/Simulation.h +++ b/core/projects/redmax/Simulation.h @@ -87,6 +87,9 @@ class Simulation { void integration_BDF1(const VectorX q0, const VectorX qdot0, const dtype h, VectorX& q1, VectorX& qdot1); void integration_SDIRK2(const VectorX q0, const VectorX qdot0, const dtype h, VectorX& q1, VectorX& qdot1); void integration_BDF2(const VectorX q0, const VectorX qdot0, const VectorX q1, const VectorX qdot1, const dtype h, VectorX& q2, VectorX& qdot2); + void integration_Euler(const VectorX q0, const VectorX qdot0, const dtype h, VectorX& q1, VectorX& qdot1); + void integration_Midpoint(const VectorX q0, const VectorX qdot0, const dtype h, VectorX& q1, VectorX& qdot1); + void integration_RK4(const VectorX q0, const VectorX qdot0, const dtype h, VectorX& q1, VectorX& qdot1); // backward methods void backward_BDF1(); diff --git a/core/projects/redmax/Simulation_Constructor.cpp b/core/projects/redmax/Simulation_Constructor.cpp index ddb05e3..2183023 100644 --- a/core/projects/redmax/Simulation_Constructor.cpp +++ b/core/projects/redmax/Simulation_Constructor.cpp @@ -12,6 +12,7 @@ #include "Body/BodySphere.h" #include "Body/BodyAbstract.h" #include "Body/BodyCapsule.h" +#include "Body/BodySDFObj.h" #include "Joint/JointFixed.h" #include "Joint/JointRevolute.h" #include "Joint/JointPrismatic.h" @@ -25,6 +26,7 @@ #include "Force/ForceGroundContact.h" #include "Force/ForceCuboidCuboidContact.h" #include "Force/ForceGeneralPrimitiveContact.h" +#include "Force/ForceGeneralSDFContact.h" #include "Actuator/Actuator.h" #include "Actuator/ActuatorMotor.h" #include "EndEffector/EndEffector.h" @@ -337,6 +339,19 @@ Joint* Simulation::parse_from_xml_file(pugi::xml_node root, pugi::xml_node node, } ForceGeneralPrimitiveContact* force = new ForceGeneralPrimitiveContact(this, it1->second, it2->second, kn, kt, mu, damping, render_contact_points); robot->add_force(force); + } else if ((std::string)(child.name()) == "general_SDF_contact") { + auto it1 = _body_map.find(child.attribute("general_body").value()); + if (it1 == _body_map.end()) { + std::string error_msg = "General contact body name error: " + (std::string)(child.attribute("general_body").value()); + throw_error(error_msg); + } + auto it2 = _body_map.find(child.attribute("SDF_body").value()); + if (it2 == _body_map.end()) { + std::string error_msg = "SDF contact body name error: " + (std::string)(child.attribute("SDF_body").value()); + throw_error(error_msg); + } + ForceGeneralSDFContact* force = new ForceGeneralSDFContact(this, it1->second, it2->second, kn, kt, mu, damping); + robot->add_force(force); } } } @@ -597,7 +612,7 @@ Joint* Simulation::parse_from_xml_file(pugi::xml_node root, pugi::xml_node node, } else if (type == "sphere") { dtype radius = (dtype)body_node.attribute("radius").as_float(); body = new BodySphere(this, joint, radius, R, pos, density); - } else if (type == "mesh") { + } else if (type == "mesh" || type == "SDF") { BodyMeshObj::TransformType transform_type = BodyMeshObj::TransformType::BODY_TO_JOINT; if (body_node.attribute("transform_type")) { std::string transform_type_str = body_node.attribute("transform_type").value(); @@ -612,9 +627,37 @@ Joint* Simulation::parse_from_xml_file(pugi::xml_node root, pugi::xml_node node, throw_error(error_msg); } } + Vector3 scale = Vector3::Ones(); + if (body_node.attribute("scale")) { + scale = str_to_eigen(body_node.attribute("scale").value()); + } std::string filename = body_node.attribute("filename").value(); filename = _asset_folder + "//" + filename; - body = new BodyMeshObj(this, joint, filename, R, pos, transform_type, density, scale); + if (type == "mesh") { + body = new BodyMeshObj(this, joint, filename, R, pos, transform_type, density, scale); + } else if (type == "SDF") { + dtype dx = 0.05; + if (body_node.attribute("dx")) { + dx = (dtype)body_node.attribute("dx").as_float(); + } + int res = 20; + if (body_node.attribute("res")) { + res = body_node.attribute("res").as_int(); + } + dtype col_th = 0.0; + if (body_node.attribute("col_th")) { + col_th = (dtype)body_node.attribute("col_th").as_float(); + } + bool load_sdf = false; + if (body_node.attribute("load_sdf")) { + load_sdf = body_node.attribute("load_sdf").as_bool(); + } + bool save_sdf = false; + if (body_node.attribute("save_sdf")) { + save_sdf = body_node.attribute("save_sdf").as_bool(); + } + body = new BodySDFObj(this, joint, filename, R, pos, dx, res, col_th, transform_type, density, scale, load_sdf, save_sdf); + } } else if (type == "abstract") { dtype mass = (dtype)body_node.attribute("mass").as_float(); From 6799ef3faf0c2b875e6838f17c529738dd2ad0a7 Mon Sep 17 00:00:00 2001 From: idan shenfeld Date: Fri, 14 Apr 2023 18:46:00 -0400 Subject: [PATCH 2/2] Add get_ground_force and get_joint_torques to the python interface Still had bug in get_ground_force --- .../redmax/Actuator/ActuatorMotor.cpp | 2 ++ core/projects/redmax/Actuator/ActuatorMotor.h | 1 + .../redmax/Force/ForceGroundContact.cpp | 3 ++- .../redmax/Force/ForceGroundContact.h | 1 + core/projects/redmax/Robot.cpp | 9 ++++++++ core/projects/redmax/Robot.h | 2 ++ core/projects/redmax/Simulation.cpp | 22 ++++++++++++++++++- core/projects/redmax/Simulation.h | 2 ++ core/projects/redmax/python_interface.cpp | 8 ++++++- 9 files changed, 47 insertions(+), 3 deletions(-) diff --git a/core/projects/redmax/Actuator/ActuatorMotor.cpp b/core/projects/redmax/Actuator/ActuatorMotor.cpp index 6aa7395..f0affa8 100644 --- a/core/projects/redmax/Actuator/ActuatorMotor.cpp +++ b/core/projects/redmax/Actuator/ActuatorMotor.cpp @@ -32,11 +32,13 @@ void ActuatorMotor::computeForce(VectorX& fm, VectorX& fr) { if (_control_mode == ControlMode::FORCE) { VectorX u = _u.cwiseMin(VectorX::Ones(_joint->_ndof)).cwiseMax(VectorX::Ones(_joint->_ndof) * -1.); _fr = map_value(u, VectorX::Constant(_joint->_ndof, -1), VectorX::Ones(_joint->_ndof, 1), _ctrl_min, _ctrl_max).cwiseMin(_ctrl_max).cwiseMax(_ctrl_min); + _current_torque = _fr; fr.segment(_joint->_index[0], _joint->_ndof) += _fr; } else { _pos_error = _u - _dofs; _vel_error = - _dofs_vel; _fr = (_ctrl_P.cwiseProduct(_pos_error) + _ctrl_D.cwiseProduct(_vel_error)).cwiseMin(_ctrl_max).cwiseMax(_ctrl_min); + _current_torque = _fr; fr.segment(_joint->_index[0], _joint->_ndof) += _fr; } } diff --git a/core/projects/redmax/Actuator/ActuatorMotor.h b/core/projects/redmax/Actuator/ActuatorMotor.h index 9a20fda..41629ca 100644 --- a/core/projects/redmax/Actuator/ActuatorMotor.h +++ b/core/projects/redmax/Actuator/ActuatorMotor.h @@ -14,6 +14,7 @@ class ActuatorMotor : public Actuator { VectorX _ctrl_P, _ctrl_D; // stored temporary variables VectorX _pos_error, _vel_error; + VectorX _current_torque; ActuatorMotor(std::string name, Joint* joint, ControlMode control_mode, VectorX ctrl_min, VectorX ctrl_max, diff --git a/core/projects/redmax/Force/ForceGroundContact.cpp b/core/projects/redmax/Force/ForceGroundContact.cpp index e925ac7..11b32cf 100644 --- a/core/projects/redmax/Force/ForceGroundContact.cpp +++ b/core/projects/redmax/Force/ForceGroundContact.cpp @@ -143,7 +143,8 @@ void ForceGroundContact::computeForce(std::vector &contacts, VectorX& f // std::cerr << "fd = " << fd.transpose() << std::endl; fm.segment(_contact_body->_index[0], 6).noalias() += J.transpose() * fd; } - } + } + _fm = fm.segment(_contact_body->_index[0], 6); } void ForceGroundContact::computeForceWithDerivative(std::vector &contacts, diff --git a/core/projects/redmax/Force/ForceGroundContact.h b/core/projects/redmax/Force/ForceGroundContact.h index 4e2df68..70cc684 100644 --- a/core/projects/redmax/Force/ForceGroundContact.h +++ b/core/projects/redmax/Force/ForceGroundContact.h @@ -14,6 +14,7 @@ class ForceGroundContact : public Force { dtype _kt; // tangential stiffness dtype _mu; // coefficient of friction dtype _damping; // damping of the contact force + VectorX _fm; // contact force in maximum coordinate ForceGroundContact(Simulation* sim, Body* contact_body, Matrix4 E_g, dtype kn = 1., dtype kt = 0., dtype mu = 0., dtype damping = 0.); diff --git a/core/projects/redmax/Robot.cpp b/core/projects/redmax/Robot.cpp index 39da058..fb104dc 100644 --- a/core/projects/redmax/Robot.cpp +++ b/core/projects/redmax/Robot.cpp @@ -3,6 +3,7 @@ #include "Body/Body.h" #include "Force/Force.h" #include "Actuator/Actuator.h" +#include "Actuator/ActuatorMotor.h" #include "EndEffector/EndEffector.h" #include "VirtualObject/VirtualObject.h" #include "Joint/JointDesignParameters.h" @@ -14,6 +15,14 @@ namespace redmax { +vector Robot::get_joint_torques(){ + vector joint_torques; + for (auto actuator : _actuators) { + joint_torques.push_back(dynamic_cast(actuator)->_current_torque); + } + return joint_torques; +} + void Robot::add_tactile_sensor(TactileSensor* tactile_sensor) { _tactile_sensors.push_back(tactile_sensor); } diff --git a/core/projects/redmax/Robot.h b/core/projects/redmax/Robot.h index d195e5b..b815686 100644 --- a/core/projects/redmax/Robot.h +++ b/core/projects/redmax/Robot.h @@ -41,6 +41,8 @@ class Robot { _end_effectors.clear(); } + vector get_joint_torques(); + void add_tactile_sensor(TactileSensor* tactile_sensor); void add_force(Force* force); diff --git a/core/projects/redmax/Simulation.cpp b/core/projects/redmax/Simulation.cpp index 81089e4..d0a1f08 100644 --- a/core/projects/redmax/Simulation.cpp +++ b/core/projects/redmax/Simulation.cpp @@ -4,11 +4,13 @@ #include "Joint/Joint.h" #include "SimViewer.h" #include "Force/Force.h" +#include "Force/ForceGroundContact.h" #include "Force/ForceGeneralPrimitiveContact.h" #include "VirtualObject/VirtualObject.h" #include "Sensor/TactileSensor.h" #include "EndEffector/EndEffector.h" - +#include +using namespace std; namespace redmax { Simulation::~Simulation() { @@ -143,6 +145,24 @@ void Simulation::print_ctrl_info() { _robot->print_ctrl_info(); } +vector Simulation::get_joint_torques() { + return _robot->get_joint_torques(); +} + +VectorX Simulation::get_ground_force(std::string body_name) { + VectorX fm; + for (auto force : _robot->_forces) { + if (dynamic_cast(const_cast(force)) != nullptr) { + // cout << dynamic_cast(const_cast(force))->_contact_body->_name << endl; + if (dynamic_cast(const_cast(force))->_contact_body->_name == body_name) { + fm = dynamic_cast(force)->_fm; + // cout << fm << endl; + } + } + } + return fm; +} + /** tactile related functions **/ // get parameters diff --git a/core/projects/redmax/Simulation.h b/core/projects/redmax/Simulation.h index 089f63f..4fdb20f 100644 --- a/core/projects/redmax/Simulation.h +++ b/core/projects/redmax/Simulation.h @@ -232,6 +232,8 @@ class Simulation { _robot = robot; } + vector get_joint_torques(); + VectorX get_ground_force(std::string body_name); // init simulation void init(bool verbose = false); diff --git a/core/projects/redmax/python_interface.cpp b/core/projects/redmax/python_interface.cpp index ab9b48d..1d29e74 100644 --- a/core/projects/redmax/python_interface.cpp +++ b/core/projects/redmax/python_interface.cpp @@ -243,7 +243,13 @@ PYBIND11_MODULE(redmax_py, m) { py::arg("folder")) .def("print_time_report", &Simulation::print_time_report, - "print time report of the simulation."); + "print time report of the simulation.") + + .def("get_ground_force", &Simulation::get_ground_force, + "Get force applied by the ground on a specific object", py::arg("body_name")) + + .def("get_joint_torques", &Simulation::get_joint_torques, + "Get torques from all the joints of the robot"); m.def("make_sim", &make_sim, "initialize a simulation instance", py::arg("env_name"), py::arg("integrator") = "BDF2"); } \ No newline at end of file