fixed submodules

CMakeLists.txt

@@ -228,6 +228,8 @@ mark_as_advanced(WITH_HEADLESS)
 
 option(WITH_QUADRIFLOW    "Build with quadriflow remesher support" ON)
 
+option(WITH_TETGEN    "Build with tetgen remesher support" ON)
+
 option(WITH_AUDASPACE    "Build with blenders audio library (only disable if you know what you're doing!)" ON)
 option(WITH_SYSTEM_AUDASPACE "Build with external audaspace library installed on the system (only enable if you know what you're doing!)" OFF)
 mark_as_advanced(WITH_AUDASPACE)
@@ -654,6 +656,7 @@ set_and_warn_dependency(WITH_BOOST WITH_INTERNATIONAL  OFF)
 set_and_warn_dependency(WITH_BOOST WITH_OPENVDB        OFF)
 set_and_warn_dependency(WITH_BOOST WITH_OPENCOLORIO    OFF)
 set_and_warn_dependency(WITH_BOOST WITH_QUADRIFLOW     OFF)
+set_and_warn_dependency(WITH_BOOST WITH_TETGEN         ON)
 set_and_warn_dependency(WITH_BOOST WITH_USD            OFF)
 set_and_warn_dependency(WITH_BOOST WITH_ALEMBIC        OFF)
 
@@ -1738,6 +1741,7 @@ if(FIRST_RUN)
   info_cfg_option(WITH_OPENVDB)
   info_cfg_option(WITH_ALEMBIC)
   info_cfg_option(WITH_QUADRIFLOW)
+  info_cfg_option(WITH_TETGEN)
   info_cfg_option(WITH_USD)
   info_cfg_option(WITH_TBB)
   info_cfg_option(WITH_GMP)

extern/CMakeLists.txt

@@ -34,6 +34,8 @@ endif()
 
 add_subdirectory(rangetree)
 add_subdirectory(wcwidth)
+add_subdirectory(softbody)
+add_subdirectory(discregrid)
 
 if(WITH_BULLET)
   if(NOT WITH_SYSTEM_BULLET)
@@ -106,6 +108,10 @@ if(WITH_QUADRIFLOW)
   add_subdirectory(quadriflow)
 endif()
 
+if(WITH_TETGEN)
+  add_subdirectory(tetgen)
+endif()
+
 if(WITH_MOD_FLUID)
   add_subdirectory(mantaflow)
 endif()

extern/discregrid/CMakeLists.txt

@@ -0,0 +1,62 @@
+# ***** BEGIN GPL LICENSE BLOCK *****
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software Foundation,
+# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+#
+# The Original Code is Copyright (C) 2006, Blender Foundation
+# All rights reserved.
+# ***** END GPL LICENSE BLOCK *****
+
+set(INC
+	discregrid/include
+	discregrid/include/Discregrid
+	extern
+)
+
+set(INC_SYS
+	${EIGEN3_INCLUDE_DIRS}
+)
+
+set(SRC
+	discregrid/include/Discregrid/discrete_grid.hpp
+	discregrid/include/Discregrid/cubic_lagrange_discrete_grid.hpp
+	discregrid/include/Discregrid/acceleration/bounding_sphere_hierarchy.hpp
+	discregrid/include/Discregrid/acceleration/bounding_sphere.hpp
+	discregrid/include/Discregrid/acceleration/kd_tree.hpp
+	discregrid/include/Discregrid/acceleration/kd_tree.inl
+	discregrid/include/Discregrid/mesh/triangle_mesh.hpp
+	discregrid/include/Discregrid/mesh/entity_containers.hpp
+	discregrid/include/Discregrid/mesh/entity_iterators.hpp
+	discregrid/include/Discregrid/mesh/halfedge.hpp
+	discregrid/include/Discregrid/geometry/mesh_distance.hpp
+	discregrid/src/geometry/point_triangle_distance.hpp
+	discregrid/include/Discregrid/utility/serialize.hpp
+	discregrid/include/Discregrid/utility/lru_cache.hpp
+	discregrid/src/utility/timing.hpp
+	discregrid/src/utility/spinlock.hpp
+	discregrid/src/discrete_grid.cpp
+	discregrid/src/cubic_lagrange_discrete_grid.cpp
+	discregrid/src/acceleration/bounding_sphere_hierarchy.cpp
+	discregrid/src/mesh/entity_containers.cpp
+	discregrid/src/mesh/entity_iterators.cpp
+	discregrid/src/mesh/triangle_mesh.cpp
+	discregrid/src/geometry/mesh_distance.cpp
+	discregrid/src/geometry/point_triangle_distance.cpp
+	discregrid/src/utility/timing.cpp
+)
+
+set(LIB
+)
+
+blender_add_lib(extern_discregrid "${SRC}" "${INC}" "${INC_SYS}" "${LIB}")

extern/discregrid/FindDiscregrid.cmake

@@ -0,0 +1,89 @@
+# - Try to find Discregrid library
+#
+# The find script can be invoked by using the following command:
+#   find_package(Discregrid)
+#
+# Once done this will define
+#
+#  DISCREGRID_FOUND - System has found the discregrid library
+#  DISCREGRID_INCLUDE_DIRS - Path to the discregrid include directory
+#  DISCREGRID_LIBRARIES - Path to the static discregrid library
+
+# ============================================================================
+# _DISCREGRID_FIND_INCLUDE_DIR
+# Internal function to find the include directories
+#     _var = variable to set
+#     _hdr = header file to look for
+# ============================================================================
+function(_DISCREGRID_FIND_INCLUDE_DIR _var _hdr)
+    find_path(${_var} ${_hdr}
+        PATHS
+        $ENV{DISCREGRID_ROOT}
+        ${DISCREGRID_ROOT}
+	PATH_SUFFIXES
+	    /include
+    )
+
+    if (${_var})
+        set(DISCREGRID_INCLUDE_DIRS ${DISCREGRID_INCLUDE_DIRS} ${${_var}} PARENT_SCOPE)
+        if (NOT DISCREGRID_SKIP_MARK_AS_ADVANCED)
+            mark_as_advanced(${_var})
+        endif()
+    endif()
+endfunction(_DISCREGRID_FIND_INCLUDE_DIR)
+
+# ============================================================================
+# _DISCREGRID_FIND_LIBRARY
+# Internal function to find libraries packaged with DISCREGRID
+#     _var = library variable to create
+# ============================================================================
+function(_DISCREGRID_FIND_LIBRARY _var _lib _mode)
+    find_library(${_var}
+	NAMES ${_lib}
+	PATHS
+        $ENV{DISCREGRID_ROOT}
+        ${DISCREGRID_ROOT}
+	PATH_SUFFIXES
+	    /lib
+    )
+
+    if(${_var})
+        set(DISCREGRID_LIBRARIES ${DISCREGRID_LIBRARIES} ${_mode} ${${_var}} PARENT_SCOPE)
+        if(NOT DISCREGRID_SKIP_MARK_AS_ADVANCED)
+            mark_as_advanced(${_var})
+        endif()
+    endif()
+endfunction(_DISCREGRID_FIND_LIBRARY)
+
+# ============================================================================
+#
+# main()
+#
+# ============================================================================
+
+#
+# Find all libraries and include directories.
+#
+_DISCREGRID_FIND_INCLUDE_DIR(DISCREGRID_DISCREGRIDH_INCLUDE_DIR Discregrid/All)
+_DISCREGRID_FIND_LIBRARY(DISCREGRID_LIBRARY_RELEASE "discregrid" optimized)
+_DISCREGRID_FIND_LIBRARY(DISCREGRID_LIBRARY_DEBUG "discregrid_d" debug)
+
+#
+# Try to enforce components.
+#
+include(FindPackageHandleStandardArgs)
+
+find_package_handle_standard_args(DISCREGRID DEFAULT_MSG
+    DISCREGRID_DISCREGRIDH_INCLUDE_DIR
+    DISCREGRID_LIBRARY_DEBUG
+    DISCREGRID_LIBRARY_RELEASE
+)
+
+if(NOT DISCREGRID_FOUND)
+    set(DISCREGRID_INCLUDE_DIRS)
+    set(DISCREGRID_LIBRARIES)
+endif()
+
+if(DISCREGRID_INCLUDE_DIRS)
+    list(REMOVE_DUPLICATES DISCREGRID_INCLUDE_DIRS)
+endif()

extern/discregrid/LICENSE

@@ -0,0 +1,21 @@
+The MIT License (MIT)
+
+Copyright (c) 2017 Dan Koschier
+
+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.

extern/discregrid/README.md

@@ -0,0 +1,100 @@
+# Discregrid
+
+<p align=center><img src="https://github.com/InteractiveComputerGraphics/Discregrid/workflows/build-linux/badge.svg">&nbsp;&nbsp; <img src="https://github.com/InteractiveComputerGraphics/Discregrid/workflows/build-windows/badge.svg"></p>
+
+![](https://www.animation.rwth-aachen.de/media/resource_files/DragonSDFDM.png)
+**Figure 1**: Left: Slice of a three-dimensional discrete signed distance field of the Stanford dragon. Right: Density map for SPH boundary handling of Stanford dragon.
+
+**Discregrid** is a static C++ library for the parallel discretization of (preferably smooth) functions on regular grids.
+The library generates a (cubic) polynomial discretization given a box-shaped domain, a grid resolution, and a function that maps a three-dimensional position in space to a real scalar value.
+In the current implementation isoparametric cubic polynomials of Serendipity type for the cell-wise discretization are employed.
+The coefficient vector for the discrete polynomial basis is computed using regular sampling of the input function at the higher-order grid's nodes.
+However, I plan to provide a spatially adaptive version of the cubic discretization and moreover an implementation of the hp-adaptive discretization algorithm described in [KDBB17].
+The algorithm to generate the discretization is moreover *fully parallelized* using OpenMP and especially well-suited for the discretization of signed distance functions.
+The library moreover provides the functionality to serialize and deserialize the a generated discrete grid.
+
+Besides the library the project includes three executable programs that serve the following purposes:
+* *GenerateSDF*: Computes a discrete (cubic) signed distance field from a triangle mesh in OBJ format.
+* *DiscreteFieldToBitmap*: Generates an image in bitmap format of a two-dimensional slice of a previously computed discretization.
+* *GenerateDensityMap*: Generates a density map according to the approach presented in [KB17] from a previously generated discrete signed distance field using the widely adopted cubic spline kernel. The program can be easily extended to work with other kernel function by simply replacing the implementation in sph_kernel.hpp.
+
+**Author**: Dan Koschier, **License**: MIT
+
+## Libraries using Discregrid
+* [PBD] - A C++ library for physically-based simulation of rigid bodies, deformables, cloth and fluids using Position-Based Dynamics. Discregrid is used to compute discrete signed distance fields of rigid objects for collision handling purposes.
+* [SPlisHSPlasH] - A C++ library for the physically-based simulation of fluids using Smoothed Particle Hydrodynamics. Discregrid is used to compute density maps according to my paper [KB17] for boundary handling.
+
+## Build Instructions
+
+This project is based on [CMake](https://cmake.org/). Simply generate project, Makefiles, etc. using [CMake](https://cmake.org/) and compile the project with the compiler of your choice. The code was tested with the following configurations:
+- Windows 10 64-bit, CMake 3.8, Visual Studio 2017
+- Debian 9 64-bit, CMake 3.8, GCC 6.3.
+
+## Usage
+In order to use the library, the main header has to be included and the static library has to be compiled and linked against the client program.
+In this regard a find script for CMake is provided, i.e. FindDiscregrid.cmake.
+The main header can be included as follows:
+```c++
+#include <Discregrid/All>
+```
+
+A base class for the data structure that generates and holds a discretization of a function f: R^3 -> R can be constructed as follows:
+```c++
+// Firstly, create a domain on which a discretization will be generated.
+Eigen::AlignedBox3d domain;
+// Then specify domain extents using e.g. domain.extend(...).
+// Secondly, specify a grid resolution.
+std::array<unsigned int, 3> resolution = {{10, 10, 10}}
+// Finally, instantiate the grid.
+Discregrid::CubicLagrangeDiscreteGrid discrete_grid(domain, resolution);
+```
+Then, an arbitrary number of functions can be discretized on the initiated grid:
+```c++
+Discregrid::DiscreteGrid::ContinuousFunction func1 = ...;
+Discregrid::DiscreteGrid::ContinuousFunction func2 = ...;
+
+auto df_index1 = discrete_grid.addFunction(func1);
+auto df_index2 = discrete_grid.addFunction(func2);
+```
+Optionally, only coefficients at nodes fulfilling a certain predicate can be generated by specifying the predicate:
+```c++
+Discregrid::DiscreteGrid::ContinuousFunction func3 = ...;
+auto df_index3 = discrete_grid.addFunction(func3, false, [&](Vector3d const& x)
+{
+	...
+	// Return true if a certain criterion for the node location x is fulfilled, e.g.
+	return x.y() > 0.0;
+});
+```
+A value of a discrete field can be evaluated by interpolation.
+Additionally, the gradient at the given query point can be computed if desired.
+```c++
+auto val1 = sdf->interpolate(df_index1, {0.1, 0.2, 0.3});
+Eigen::Vector3d grad2;
+auto val2 = sdf->interpolate(df_index2, {0.3, 0.2, 0.1}, &grad2);
+```
+
+If a discretization of the input function is only required in certain regions of the given domain, the discretization can be reduced resulting in a sparsely populated grid to save memory:
+```c++
+discrete_grid.reduce_field(df_index1, [](Eigen::Vector3d const& x, double v)
+{
+	// E.g.
+	return x.x() < 0.0 && v > 0.0;
+});
+```
+Here x represents the location of sample point in the grid and v represents the sampled value of the input function. If the predicated function evaluates to true the sample point is kept but discarded otherwise.
+
+Optionally, the data structure can be serialized and deserialized via
+```c++
+discrete_grid.save(filename);
+discrete_grid.load(filename); // or
+discrete_grid = Discregrid::CubicLagrangeDiscreteGrid(filename);
+```
+
+## References
+
+* [KDBB17] D. Koschier, C. Deul, M. Brand and J. Bender, 2017. "An hp-Adaptive Discretization Algorithm for Signed Distance Field Generation", IEEE Transactions on Visualiztion and Computer Graphics 23, 10, 2208-2221.
+* [KB17] D. Koschier and J. Bender, 2017. "Density Maps for Improved SPH Boundary Handling", ACM SIGGRAPH/Eurographics Symposium on Computer Animation, 1-10.
+
+[PBD]: <https://github.com/InteractiveComputerGraphics/PositionBasedDynamics>
+[SPlisHSPlasH]: <https://github.com/InteractiveComputerGraphics/SPlisHSPlasH>

extern/discregrid/cmake/Modules/FindEigen3.cmake

@@ -0,0 +1,81 @@
+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+#   find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+#  EIGEN3_FOUND - system has eigen lib with correct version
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+#  EIGEN3_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+  if(NOT Eigen3_FIND_VERSION_MAJOR)
+    set(Eigen3_FIND_VERSION_MAJOR 2)
+  endif(NOT Eigen3_FIND_VERSION_MAJOR)
+  if(NOT Eigen3_FIND_VERSION_MINOR)
+    set(Eigen3_FIND_VERSION_MINOR 91)
+  endif(NOT Eigen3_FIND_VERSION_MINOR)
+  if(NOT Eigen3_FIND_VERSION_PATCH)
+    set(Eigen3_FIND_VERSION_PATCH 0)
+  endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK FALSE)
+  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK TRUE)
+  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+  if(NOT EIGEN3_VERSION_OK)
+
+    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+                   "but at least version ${Eigen3_FIND_VERSION} is required")
+  endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+  # in cache already
+  _eigen3_check_version()
+  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+      PATHS
+      ${CMAKE_INSTALL_PREFIX}/include
+      ${KDE4_INCLUDE_DIR}
+      PATH_SUFFIXES eigen3 eigen
+    )
+
+  if(EIGEN3_INCLUDE_DIR)
+    _eigen3_check_version()
+  endif(EIGEN3_INCLUDE_DIR)
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+  mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
+

extern/discregrid/cmake/Modules/PatchOpenMPApple.cmake

@@ -0,0 +1,28 @@
+# This file adds OpenMP to your project if you are using Apple Clang.
+
+option(APPLE_OMP_AUTOADD "Add OpenMP if using AppleClang" ON)
+
+find_package(OpenMP QUIET)
+    if(NOT "${OpenMP_FOUND}" OR NOT "${OpenMP_CXX_FOUND}")
+        if("${APPLE_OMP_AUTOADD}" AND "${CMAKE_CXX_COMPILER_ID}" STREQUAL "AppleClang" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS "7")
+        message(STATUS "AppleClang >= 7.0 detected, adding OpenMP. Disable with -DAPPLE_OMP_AUTOADD=OFF")
+
+        find_program(BREW NAMES brew)
+        if(BREW)
+            execute_process(COMMAND ${BREW} ls libomp RESULT_VARIABLE BREW_RESULT_CODE OUTPUT_QUIET ERROR_QUIET)
+            if(BREW_RESULT_CODE)
+                message(STATUS "This program supports OpenMP on Mac through Brew. Please run \"brew install libomp\"")
+            else()
+                execute_process(COMMAND ${BREW} --prefix libomp OUTPUT_VARIABLE BREW_LIBOMP_PREFIX OUTPUT_STRIP_TRAILING_WHITESPACE)
+                set(OpenMP_CXX_FLAGS "-Xpreprocessor -fopenmp")
+                set(OpenMP_CXX_LIB_NAMES "omp")
+                set(OpenMP_omp_LIBRARY "${BREW_LIBOMP_PREFIX}/lib/libomp.dylib")
+                set(OpenMP_INCLUDE_DIR "${BREW_LIBOMP_PREFIX}/include")
+                include_directories("${BREW_LIBOMP_PREFIX}/include")
+                message(STATUS "Using Homebrew libomp from ${BREW_LIBOMP_PREFIX}")
+            endif()
+        else()
+            message(STATUS "This program supports OpenMP on Mac through Homebrew, installing Homebrew recommmended https://brew.sh")
+        endif()
+        endif()
+    endif()

extern/discregrid/cmd/CMakeLists.txt

@@ -0,0 +1,3 @@
+add_subdirectory(generate_sdf)
+add_subdirectory(discrete_field_to_bitmap)
+add_subdirectory(generate_density_map)

extern/discregrid/cmd/discrete_field_to_bitmap/CMakeLists.txt

@@ -0,0 +1,48 @@
+# Eigen library.
+find_package(Eigen3 REQUIRED)
+
+# Set include directories.
+include_directories(
+	../../extern
+	../../discregrid/include
+	${EIGEN3_INCLUDE_DIR}
+)
+
+
+if(WIN32)
+	add_definitions(-D_SCL_SECURE_NO_WARNINGS)
+	add_definitions(-D_CRT_SECURE_NO_WARNINGS)
+endif(WIN32)
+
+if ( CMAKE_COMPILER_IS_GNUCC )
+    set(CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -Wno-multichar")
+endif ( CMAKE_COMPILER_IS_GNUCC )
+
+# OpenMP support.
+if(APPLE)
+	include(PatchOpenMPApple)
+else()
+	find_package(OpenMP REQUIRED)
+endif()
+
+if(OPENMP_FOUND)
+	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+	set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+endif()
+
+add_executable(DiscreteFieldToBitmap
+	main.cpp
+	bmp_file.hpp
+	bmp_file.cpp
+)
+
+add_dependencies(DiscreteFieldToBitmap
+	Discregrid
+)
+
+target_link_libraries(DiscreteFieldToBitmap
+	Discregrid
+)
+
+set_target_properties(DiscreteFieldToBitmap PROPERTIES FOLDER Cmd)

extern/discregrid/cmd/discrete_field_to_bitmap/bmp_file.cpp

@@ -0,0 +1,147 @@
+#include <cstdio>
+#include <cstdlib>
+#include "bmp_file.hpp"
+
+// Compression Type
+#define BI_RGB      0L
+#define BI_RLE8     1L
+#define BI_RLE4     2L
+
+
+// -------------------------------------------------------------------
+bool BmpReaderWriter::loadFile(const char *filename, unsigned int &width, unsigned int &height, unsigned char *&data)
+{
+	if (data) 
+	{		
+		delete [] data;
+		data = NULL;
+	}
+	width = 0;
+	height = 0;
+
+	FILE *f = fopen(filename, "rb");
+	if (!f) 
+		return false;
+
+	size_t num;
+	BMPHEADER header;
+	num = fread(&header, sizeof(BMPHEADER), 1, f);
+	if(isBigEndian()) header.Type = endianSwap(header.Type);
+	if (num != 1) { fclose(f); return false; }
+	if (header.Type != 'MB') { fclose(f); return false; }
+
+	BMPINFO info;
+	num = fread(&info, sizeof(BMPINFO), 1, f);
+	if (num != 1) { fclose(f); return false; }
+	if(isBigEndian()) info.Size = endianSwap(info.Size);
+	if(isBigEndian()) info.BitCount = endianSwap(info.BitCount);
+	if(isBigEndian()) info.Compression = endianSwap(info.Compression);
+	if(isBigEndian()) info.Width = endianSwap(info.Width);
+	if(isBigEndian()) info.Height = endianSwap(info.Height);
+
+	if (info.Size != sizeof(BMPINFO)) { fclose(f); return false; }
+	if (info.BitCount != 24) { fclose(f); return false; }
+	if (info.Compression != BI_RGB) { fclose(f); return false; }
+
+	width = info.Width;
+	height = info.Height;
+	data = new unsigned char[width * height * 3];
+
+	int lineLen = (((info.Width * (info.BitCount>>3)) + 3)>>2)<<2;
+	unsigned char *line = new unsigned char[lineLen];
+
+	for(int i = info.Height-1; i >= 0; i--) {
+		num = fread(line, lineLen, 1, f);
+		if (num != 1) { fclose(f); return false; }
+		unsigned char *src = line;
+		unsigned char *dest = data + i*info.Width*3;
+		for(unsigned int j = 0; j < info.Width; j++) {
+			unsigned char r,g,b;
+			b = *src++; g = *src++; r = *src++;
+			*dest++ = r; *dest++ = g; *dest++ = b;
+		}
+	}
+
+	delete [] line;
+	fclose(f);
+
+	return true;
+}
+
+// -------------------------------------------------------------------
+bool BmpReaderWriter::saveFile(const char *filename, int width, int height, unsigned char *data)
+{
+	FILE *f = fopen(filename, "wb");
+	if (!f) return false;
+
+	// todo : works on pcs only, swap correctly if big endian 
+	BMPHEADER header;
+	header.Type = 'MB';
+	header.Size = sizeof(BMPINFO);
+	header.Reserved1 = 0;
+	header.Reserved2 = 0;
+	header.OffBits = sizeof(BMPHEADER) + sizeof(BMPINFO);
+	fwrite(&header, sizeof(BMPHEADER), 1, f);
+
+	BMPINFO info;
+	info.Size = sizeof(BMPINFO);
+	info.Width = width;
+	info.Height = height;
+	info.Planes = 1;
+	info.BitCount = 24;
+	info.Compression = BI_RGB;
+	info.XPelsPerMeter = 4000;
+	info.YPelsPerMeter = 4000;
+	info.ClrUsed = 0;
+	info.ClrImportant = 0;
+	fwrite(&info, sizeof(info), 1, f);
+
+	// padded to multiple of 4
+	int lineLen = (((info.Width * (info.BitCount>>3)) + 3)>>2)<<2;
+	info.SizeImage = lineLen * height;
+
+	unsigned char *line = new unsigned char[lineLen];
+
+	for(int i = 0; i < height; i++) 
+	{
+		unsigned char *src = data + i*width*3;
+		unsigned char *dest = line;
+		for(int j = 0; j < width; j++) 
+		{
+			unsigned char r,g,b;
+			r = *src++; g = *src++; b = *src++;
+			*dest++ = b; *dest++ = g; *dest++ = r;
+		}
+		for (int j = 3*width; j < lineLen; j++)
+			*dest++ = 0;
+		fwrite(line, lineLen, 1, f);
+	}
+
+	delete [] line;
+	fclose(f);
+
+	return true;
+}
+
+bool BmpReaderWriter::isBigEndian()
+{
+	int i = 1; return *((char*)&i) == 0;
+}
+
+unsigned short BmpReaderWriter::endianSwap(unsigned short nValue)
+{
+	return (((nValue >> 8)) | (nValue << 8));
+}
+
+unsigned int BmpReaderWriter::endianSwap(unsigned int i)
+{
+	unsigned char b1, b2, b3, b4;
+
+	b1 = i & 255;
+	b2 = (i >> 8) & 255;
+	b3 = (i >> 16) & 255;
+	b4 = (i >> 24) & 255;
+
+	return ((unsigned int)b1 << 24) + ((unsigned int)b2 << 16) + ((unsigned int)b3 << 8) + b4;
+}
+

extern/discregrid/cmd/discrete_field_to_bitmap/bmp_file.hpp

@@ -0,0 +1,42 @@
+#pragma once
+
+class BmpReaderWriter
+{
+public:
+	static bool isBigEndian();
+	static unsigned short endianSwap(unsigned short nValue);
+	static unsigned int endianSwap(unsigned int i);
+
+	// -------------------------------------------------------------------
+
+#pragma pack(1)
+
+	struct BMPHEADER {
+		unsigned short		Type;
+		unsigned int		Size;
+		unsigned short		Reserved1;
+		unsigned short		Reserved2;
+		unsigned int		OffBits;
+	};
+
+	// Only Win3.0 BMPINFO (see later for OS/2)
+	struct BMPINFO {
+		unsigned int		Size;
+		unsigned int		Width;
+		unsigned int		Height;
+		unsigned short		Planes;
+		unsigned short		BitCount;
+		unsigned int		Compression;
+		unsigned int		SizeImage;
+		unsigned int		XPelsPerMeter;
+		unsigned int		YPelsPerMeter;
+		unsigned int		ClrUsed;
+		unsigned int		ClrImportant;
+	};
+
+#pragma pack()
+
+	// Data is persists until the class is destructed.
+	static bool loadFile(const char *filename, unsigned int &width, unsigned int &height, unsigned char *&data);
+	static bool saveFile(const char *filename, int width, int height, unsigned char *data);
+};

extern/discregrid/cmd/discrete_field_to_bitmap/main.cpp

@@ -0,0 +1,193 @@
+
+#include <Discregrid/All>
+#include <Eigen/Dense>
+#include <cxxopts/cxxopts.hpp>
+
+#include <string>
+#include <iostream>
+#include <array>
+
+#include "bmp_file.hpp"
+
+using namespace Eigen;
+
+namespace
+{
+std::array<unsigned char, 3u> doubleToGreenBlueInverse(double v)
+{
+	if (v >= 0.0)
+	{
+		return {{0u, static_cast<unsigned char>(std::min(std::max(255.0 * (1.0 - v), 0.0), 255.0)), 0u}};
+	}
+	return {{0u, 0u, static_cast<unsigned char>(std::min(std::max(255.0 * (1.0 + v), 0.0), 255.0))}};
+}
+
+std::array<unsigned char, 3u> doubleToRedSequential(double v)
+{
+	return {{static_cast<unsigned char>(std::min(std::max(255.0 * v, 0.0), 255.0)), 0u, 0u}};
+}
+
+}
+
+int main(int argc, char* argv[])
+{
+	cxxopts::Options options(argv[0], "Transforms a slice of a discrete SDF to a bitmap image.");
+	options.positional_help("[input SDF file]");
+
+	options.add_options()
+	("h,help", "Prints this help text")
+	("f,field_id", "ID in which the SDF to export is stored.", cxxopts::value<unsigned int>()->default_value("0"))
+	("s,samples", "Number of samples in width direction", cxxopts::value<unsigned int>()->default_value("1024"))
+	("p,plane", "Plane in which the image slice is extracted", cxxopts::value<std::string>()->default_value("xy"))
+	("d,depth", "Relative depth value between -1 and 1 in direction of the axis orthogonal to the plane", cxxopts::value<double>()->default_value("0"))
+	("o,output", "Output (in bmp format)", cxxopts::value<std::string>()->default_value(""))
+	("c,colormap", "Color map options: redsequential (rs), green blue inverse diverging (gb) (suitable for visualiztion of signed distance fields)", cxxopts::value<std::string>()->default_value("gb"))
+	("input", "SDF file", cxxopts::value<std::vector<std::string>>())
+	;
+
+	try
+	{
+		options.parse_positional("input");
+		auto result = options.parse(argc, argv);
+
+		if (result.count("help"))
+		{
+			std::cout << options.help() << std::endl;
+			std::cout << std::endl << std::endl << "Example: SDFToBitmap -p xz file.sdf" << std::endl;
+			exit(0);
+		}
+		if (!result.count("input"))
+		{
+			std::cout << "ERROR: No input file given." << std::endl;
+			std::cout << options.help() << std::endl;
+			std::cout << std::endl << std::endl << "Example: SDFToBitmap -p xz file.sdf" << std::endl;
+			exit(1);
+		}
+
+		auto sdf = std::unique_ptr<Discregrid::DiscreteGrid>{};
+
+		auto filename = result["input"].as<std::vector<std::string>>().front();
+		auto lastindex = filename.find_last_of(".");
+		auto extension = filename.substr(lastindex + 1, filename.length() - lastindex);
+
+		std::cout << "Load SDF...";
+		if (extension == "cdf" || extension == "cdm")
+		{
+			sdf = std::unique_ptr<Discregrid::CubicLagrangeDiscreteGrid>(
+				new Discregrid::CubicLagrangeDiscreteGrid(filename));
+		}
+		std::cout << "DONE" << std::endl;
+
+		auto depth = result["d"].as<double>();
+		auto const& domain = sdf->domain();
+		auto diag = domain.diagonal().eval();
+
+		auto plane = result["p"].as<std::string>();
+		if (plane.length() != 2 && plane[0] != plane[1])
+		{
+			std::cerr << "ERROR: Invalid option for plane provided. Should be one of the following options: xy, xz, yz, yx" << std::endl;
+			exit(1);
+		}
+
+		auto dir = Vector3i::Zero().eval();
+		if (plane[0] == 'y')
+			dir(0) = 1;
+		else if (plane[0] == 'z')
+			dir(0) = 2;
+		if (plane[1] == 'y')
+			dir(1) = 1;
+		else if (plane[1] == 'z')
+			dir(1) = 2;
+		if (dir(0) != 1 && dir(1) != 1)
+			dir(2) = 1;
+		if (dir(0) != 2 && dir(1) != 2)
+			dir(2) = 2;
+
+		auto xsamples = result["s"].as<unsigned int>();
+		auto ysamples = static_cast<unsigned int>(std::round(diag(dir(1)) / diag(dir(0)) * static_cast<double>(xsamples)));
+
+		auto xwidth = diag(dir(0)) / xsamples;
+		auto ywidth = diag(dir(1)) / ysamples;
+
+		auto data = std::vector<double>{};
+		data.resize(xsamples * ysamples);
+
+		auto field_id = result["f"].as<unsigned int>();
+
+		std::cout << "Sample field...";
+#pragma omp parallel for
+		for (int k = 0; k < static_cast<int>(xsamples * ysamples); ++k)
+		{
+			auto i = k % xsamples;
+			auto j = k / xsamples;
+
+			auto xr = static_cast<double>(i) / static_cast<double>(xsamples);
+			auto yr = static_cast<double>(j) / static_cast<double>(ysamples);
+
+			auto x = domain.min()(dir(0)) + xr * diag(dir(0)) + 0.5 * xwidth;
+			auto y = domain.min()(dir(1)) + yr * diag(dir(1)) + 0.5 * ywidth;
+
+			auto sample = Vector3d{};
+			sample(dir(0)) = x;
+			sample(dir(1)) = y;
+			sample(dir(2)) = domain.min()(dir(2)) + 0.5 * (1.0 + depth) * diag(dir(2));
+
+			data[k] = sdf->interpolate(field_id, sample);
+			if (data[k] == std::numeric_limits<double>::max())
+			{
+				data[k] = 0.0;
+			}
+		}
+
+		std::cout << "DONE" << std::endl;
+
+		auto min_v = *std::min_element(data.begin(), data.end());
+		auto max_v = *std::max_element(data.begin(), data.end());
+
+		auto out_file = result["o"].as<std::string>();
+		if (out_file == "")
+		{
+			out_file = filename;
+			if (out_file.find(".") != std::string::npos)
+			{
+				auto lastindex = out_file.find_last_of(".");
+				out_file = out_file.substr(0, lastindex);
+			}
+			out_file += ".bmp";
+		}
+
+		std::cout << "Ouput file: " << out_file << std::endl;
+
+		std::cout << "Export BMP...";
+		std::transform(data.begin(), data.end(), data.begin(), [&max_v, &min_v](double v) {return v >= 0.0 ? v / std::abs(max_v) : v / std::abs(min_v); });
+
+		auto pixels = std::vector<std::array<unsigned char, 3u>>(data.size());
+
+		auto cm = result["c"].as<std::string>();
+		if (cm != "gb" && cm != "rs")
+		{
+			std::cerr << "WARNING: Unknown color map option. Fallback to mode 'gb'." << std::endl;
+		}
+
+		if (cm == "gb")
+			std::transform(data.begin(), data.end(), pixels.begin(), doubleToGreenBlueInverse);
+		else if (cm == "rs")
+			std::transform(data.begin(), data.end(), pixels.begin(), doubleToRedSequential);
+
+		BmpReaderWriter::saveFile(out_file.c_str(), xsamples, ysamples, &pixels.front()[0]);
+		std::cout << "DONE" << std::endl;
+
+		std::cout << std::endl << "Statistics:" << std::endl;
+		std::cout << "\tdomain         = " << domain.min().transpose() << ", " << domain.max().transpose() << std::endl;
+		std::cout << "\tmin value      = " << min_v << std::endl;
+		std::cout << "\tmax value      = " << max_v << std::endl;
+		std::cout << "\tbmp resolution = " << xsamples << " x " << ysamples << std::endl;
+	}
+	catch (cxxopts::OptionException const& e)
+	{
+		std::cout << "error parsing options: " << e.what() << std::endl;
+		exit(1);
+	}
+	
+	return 0;
+}

extern/discregrid/cmd/generate_density_map/CMakeLists.txt

@@ -0,0 +1,44 @@
+# Eigen library.
+find_package(Eigen3 REQUIRED)
+
+# Set include directories.
+include_directories(
+	../../extern
+	../../discregrid/include
+	${EIGEN3_INCLUDE_DIR}
+)
+
+if(WIN32)
+	add_definitions(-D_SCL_SECURE_NO_WARNINGS)
+	add_definitions(-D_USE_MATH_DEFINES)
+endif(WIN32)
+
+add_executable(GenerateDensityMap
+	main.cpp
+	gauss_quadrature.hpp
+	gauss_quadrature.cpp
+	sph_kernel.hpp
+)
+
+# OpenMP support.
+if(APPLE)
+	include(PatchOpenMPApple)
+else()
+	find_package(OpenMP REQUIRED)
+endif()
+
+if(OPENMP_FOUND)
+	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+	set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+endif()
+
+add_dependencies(GenerateDensityMap
+	Discregrid
+)
+
+target_link_libraries(GenerateDensityMap
+	Discregrid
+)
+
+set_target_properties(GenerateDensityMap PROPERTIES FOLDER Cmd)

extern/discregrid/cmd/generate_density_map/gauss_quadrature.hpp

@@ -0,0 +1,16 @@
+#pragma once
+
+#include <Eigen/Dense>
+
+class GaussQuadrature
+{
+public:
+
+    using Integrand = std::function<double(Eigen::Vector3d const&)>;
+    using Domain = Eigen::AlignedBox3d;
+
+    static double integrate(Integrand integrand, Domain const& domain, unsigned int p);
+};
+
+
+

extern/discregrid/cmd/generate_density_map/main.cpp

@@ -0,0 +1,171 @@
+
+#include <Discregrid/All>
+#include <Eigen/Dense>
+#include <cxxopts/cxxopts.hpp>
+
+#include <string>
+#include <iostream>
+#include <array>
+
+#include "sph_kernel.hpp"
+#include "gauss_quadrature.hpp"
+
+using namespace Eigen;
+
+std::istream& operator>>(std::istream& is, std::array<unsigned int, 3>& data)
+{
+	is >> data[0] >> data[1] >> data[2];
+	return is;
+}
+
+std::istream& operator>>(std::istream& is, AlignedBox3d& data)
+{
+	is	>> data.min()[0] >> data.min()[1] >> data.min()[2]
+		>> data.max()[0] >> data.max()[1] >> data.max()[2];
+	return is;
+}
+
+
+int main(int argc, char* argv[])
+{
+	cxxopts::Options options(argv[0], "Generates a signed distance field from a closed two-manifold triangle mesh.");
+	options.positional_help("[input OBJ file]");
+
+	options.add_options()
+	("h,help", "Prints this help text")
+	("r,rest_density", "Rest density rho0 of the fluid", cxxopts::value<double>()->default_value("1000.0"))
+	("i,invert", "Invert field")
+	("s,smoothing_length", "Kernel smoothing length", cxxopts::value<double>()->default_value("0.1"))
+	("o,output", "Ouput file in cdf format", cxxopts::value<std::string>()->default_value(""))
+	("no-reduction", "Disables discarding of cells for sparse layout.")
+	("input", "Discrete grid file containing input SDF in field 0", cxxopts::value<std::vector<std::string>>())
+	;
+
+	try
+	{
+		options.parse_positional("input");
+		auto result = options.parse(argc, argv);
+
+		if (result.count("help"))
+		{
+			std::cout << options.help() << std::endl;
+			std::cout << std::endl << std::endl << "Example: GenerateSDF -r \"50 50 50\" dragon.obj" << std::endl;
+			exit(0);
+		}
+		if (!result.count("input"))
+		{
+			std::cout << "ERROR: No input SDF given." << std::endl;
+			std::cout << options.help() << std::endl;
+			std::cout << std::endl << std::endl << "Example: GenerateDensityMap -r \"50 50 50\" field.cdf" << std::endl;
+			exit(1);
+		}
+		auto filename = result["input"].as<std::vector<std::string>>().front();
+
+		if (!std::ifstream(filename).good())
+		{
+			std::cerr << "ERROR: Input file does not exist!" << std::endl;
+			exit(1);
+		}
+
+		auto sdf = std::unique_ptr<Discregrid::DiscreteGrid>{};
+
+		auto lastindex = filename.find_last_of(".");
+		auto extension = filename.substr(lastindex + 1, filename.length() - lastindex);
+
+		std::cout << "Load SDF...";
+		if (extension == "cdf")
+		{
+			sdf = std::unique_ptr<Discregrid::CubicLagrangeDiscreteGrid>(
+				new Discregrid::CubicLagrangeDiscreteGrid(filename));
+		}
+		std::cout << "DONE" << std::endl;
+
+		auto h = result["s"].as<double>();
+		auto sph_kernel = CubicKernel{};
+		sph_kernel.setRadius(h);
+		auto gamma = [&](Vector3d const& x)
+		{
+			auto ar = sph_kernel.getRadius();
+			auto dist = sdf->interpolate(0u, x);
+			if (dist > ar)
+				return 0.0;
+			return 1.0 - dist / ar;
+		};
+		auto int_domain = AlignedBox3d(Vector3d::Constant(-h), Vector3d::Constant(h));
+		auto rho0 = result["r"].as<double>();
+		auto density_func = [&](Vector3d const& x)
+		{
+			auto dist = sdf->interpolate(0u, x);
+			if (dist > 2.0 * sph_kernel.getRadius())
+			{
+				return 0.0;
+			}
+
+			auto integrand = [&sph_kernel, &gamma, &x](Vector3d const& xi)
+			{
+				auto res = gamma(x + xi) * sph_kernel.W(xi);
+				return res;
+			};
+
+			auto res = GaussQuadrature::integrate(integrand, int_domain, 30);
+			return rho0 * res;
+		};
+
+		auto no_reduction = result["no-reduction"].count() > 0u;
+
+
+		auto cell_diag = sdf->cellSize().norm();
+		std::cout << "Generate density map..." << std::endl;
+		sdf->addFunction(density_func, true, [&](Vector3d const& x_)
+		{
+			if (no_reduction)
+			{
+				return true;
+			}
+			auto x = x_.cwiseMax(sdf->domain().min()).cwiseMin(sdf->domain().max());
+			auto dist = sdf->interpolate(0u, x);
+			if (dist == std::numeric_limits<double>::max())
+			{
+				return false;
+			}
+
+			return -6.0 * h < dist + cell_diag && dist - cell_diag < 2.0 * h;
+		});
+
+		if (result["no-reduction"].count() == 0u)
+		{
+			std::cout << "Reduce discrete fields...";
+			sdf->reduceField(0u, [&](const Vector3d &, double v)
+			{
+				return -6.0 * h < v + cell_diag && v - cell_diag < 2.0 * h;
+			});
+			sdf->reduceField(1u, [&](const Vector3d &, double v)
+			{
+				return 0.0 <= v && v <= 3.0 * rho0;
+			});
+			std::cout << "DONE" << std::endl;
+		}
+
+		std::cout << "Serialize discretization...";
+		auto output_file = result["o"].as<std::string>();
+		if (output_file == "")
+		{
+			output_file = filename;
+			if (output_file.find(".") != std::string::npos)
+			{
+				auto lastindex = output_file.find_last_of(".");
+				output_file = output_file.substr(0, lastindex);
+			}
+			output_file += ".cdm";
+		}
+		sdf->save(output_file);
+		std::cout << "DONE" << std::endl;
+	}
+	catch (cxxopts::OptionException const& e)
+	{
+		std::cout << "error parsing options: " << e.what() << std::endl;
+		exit(1);
+	}
+	
+	return 0;
+}

extern/discregrid/cmd/generate_density_map/sph_kernel.hpp

@@ -0,0 +1,82 @@
+#pragma once
+
+
+#include <Eigen/Dense>
+
+class CubicKernel
+{
+public:
+	 double getRadius() { return m_radius; }
+	 void setRadius(double val)
+	{
+		m_radius = val;
+		const double pi = static_cast<double>(M_PI);
+
+		const double h3 = m_radius*m_radius*m_radius;
+		m_k = 8.0 / (pi*h3);
+		m_l = 48.0 / (pi*h3);
+		m_W_zero = W(Eigen::Vector3d::Zero());
+	}
+
+public:
+	double W(Eigen::Vector3d const& r)
+	{
+		double res = 0.0;
+		const double rl = r.norm();
+		const double q = rl/m_radius;
+		if (q <= 1.0)
+		{
+			if (q <= 0.5)
+			{
+				const double q2 = q*q;
+				const double q3 = q2*q;
+				res = m_k * (6.0*q3-6.0*q2+1.0);
+			}
+			else
+			{
+				auto _1mq = 1.0 - q;
+				res = m_k * (2.0*_1mq*_1mq*_1mq);
+			}
+		}
+		return res;
+	}
+
+	Eigen::Vector3d gradW(const Eigen::Vector3d &r)
+	{
+		using namespace Eigen;
+		Vector3d res;
+		const double rl = r.norm();
+		const double q = rl / m_radius;
+		if (q <= 1.0)
+		{
+			if (rl > 1.0e-6)
+			{
+				const Vector3d gradq = r * ((double) 1.0 / (rl*m_radius));
+				if (q <= 0.5)
+				{
+					res = m_l*q*((double) 3.0*q - (double) 2.0)*gradq;
+				}
+				else
+				{
+					const double factor = 1.0 - q;
+					res = m_l*(-factor*factor)*gradq;
+				}
+			}
+		}
+		else
+			res.setZero();
+
+		return res;
+	}
+
+	double W_zero()
+	{
+		return m_W_zero;
+	}
+
+private:
+	double m_radius;
+	double m_k;
+	double m_l;
+	double m_W_zero;
+};

extern/discregrid/cmd/generate_sdf/CMakeLists.txt

@@ -0,0 +1,46 @@
+# Declare configuration file to embed supplied mesh files.
+set(RESOURCE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/resources/")
+configure_file("../resource_path.hpp.in" "${CMAKE_CURRENT_SOURCE_DIR}/resource_path.hpp")
+
+# Eigen library.
+find_package(Eigen3 REQUIRED)
+
+# Set include directories.
+include_directories(
+	../../extern
+	../../discregrid/include
+	${EIGEN3_INCLUDE_DIR}
+)
+
+if(WIN32)
+	add_definitions(-D_SCL_SECURE_NO_WARNINGS)
+	add_definitions(-D_USE_MATH_DEFINES)
+endif(WIN32)
+
+# OpenMP support.
+if(APPLE)
+	include(PatchOpenMPApple)
+else()
+	find_package(OpenMP REQUIRED)
+endif()
+
+if(OPENMP_FOUND)
+	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+	set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+endif()
+
+add_executable(GenerateSDF
+	main.cpp
+	resource_path.hpp
+)
+
+add_dependencies(GenerateSDF
+	Discregrid
+)
+
+target_link_libraries(GenerateSDF
+	Discregrid
+)
+
+set_target_properties(GenerateSDF PROPERTIES FOLDER Cmd)

extern/discregrid/cmd/generate_sdf/main.cpp

@@ -0,0 +1,130 @@
+
+#include <Discregrid/All>
+#include <Eigen/Dense>
+
+#include "resource_path.hpp"
+
+#include <string>
+#include <iostream>
+#include <array>
+
+using namespace Eigen;
+
+std::istream& operator>>(std::istream& is, std::array<unsigned int, 3>& data)  
+{  
+	is >> data[0] >> data[1] >> data[2];  
+	return is;  
+}  
+
+std::istream& operator>>(std::istream& is, AlignedBox3d& data)  
+{  
+	is	>> data.min()[0] >> data.min()[1] >> data.min()[2]
+		>> data.max()[0] >> data.max()[1] >> data.max()[2];  
+	return is;  
+}  
+
+#include <cxxopts/cxxopts.hpp>
+
+int main(int argc, char* argv[])
+{
+	cxxopts::Options options(argv[0], "Generates a signed distance field from a closed two-manifold triangle mesh.");
+	options.positional_help("[input OBJ file]");
+
+	options.add_options()
+	("h,help", "Prints this help text")
+	("r,resolution", "Grid resolution", cxxopts::value<std::array<unsigned int, 3>>()->default_value("10 10 10"))
+	("d,domain", "Domain extents (bounding box), format: \"minX minY minZ maxX maxY maxZ\"", cxxopts::value<AlignedBox3d>())
+	("i,invert", "Invert SDF")
+	("o,output", "Ouput file in cdf format", cxxopts::value<std::string>()->default_value(""))
+	("input", "OBJ file containing input triangle mesh", cxxopts::value<std::vector<std::string>>())
+	;
+
+	try
+	{
+		options.parse_positional("input");
+		auto result = options.parse(argc, argv);
+
+		if (result.count("help"))
+		{
+			std::cout << options.help() << std::endl;
+			std::cout << std::endl << std::endl << "Example: GenerateSDF -r \"50 50 50\" dragon.obj" << std::endl;
+			exit(0);
+		}
+		if (!result.count("input"))
+		{
+			std::cout << "ERROR: No input mesh given." << std::endl;
+			std::cout << options.help() << std::endl;
+			std::cout << std::endl << std::endl << "Example: GenerateSDF -r \"50 50 50\" dragon.obj" << std::endl;
+			exit(1);
+		}
+		auto resolution = result["r"].as<std::array<unsigned int, 3>>();
+		auto filename = result["input"].as<std::vector<std::string>>().front();
+
+		if (!std::ifstream(filename).good())
+		{
+			std::cerr << "ERROR: Input file does not exist!" << std::endl;
+			exit(1);
+		}
+
+		std::cout << "Load mesh...";
+		Discregrid::TriangleMesh mesh(filename);
+		std::cout << "DONE" << std::endl;
+
+		std::cout << "Set up data structures...";
+		Discregrid::MeshDistance md(mesh);
+		std::cout << "DONE" << std::endl;
+
+		Eigen::AlignedBox3d domain;
+		domain.setEmpty();
+		if (result.count("d"))
+		{
+			domain = result["d"].as<Eigen::AlignedBox3d>();
+		}
+		if (domain.isEmpty())
+		{
+			for (auto const& x : mesh.vertices())
+			{
+				domain.extend(x);
+			}
+			domain.max() += 1.0e-3 * domain.diagonal().norm() * Vector3d::Ones();
+			domain.min() -= 1.0e-3 * domain.diagonal().norm() * Vector3d::Ones();
+		}
+
+		Discregrid::CubicLagrangeDiscreteGrid sdf(domain, resolution);
+		auto func = Discregrid::DiscreteGrid::ContinuousFunction{};
+		if (result.count("invert"))
+		{
+			func = [&md](Vector3d const& xi) {return -1.0 * md.signedDistanceCached(xi); };
+		}
+		else
+		{
+			func = [&md](Vector3d const& xi) {return md.signedDistanceCached(xi); };
+		}
+
+		std::cout << "Generate discretization..." << std::endl;
+		sdf.addFunction(func, true);
+		std::cout << "DONE" << std::endl;
+
+		std::cout << "Serialize discretization...";
+		auto output_file = result["o"].as<std::string>();
+		if (output_file == "")
+		{
+			output_file = filename;
+			if (output_file.find(".") != std::string::npos)
+			{
+				auto lastindex = output_file.find_last_of(".");
+				output_file = output_file.substr(0, lastindex);
+			}
+			output_file += ".cdf";
+		}
+		sdf.save(output_file);
+		std::cout << "DONE" << std::endl;
+	}
+	catch (cxxopts::OptionException const& e)
+	{
+		std::cout << "error parsing options: " << e.what() << std::endl;
+		exit(1);
+	}
+	
+	return 0;
+}

extern/discregrid/cmd/resource_path.hpp.in

@@ -0,0 +1,7 @@
+
+#ifndef RESOURCE_PATH_HPP__
+#define RESOURCE_PATH_HPP__
+
+static char const* const RESOURCE_PATH = "@RESOURCE_PATH@";
+
+#endif // RESOURCE_PATH_HPP__

extern/discregrid/discregrid/CMakeLists.txt

@@ -0,0 +1,143 @@
+
+set(HEADERS
+	include/Discregrid/discrete_grid.hpp
+	include/Discregrid/cubic_lagrange_discrete_grid.hpp
+)
+
+set(HEADERS_ACCELERATION
+
+	include/Discregrid/acceleration/bounding_sphere_hierarchy.hpp
+	include/Discregrid/acceleration/bounding_sphere.hpp
+	include/Discregrid/acceleration/kd_tree.hpp
+	include/Discregrid/acceleration/kd_tree.inl
+)
+
+set(HEADERS_MESH
+	include/Discregrid/mesh/triangle_mesh.hpp
+	include/Discregrid/mesh/entity_containers.hpp
+	include/Discregrid/mesh/entity_iterators.hpp
+	include/Discregrid/mesh/halfedge.hpp
+)
+
+set(HEADERS_GEOMETRY
+	include/Discregrid/geometry/mesh_distance.hpp
+
+	src/geometry/point_triangle_distance.hpp
+)
+
+set(HEADERS_UTILITY
+	include/Discregrid/utility/serialize.hpp
+	include/Discregrid/utility/lru_cache.hpp
+
+	src/utility/timing.hpp
+	src/utility/spinlock.hpp
+)
+
+set(SOURCES
+	src/discrete_grid.cpp
+	src/cubic_lagrange_discrete_grid.cpp
+)
+
+set(SOURCES_DATA
+)
+
+set(SOURCES_ACCELERATION
+	src/acceleration/bounding_sphere_hierarchy.cpp
+)
+
+set(SOURCES_MESH
+	src/mesh/entity_containers.cpp
+	src/mesh/entity_iterators.cpp
+	src/mesh/triangle_mesh.cpp
+)
+
+set(SOURCES_GEOMETRY
+	src/geometry/mesh_distance.cpp
+	src/geometry/point_triangle_distance.cpp
+)
+
+set(SOURCES_UTILITY
+	src/utility/timing.cpp
+)
+
+macro(SOURCEGROUP name)
+	string(TOLOWER ${name} name_lower)
+	string(SUBSTRING ${name_lower} 0 1 FIRST_LETTER)
+	string(TOUPPER ${FIRST_LETTER} FIRST_LETTER)
+	string(REGEX REPLACE "^.(.*)" "${FIRST_LETTER}\\1" dname "${name_lower}")
+	source_group("Source Files ${dname}" FILES ${SOURCES_${name}})
+	source_group("Header Files ${dname}" FILES ${HEADERS_${name}})
+endmacro()
+SOURCEGROUP(MESH)
+SOURCEGROUP(ACCELERATION)
+SOURCEGROUP(DATA)
+SOURCEGROUP(GEOMETRY)
+SOURCEGROUP(UTILITY)
+
+# OpenMP support.
+if(APPLE)
+	include(PatchOpenMPApple)
+else()
+	find_package(OpenMP REQUIRED)
+endif()
+
+if(OPENMP_FOUND)
+	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+	set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+endif()
+
+# Eigen library.
+find_package(Eigen3 REQUIRED)
+
+# Set include directories.
+include_directories(
+	include
+	include/Discregrid
+	${EIGEN3_INCLUDE_DIR}
+	../extern
+)
+
+# Disable stupid MSVC compiler warnings.
+if(WIN32)
+	add_definitions(-D_SCL_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS)
+	add_definitions(-DNOMINMAX)
+	add_definitions(-D_USE_MATH_DEFINES)
+endif(WIN32)
+
+set(DISCREGRID_SOURCE_FILES
+	${HEADERS}
+	${SOURCES}
+	${HEADERS_ACCELERATION}
+	${SOURCES_ACCELERATION}
+	${SOURCES_DATA}
+	${HEADERS_MESH}
+	${SOURCES_MESH}
+	${HEADERS_GEOMETRY}
+	${SOURCES_GEOMETRY}
+	${HEADERS_UTILITY}
+	${SOURCES_UTILITY}
+)
+
+if(BUILD_AS_SHARED_LIBS)
+	add_library(Discregrid SHARED ${DISCREGRID_SOURCE_FILES})
+else()
+	add_library(Discregrid ${DISCREGRID_SOURCE_FILES})
+endif()
+
+# Discregrid has the following dependencies.
+#add_dependencies(Discregrid
+#)
+
+# Set link libraries.
+target_link_libraries(Discregrid
+)
+
+install(TARGETS Discregrid
+		RUNTIME DESTINATION bin
+		LIBRARY DESTINATION lib
+		ARCHIVE DESTINATION lib)
+
+install(DIRECTORY include
+DESTINATION .
+PATTERN "*.hpp")

extern/discregrid/discregrid/include/Discregrid/All

@@ -0,0 +1,3 @@
+#include "cubic_lagrange_discrete_grid.hpp"
+#include "geometry/mesh_distance.hpp"
+#include "mesh/triangle_mesh.hpp"

extern/discregrid/discregrid/include/Discregrid/acceleration/bounding_sphere.hpp

@@ -0,0 +1,304 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <vector>
+
+namespace Discregrid
+{
+
+/**
+ * \brief Computes smallest enclosing spheres of pointsets using Welzl's algorithm
+ * \Author: Tassilo Kugelstadt
+ */
+class BoundingSphere
+{
+
+public:
+
+	/**
+	 * \brief default constructor sets the center and radius to zero.
+	 */
+	BoundingSphere() : m_x(Eigen::Vector3d::Zero()), m_r(0.0) {}
+
+	/**
+	 * \brief constructor which sets the center and radius
+	 *
+	 * \param x	3d coordinates of the center point
+	 * \param r radius of the sphere
+	 */
+	BoundingSphere(const Eigen::Vector3d& x, double r) : m_x(x), m_r(r) {}
+
+	/**
+	 * \brief	constructs a sphere for one point (with radius 0)
+	 *
+	 * \param a	3d coordinates of point a
+	 */
+	BoundingSphere(const Eigen::Vector3d& a)
+	{
+		m_x = a;
+		m_r = 0.0;
+	}
+
+	/**
+	 * \brief	constructs the smallest enclosing sphere for two points
+	 *
+	 * \param a 3d coordinates of point a
+	 * \param b 3d coordinates of point b
+	 */
+	BoundingSphere(const Eigen::Vector3d& a, const Eigen::Vector3d& b)
+	{
+		const Eigen::Vector3d ba = b - a;
+
+		m_x = (a + b) * 0.5;
+		m_r = 0.5 * ba.norm();
+	}
+
+	/**
+	 * \brief	constructs the smallest enclosing sphere for three points
+	 *
+	 * \param a 3d coordinates of point a
+	 * \param b 3d coordinates of point b
+	 * \param c 3d coordinates of point c
+	 */
+	BoundingSphere(const Eigen::Vector3d& a, const Eigen::Vector3d& b, const Eigen::Vector3d& c)
+	{
+		const Eigen::Vector3d ba = b - a;
+		const Eigen::Vector3d ca = c - a;
+		const Eigen::Vector3d baxca = ba.cross(ca);
+		Eigen::Vector3d r;
+		Eigen::Matrix3d T;
+		T << ba[0], ba[1], ba[2],
+			ca[0], ca[1], ca[2],
+			baxca[0], baxca[1], baxca[2];
+
+		r[0] = 0.5 * ba.squaredNorm();
+		r[1] = 0.5 * ca.squaredNorm();
+		r[2] = 0.0;
+
+		m_x = T.inverse() * r;
+		m_r = m_x.norm();
+		m_x += a;
+	}
+
+	/**
+	 * \brief constructs the smallest enclosing sphere for four points
+	 *
+	 * \param a 3d coordinates of point a
+	 * \param b 3d coordinates of point b
+	 * \param c 3d coordinates of point c
+	 * \param d 3d coordinates of point d
+	 */
+	BoundingSphere(const Eigen::Vector3d& a, const Eigen::Vector3d& b, const Eigen::Vector3d& c, const Eigen::Vector3d& d)
+	{
+		const Eigen::Vector3d ba = b - a;
+		const Eigen::Vector3d ca = c - a;
+		const Eigen::Vector3d da = d - a;
+		Eigen::Vector3d r;
+		Eigen::Matrix3d T;
+		T << ba[0], ba[1], ba[2],
+			ca[0], ca[1], ca[2],
+			da[0], da[1], da[2];
+
+		r[0] = 0.5 * ba.squaredNorm();
+		r[1] = 0.5 * ca.squaredNorm();
+		r[2] = 0.5 * da.squaredNorm();
+		m_x = T.inverse() * r;
+		m_r = m_x.norm();
+		m_x += a;
+	}
+
+	/**
+	 * \brief	constructs the smallest enclosing sphere a given pointset
+	 *
+	 * \param p vertices of the points
+	 */
+	BoundingSphere(const std::vector<Eigen::Vector3d>& p)
+	{
+		m_r = 0;
+		m_x.setZero();
+		setPoints(p);
+	}
+
+	/**
+	 * \brief	Getter for the center of the sphere
+	 *
+	 * \return	const reference of the sphere center
+	 */
+	Eigen::Vector3d const& x() const { return m_x; }
+
+	/**
+	 * \brief	Access function for center of the sphere
+	 *
+	 * \return	reference of the sphere center
+	 */
+	Eigen::Vector3d& x() { return m_x; }
+
+	/**
+	 * \brief	Getter for the radius
+	 *
+	 * \return	Radius of the sphere
+	 */
+	double r() const { return m_r; }
+
+	/**
+	 * \brief	Access function for the radius
+	 *
+	 * \return	Reference to the radius of the sphere
+	 */
+	double& r() { return m_r; }
+
+	/**
+	 * \brief	constructs the smallest enclosing sphere a given pointset
+	 *
+	 * \param p vertices of the points
+	 */
+	void setPoints(const std::vector<Eigen::Vector3d>& p)
+	{
+		//remove duplicates
+		std::vector<Eigen::Vector3d> v(p);
+		std::sort(v.begin(), v.end(), [](const Eigen::Vector3d& a, const Eigen::Vector3d& b)
+			{
+				if (a[0] < b[0]) return true;
+				if (a[0] > b[0]) return false;
+				if (a[1] < b[1]) return true;
+				if (a[1] > b[1]) return false;
+				return (a[2] < b[2]);
+			});
+		v.erase(std::unique(v.begin(), v.end(), [](Eigen::Vector3d& a, Eigen::Vector3d& b) { return a.isApprox(b); }), v.end());
+
+		Eigen::Vector3d d;
+		const int n = int(v.size());
+
+		//generate random permutation of the points and perturb the points by epsilon to avoid corner cases
+		const double epsilon = 1.0e-6;
+		for (int i = n - 1; i > 0; i--)
+		{
+			const Eigen::Vector3d epsilon_vec = epsilon * Eigen::Vector3d::Random();
+			const int j = static_cast<int>(floor(i * double(rand()) / RAND_MAX));
+			d = v[i] + epsilon_vec;
+			v[i] = v[j] - epsilon_vec;
+			v[j] = d;
+		}
+
+		BoundingSphere S = BoundingSphere(v[0], v[1]);
+
+		for (int i = 2; i < n; i++)
+		{
+			//SES0
+			d = v[i] - S.x();
+			if (d.squaredNorm() > S.r()* S.r())
+				S = ses1(i, v, v[i]);
+		}
+
+		m_x = S.m_x;
+		m_r = S.m_r + epsilon;	//add epsilon to make sure that all non-perturbed points are inside the sphere
+	}
+
+	/**
+	 * \brief		intersection test for two spheres
+	 *
+	 * \param other other sphere to be tested for intersection
+	 * \return		returns true when this sphere and the other sphere are intersecting
+	 */
+	bool overlaps(BoundingSphere const& other) const
+	{
+		const double rr = m_r + other.m_r;
+		return (m_x - other.m_x).squaredNorm() < rr * rr;
+	}
+
+	/**
+	 * \brief		tests whether the given sphere other is contained in the sphere
+	 *
+	 * \param		other bounding sphere
+	 * \return		returns true when the other is contained in this sphere or vice versa
+	 */
+	bool contains(BoundingSphere const& other) const
+	{
+		const double rr = r() - other.r();
+		return (x() - other.x()).squaredNorm() < rr * rr;
+	}
+
+	/**
+	 * \brief		tests whether the given point other is contained in the sphere 
+	 *
+	 * \param		other 3d coordinates of a point
+	 * \return		returns true when the point is contained in the sphere
+	 */
+	bool contains(Eigen::Vector3d const& other) const
+	{
+		return (x() - other).squaredNorm() < m_r * m_r;
+	}
+
+private:
+
+	/**
+	 * \brief		constructs the smallest enclosing sphere for n points with the points q1, q2, and q3 on the surface of the sphere
+	 *
+	 * \param n		number of points
+	 * \param p		vertices of the points
+	 * \param q1	3d coordinates of a point on the surface
+	 * \param q2	3d coordinates of a second point on the surface
+	 * \param q3	3d coordinates of a third point on the surface
+	 * \return		smallest enclosing sphere
+	 */
+	BoundingSphere ses3(int n, std::vector<Eigen::Vector3d>& p, Eigen::Vector3d& q1, Eigen::Vector3d& q2, Eigen::Vector3d& q3)
+	{
+		BoundingSphere S(q1, q2, q3);
+
+		for (int i = 0; i < n; i++)
+		{
+			Eigen::Vector3d d = p[i] - S.x();
+			if (d.squaredNorm() > S.r()* S.r())
+				S = BoundingSphere(q1, q2, q3, p[i]);
+		}
+		return S;
+	}
+
+	/**
+	 * \brief		constructs the smallest enclosing sphere for n points with the points q1 and q2 on the surface of the sphere
+	 *
+	 * \param n		number of points
+	 * \param p		vertices of the points
+	 * \param q1	3d coordinates of a point on the surface
+	 * \param q2	3d coordinates of a second point on the surface
+	 * \return		smallest enclosing sphere
+	 */
+	BoundingSphere ses2(int n, std::vector<Eigen::Vector3d>& p, Eigen::Vector3d& q1, Eigen::Vector3d& q2)
+	{
+		BoundingSphere S(q1, q2);
+
+		for (int i = 0; i < n; i++)
+		{
+			Eigen::Vector3d d = p[i] - S.x();
+			if (d.squaredNorm() > S.r()* S.r())
+				S = ses3(i, p, q1, q2, p[i]);
+		}
+		return S;
+	}
+	/**
+	 * \brief		constructs the smallest enclosing sphere for n points with the point q1 on the surface of the sphere
+	 *
+	 * \param n		number of points
+	 * \param p		vertices of the points
+	 * \param q1	3d coordinates of a point on the surface
+	 * \return		smallest enclosing sphere
+	 */
+	BoundingSphere ses1(int n, std::vector<Eigen::Vector3d>& p, Eigen::Vector3d& q1)
+	{
+		BoundingSphere S(p[0], q1);
+
+		for (int i = 1; i < n; i++)
+		{
+			Eigen::Vector3d d = p[i] - S.x();
+			if (d.squaredNorm() > S.r()* S.r())
+				S = ses2(i, p, q1, p[i]);
+		}
+		return S;
+	}
+
+	Eigen::Vector3d m_x;
+	double m_r;
+};
+
+}
+

extern/discregrid/discregrid/include/Discregrid/acceleration/bounding_sphere_hierarchy.hpp

@@ -0,0 +1,71 @@
+#pragma once
+
+#include "bounding_sphere.hpp"
+#include "kd_tree.hpp"
+
+namespace Discregrid
+{
+
+class TriangleMeshBSH : public KDTree<BoundingSphere>
+{
+
+public:
+
+	using super = KDTree<BoundingSphere>;
+
+	TriangleMeshBSH(std::vector<Eigen::Vector3d> const& vertices,
+		std::vector<std::array<unsigned int, 3>> const& faces);
+
+	Eigen::Vector3d const& entityPosition(unsigned int i) const final;
+	void computeHull(unsigned int b, unsigned int n, BoundingSphere& hull) const final;
+
+private:
+
+	std::vector<Eigen::Vector3d> const& m_vertices;
+	std::vector<std::array<unsigned int, 3>> const& m_faces;
+
+	std::vector<Eigen::Vector3d> m_tri_centers;
+};
+
+class TriangleMeshBBH : public KDTree<Eigen::AlignedBox3d>
+{
+public:
+
+	using super = KDTree<Eigen::AlignedBox3d>;
+
+	TriangleMeshBBH(std::vector<Eigen::Vector3d> const& vertices,
+		std::vector<std::array<unsigned int, 3>> const& faces);
+
+	Eigen::Vector3d const& entityPosition(unsigned int i) const final;
+	void computeHull(unsigned int b, unsigned int n, Eigen::AlignedBox3d& hull) const final;
+
+private:
+
+	std::vector<Eigen::Vector3d> const& m_vertices;
+	std::vector<std::array<unsigned int, 3>> const& m_faces;
+
+	std::vector<Eigen::Vector3d> m_tri_centers;
+
+
+};
+
+class PointCloudBSH : public KDTree<BoundingSphere>
+{
+
+public:
+
+	using super = KDTree<BoundingSphere>;
+
+	PointCloudBSH();
+	PointCloudBSH(std::vector<Eigen::Vector3d> const& vertices);
+
+	Eigen::Vector3d const& entityPosition(unsigned int i) const final;
+	void computeHull(unsigned int b, unsigned int n, BoundingSphere& hull)
+		const final;
+
+private:
+
+	std::vector<Eigen::Vector3d> const* m_vertices;
+};
+
+}

extern/discregrid/discregrid/include/Discregrid/acceleration/kd_tree.hpp

@@ -0,0 +1,91 @@
+#pragma once
+
+#include <vector>
+#include <functional>
+#include <algorithm>
+#include <numeric>
+#include <queue>
+#include <iostream>
+
+#include <Eigen/Dense>
+
+#include <array>
+#include <list>
+#include <stack>
+
+namespace Discregrid
+{
+
+template <typename HullType>
+class KDTree
+{
+public:
+
+    using TraversalPredicate = std::function<bool (unsigned int node_index, unsigned int depth)> ;
+    using TraversalCallback = std::function <void (unsigned int node_index, unsigned int depth)>;
+    using TraversalPriorityLess = std::function<bool (std::array<int, 2> const& nodes)>;
+
+    struct Node
+    {
+        Node(unsigned int b_, unsigned int n_)
+            : children({{-1, -1}})
+            , begin(b_), n(n_) {}
+
+        Node() = default;
+
+        bool isLeaf() const { return children[0] < 0 && children[1] < 0; }
+
+        // Index of child nodes in nodes array.
+        // -1 if child does not exist.
+        std::array<int, 2> children;
+
+        // Index according entries in entity list.
+        unsigned int begin;
+
+        // Number of owned entries.
+        unsigned int n;
+    };
+
+    struct QueueItem { unsigned int n, d; };
+    using TraversalQueue = std::queue<QueueItem>;
+
+    KDTree(std::size_t n)
+        : m_lst(n) {}
+
+    virtual ~KDTree() {}
+
+    Node const& node(unsigned int i) const { return m_nodes[i]; }
+    HullType const& hull(unsigned int i) const { return m_hulls[i]; }
+    unsigned int entity(unsigned int i) const { return m_lst[i]; }
+
+    void construct();
+    void update();
+    void traverseDepthFirst(TraversalPredicate pred, TraversalCallback cb,
+        TraversalPriorityLess const& pless = nullptr) const;
+    void traverseBreadthFirst(TraversalPredicate const& pred, TraversalCallback const& cb, unsigned int start_node = 0, TraversalPriorityLess const& pless = nullptr, TraversalQueue& pending = TraversalQueue()) const;
+
+protected:
+
+    void construct(unsigned int node, Eigen::AlignedBox3d const& box,
+        unsigned int b, unsigned int n);
+    void traverseDepthFirst(unsigned int node, unsigned int depth,
+        TraversalPredicate pred, TraversalCallback cb, TraversalPriorityLess const& pless) const;
+    void traverseBreadthFirst(TraversalQueue& pending,
+        TraversalPredicate const& pred, TraversalCallback const& cb, TraversalPriorityLess const& pless = nullptr) const;
+
+    unsigned int addNode(unsigned int b, unsigned int n);
+
+    virtual Eigen::Vector3d const& entityPosition(unsigned int i) const = 0;
+    virtual void computeHull(unsigned int b, unsigned int n, HullType& hull) const = 0;
+
+protected:
+
+    std::vector<unsigned int> m_lst;
+
+    std::vector<Node> m_nodes;
+    std::vector<HullType> m_hulls;
+};
+
+#include "kd_tree.inl"
+}
+

extern/discregrid/discregrid/include/Discregrid/acceleration/kd_tree.inl

@@ -0,0 +1,221 @@
+
+#include "bounding_sphere.hpp"
+#include <stack>
+
+template<typename HullType> void
+KDTree<HullType>::construct()
+{
+    m_nodes.clear();
+    m_hulls.clear();
+    if (m_lst.empty()) return;
+
+    std::iota(m_lst.begin(), m_lst.end(), 0);
+
+    // Determine bounding box of considered domain.
+    auto box = Eigen::AlignedBox3d{};
+    for (auto i = 0u; i < m_lst.size(); ++i)
+        box.extend(entityPosition(i));
+
+    auto ni = addNode(0, static_cast<unsigned int>(m_lst.size()));
+    construct(ni, box, 0, static_cast<unsigned int>(m_lst.size()));
+}
+
+template<typename HullType> void
+KDTree<HullType>::construct(unsigned int node, Eigen::AlignedBox3d const& box, unsigned int b,
+    unsigned int n)
+{
+    // If only one element is left end recursion.
+    //if (n == 1) return;
+    if (n < 10) return;
+
+    // Determine longest side of bounding box.
+    auto max_dir = 0;
+    auto d = box.diagonal().eval();
+    if (d(1) >= d(0) && d(1) >= d(2))
+        max_dir = 1;
+    else if (d(2) >= d(0) && d(2) >= d(1))
+        max_dir = 2;
+
+#ifdef _DEBUG
+    for (auto i = 0u; i < n; ++i)
+    {
+        if (!box.contains(entityPosition(m_lst[b + i])))
+            std::cerr << "ERROR: Bounding box wrong!" << std::endl;
+    }
+#endif
+
+    // Sort range according to center of the longest side.
+    std::sort(m_lst.begin() + b, m_lst.begin() + b + n, 
+        [&](unsigned int a, unsigned int b)
+        {
+            return entityPosition(a)(max_dir) < entityPosition(b)(max_dir);
+        }
+    );
+
+    auto hal = n / 2;
+    auto n0 = addNode(b      , hal    );
+    auto n1 = addNode(b + hal, n - hal);
+    m_nodes[node].children[0] = n0;
+    m_nodes[node].children[1] = n1;
+
+    auto c = 0.5 * (
+        entityPosition(m_lst[b + hal -1])(max_dir) + 
+        entityPosition(m_lst[b + hal   ])(max_dir));
+    auto l_box = box; l_box.max()(max_dir) = c;
+    auto r_box = box; r_box.min()(max_dir) = c;
+
+    construct(m_nodes[node].children[0], l_box, b, hal);
+    construct(m_nodes[node].children[1], r_box, b + hal, n - hal);
+}
+
+template<typename HullType> void
+KDTree<HullType>::traverseDepthFirst(TraversalPredicate pred, TraversalCallback cb,
+    TraversalPriorityLess const& pless) const
+{
+    if (m_nodes.empty())
+        return;
+
+    if (pred(0, 0)) 
+        traverseDepthFirst(0, 0, pred, cb, pless);
+}
+
+template<typename HullType> void
+KDTree<HullType>::traverseDepthFirst(unsigned int node_index, 
+    unsigned int depth, TraversalPredicate pred, TraversalCallback cb,
+    TraversalPriorityLess const& pless) const
+{
+
+    //auto pending = std::stack<QueueItem>{};
+    //pending.push({node_index, depth});
+    //while (!pending.empty())
+    //{
+    //    auto n = pending.top().n;
+    //    auto d = pending.top().d;
+    //    auto const& node = m_nodes[n];
+    //    pending.pop();
+
+    //    cb(n, d);
+    //    auto is_pred = pred(n, d);
+    //    if (!node.is_leaf() && is_pred)
+    //    {
+    //        if (pless && !pless(node.children))
+    //        {
+    //            pending.push({ static_cast<unsigned int>(node.children[1]), d + 1 });
+    //            pending.push({ static_cast<unsigned int>(node.children[0]), d + 1 });
+    //        }
+    //        else
+    //        {
+    //            pending.push({ static_cast<unsigned int>(node.children[0]), d + 1 });
+    //            pending.push({ static_cast<unsigned int>(node.children[1]), d + 1 });
+    //        }
+    //    }
+    //}
+
+
+    Node const& node = m_nodes[node_index];
+
+    cb(node_index, depth);
+    auto is_pred = pred(node_index, depth);
+    if (!node.isLeaf() && is_pred)
+    {
+        if (pless && !pless(node.children))
+        {
+            traverseDepthFirst(m_nodes[node_index].children[1], depth + 1, pred, cb, pless);
+            traverseDepthFirst(m_nodes[node_index].children[0], depth + 1, pred, cb, pless);
+        }
+        else
+        {
+            traverseDepthFirst(m_nodes[node_index].children[0], depth + 1, pred, cb, pless);
+            traverseDepthFirst(m_nodes[node_index].children[1], depth + 1, pred, cb, pless);
+        }
+    }
+
+
+
+
+        //    auto n = pending.front().n;
+        //auto d = pending.front().d;
+        //auto const& node = m_nodes[n];
+        //pending.pop();
+
+        //cb(n, d);
+        //auto is_pred = pred(n, d);
+        //if (!node.is_leaf() && is_pred)
+        //{
+        //    if (pless && !pless(node.children))
+        //    {
+        //        pending.push({ static_cast<unsigned int>(node.children[1]), d + 1 });
+        //        pending.push({ static_cast<unsigned int>(node.children[0]), d + 1 });
+        //    }
+        //    else
+        //    {
+        //        pending.push({ static_cast<unsigned int>(node.children[0]), d + 1 });
+        //        pending.push({ static_cast<unsigned int>(node.children[1]), d + 1 });
+        //    }
+        //}
+}
+
+
+template <typename HullType> void
+KDTree<HullType>::traverseBreadthFirst(TraversalPredicate const& pred, 
+    TraversalCallback const& cb, unsigned int start_node, TraversalPriorityLess const& pless,
+    TraversalQueue& pending) const
+{
+    //auto pending = TraversalQueue{};
+
+    cb(start_node, 0);
+    if (pred(start_node, 0)) pending.push({ start_node, 0 });
+    traverseBreadthFirst(pending, pred, cb, pless);
+}
+
+template <typename HullType> unsigned int
+KDTree<HullType>::addNode(unsigned int b, unsigned int n)
+{
+    HullType hull;
+    computeHull(b, n, hull);
+    m_hulls.push_back(hull);
+    m_nodes.push_back({ b, n });
+    return static_cast<unsigned int>(m_nodes.size() - 1);
+}
+
+template <typename HullType> void
+KDTree<HullType>::update()
+{
+    traverseDepthFirst(
+        [&](unsigned int, unsigned int) { return true; },
+        [&](unsigned int node_index, unsigned int)
+        {
+            auto const& nd = node(node_index);
+            computeHull(nd.begin, nd.n, hull(node_index));
+        }
+    );
+}
+
+template <typename HullType> void 
+KDTree<HullType>::traverseBreadthFirst(TraversalQueue& pending, 
+    TraversalPredicate const& pred, TraversalCallback const& cb, TraversalPriorityLess const& pless) const
+{
+    while (!pending.empty())
+    {
+        auto n = pending.front().n;
+        auto d = pending.front().d;
+        auto const& node = m_nodes[n];
+        pending.pop();
+
+        cb(n, d);
+        auto is_pred = pred(n, d);
+        if (!node.is_leaf() && is_pred)
+        {
+            if (pless && !pless(node.children))
+            {
+                pending.push({ static_cast<unsigned int>(node.children[1]), d + 1 });
+                pending.push({ static_cast<unsigned int>(node.children[0]), d + 1 });
+            }
+            else
+            {
+                pending.push({ static_cast<unsigned int>(node.children[0]), d + 1 });
+                pending.push({ static_cast<unsigned int>(node.children[1]), d + 1 });
+            }
+        }
+    }
+}

extern/discregrid/discregrid/include/Discregrid/cubic_lagrange_discrete_grid.hpp

@@ -0,0 +1,78 @@
+#pragma once
+
+#include "discrete_grid.hpp"
+
+namespace Discregrid
+{
+
+class CubicLagrangeDiscreteGrid : public DiscreteGrid
+{
+public:
+
+	CubicLagrangeDiscreteGrid(){ this->m_n_cells = 0; }
+	CubicLagrangeDiscreteGrid(std::string const& filename);
+	CubicLagrangeDiscreteGrid(Eigen::AlignedBox3d const& domain,
+		std::array<unsigned int, 3> const& resolution);
+
+	void save(std::string const& filename) const override;
+	void load(std::string const& filename) override;
+
+	unsigned int addFunction(
+		ContinuousFunction const& func,
+		std::vector<std::thread::id> *thread_map,
+		bool verbose = false,
+		SamplePredicate const& pred = nullptr) override;
+
+
+	std::size_t nCells() const { return m_n_cells; };
+	double interpolate(unsigned int field_id, Eigen::Vector3d const& xi,
+		Eigen::Vector3d* gradient = nullptr) const override;
+
+	/**
+	 * @brief Determines the shape functions for the discretization with ID field_id at point xi.
+	 * 
+	 * @param field_id Discretization ID
+	 * @param x Location where the shape functions should be determined
+	 * @param cell cell of x
+	 * @param c0 vector required for the interpolation
+	 * @param N	shape functions for the cell of x
+	 * @param dN (Optional) derivatives of the shape functions, required to compute the gradient
+	 * @return Success of the function.
+	 */
+	bool determineShapeFunctions(unsigned int field_id, Eigen::Vector3d const &x,
+		std::array<unsigned int, 32> &cell, Eigen::Vector3d &c0, Eigen::Matrix<double, 32, 1> &N, 
+		Eigen::Matrix<double, 32, 3> *dN = nullptr) const override;
+
+	/**
+	 * @brief Evaluates the given discretization with ID field_id at point xi.
+	 * 
+	 * @param field_id Discretization ID
+	 * @param xi Location where the discrete function is evaluated
+	 * @param cell cell of xi
+	 * @param c0 vector required for the interpolation
+	 * @param N	shape functions for the cell of xi
+	 * @param gradient (Optional) if a pointer to a vector is passed the gradient of the discrete function will be evaluated
+	 * @param dN (Optional) derivatives of the shape functions, required to compute the gradient
+	 * @return double Results of the evaluation of the discrete function at point xi
+	 */
+	double interpolate(unsigned int field_id, Eigen::Vector3d const& xi, const std::array<unsigned int, 32> &cell, const Eigen::Vector3d &c0, const Eigen::Matrix<double, 32, 1> &N,
+		Eigen::Vector3d* gradient = nullptr, Eigen::Matrix<double, 32, 3> *dN = nullptr) const override;
+
+	void reduceField(unsigned int field_id, Predicate pred) override;
+
+	void forEachCell(unsigned int field_id,
+		std::function<void(unsigned int, Eigen::AlignedBox3d const&, unsigned int)> const& cb) const;
+
+private:
+
+	Eigen::Vector3d indexToNodePosition(unsigned int l) const;
+
+
+private:
+
+	std::vector<std::vector<double>> m_nodes;
+	std::vector<std::vector<std::array<unsigned int, 32>>> m_cells;
+	std::vector<std::vector<unsigned int>> m_cell_map;
+};
+
+}

extern/discregrid/discregrid/include/Discregrid/discrete_grid.hpp

@@ -0,0 +1,105 @@
+#pragma once
+
+#include <vector>
+#include <fstream>
+#include <array>
+#include <Eigen/Dense>
+#include <thread>
+#include <map>
+
+namespace Discregrid
+{
+
+class DiscreteGrid
+{
+public:
+
+	using CoefficientVector = Eigen::Matrix<double, 32, 1>;
+	using ContinuousFunction = std::function<double(Eigen::Vector3d const&)>;
+	using MultiIndex = std::array<unsigned int, 3>;
+	using Predicate = std::function<bool(Eigen::Vector3d const&, double)>;
+	using SamplePredicate = std::function<bool(Eigen::Vector3d const&)>;
+
+	DiscreteGrid() = default;
+	DiscreteGrid(Eigen::AlignedBox3d const& domain, std::array<unsigned int, 3> const& resolution)
+		: m_domain(domain), m_resolution(resolution), m_n_fields(0u)
+	{
+		auto n = Eigen::Matrix<unsigned int, 3, 1>::Map(resolution.data());
+		m_cell_size = domain.diagonal().cwiseQuotient(n.cast<double>());
+		m_inv_cell_size = m_cell_size.cwiseInverse();
+		m_n_cells = n.prod();
+	}
+	virtual ~DiscreteGrid() = default;
+
+	virtual void save(std::string const& filename) const = 0;
+	virtual void load(std::string const& filename) = 0;
+
+	virtual unsigned int addFunction(
+		ContinuousFunction const& func,
+		std::vector<std::thread::id> *thread_map,
+		bool verbose = false,
+		SamplePredicate const& pred = nullptr) = 0;
+
+	double interpolate(Eigen::Vector3d const& xi, Eigen::Vector3d* gradient = nullptr) const
+	{
+		return interpolate(0u, xi, gradient);
+	}
+
+	virtual double interpolate(unsigned int field_id, Eigen::Vector3d const& xi,
+		Eigen::Vector3d* gradient = nullptr) const = 0;
+
+	/**
+	 * @brief Determines the shape functions for the discretization with ID field_id at point xi.
+	 * 
+	 * @param field_id Discretization ID
+	 * @param x Location where the shape functions should be determined
+	 * @param cell cell of x
+	 * @param c0 vector required for the interpolation
+	 * @param N	shape functions for the cell of x
+	 * @param dN (Optional) derivatives of the shape functions, required to compute the gradient
+	 * @return Success of the function.
+	 */
+	virtual bool determineShapeFunctions(unsigned int field_id, Eigen::Vector3d const &x,
+		std::array<unsigned int, 32> &cell, Eigen::Vector3d &c0, Eigen::Matrix<double, 32, 1> &N,
+		Eigen::Matrix<double, 32, 3> *dN = nullptr) const = 0;
+
+	/**
+	 * @brief Evaluates the given discretization with ID field_id at point xi.
+	 * 
+	 * @param field_id Discretization ID
+	 * @param xi Location where the discrete function is evaluated
+	 * @param cell cell of xi
+	 * @param c0 vector required for the interpolation
+	 * @param N	shape functions for the cell of xi
+	 * @param gradient (Optional) if a pointer to a vector is passed the gradient of the discrete function will be evaluated
+	 * @param dN (Optional) derivatives of the shape functions, required to compute the gradient
+	 * @return double Results of the evaluation of the discrete function at point xi
+	 */
+	virtual double interpolate(unsigned int field_id, Eigen::Vector3d const& xi, const std::array<unsigned int, 32> &cell, const Eigen::Vector3d &c0, const Eigen::Matrix<double, 32, 1> &N,
+		Eigen::Vector3d* gradient = nullptr, Eigen::Matrix<double, 32, 3> *dN = nullptr) const = 0;
+
+	virtual void reduceField(unsigned int field_id, Predicate pred) {}
+
+
+	MultiIndex singleToMultiIndex(unsigned int i) const;
+	unsigned int multiToSingleIndex(MultiIndex const& ijk) const;
+
+	Eigen::AlignedBox3d subdomain(MultiIndex const& ijk) const;
+	Eigen::AlignedBox3d subdomain(unsigned int l) const;
+
+	Eigen::AlignedBox3d const& domain() const { return m_domain; }
+	std::array<unsigned int, 3> const& resolution() const { return m_resolution; };
+	Eigen::Vector3d const& cellSize() const { return m_cell_size;}
+	Eigen::Vector3d const& invCellSize() const { return m_inv_cell_size;}
+
+protected:
+
+
+	Eigen::AlignedBox3d m_domain;
+	std::array<unsigned int, 3> m_resolution;
+	Eigen::Vector3d m_cell_size;
+	Eigen::Vector3d m_inv_cell_size;
+	std::size_t m_n_cells;
+	std::size_t m_n_fields;
+};
+}

extern/discregrid/discregrid/include/Discregrid/geometry/mesh_distance.hpp

@@ -0,0 +1,126 @@
+#pragma once
+
+#include <Discregrid/mesh/triangle_mesh.hpp>
+
+namespace std {
+	template <> struct hash<Eigen::Vector3d>
+	{
+		std::size_t operator()(Eigen::Vector3d const& x) const
+		{
+			std::size_t seed = 0;
+			std::hash<double> hasher;
+			seed ^= hasher(x[0]) + 0x9e3779b9 + (seed<<6) + (seed>>2);
+			seed ^= hasher(x[1]) + 0x9e3779b9 + (seed<<6) + (seed>>2);
+			seed ^= hasher(x[2]) + 0x9e3779b9 + (seed<<6) + (seed>>2);
+			return seed;
+		}
+	};
+
+	template <> struct less<Eigen::Vector3d>
+	{
+		bool operator()(Eigen::Vector3d const& left, Eigen::Vector3d const& right) const
+		{
+			for (auto i = 0u; i < 3u; ++i)
+			{
+				if (left(i) < right(i))
+					return true;
+				else if (left(i) > right(i))
+					return false;
+			}
+			return false;
+		}
+	};
+}
+#include <Discregrid/utility/lru_cache.hpp>
+
+#include <Discregrid/acceleration/bounding_sphere_hierarchy.hpp>
+
+
+#include <array>
+#include <vector>
+#include <set>
+#include <map>
+#include <unordered_map>
+#include <thread>
+
+#include <Eigen/Dense>
+
+namespace Discregrid
+{
+
+enum class NearestEntity;
+class TriangleMesh;
+class Halfedge;
+class MeshDistance
+{
+
+	struct Candidate
+	{
+		bool operator<(Candidate const& other) const { return b < other.b; }
+		unsigned int node_index;
+		double b, w;
+	};
+
+public:
+
+	MeshDistance(TriangleMesh const& mesh, bool precompute_normals = true);
+
+	// Returns the shortest unsigned distance from a given point x to
+	// the stored mesh.
+	// Thread-safe function.
+	double distance(Eigen::Vector3d const& x, Eigen::Vector3d* nearest_point = nullptr,
+		unsigned int* nearest_face = nullptr, NearestEntity* ne = nullptr) const;
+
+	// Requires a closed two-manifold mesh as input data.
+	// Thread-safe function.
+	double signedDistance(Eigen::Vector3d const& x) const;
+	double signedDistanceCached(Eigen::Vector3d const& x) const;
+
+	double unsignedDistance(Eigen::Vector3d const& x) const;
+	double unsignedDistanceCached(Eigen::Vector3d const& x) const;
+
+	// So, the original discregrid uses OpenMP which has
+	// handy functions like omp_get_thread_num(). Switching to non-OpenMP
+	// requires some overhead to get the current thread ID which is what
+	// you see here. So this pointer is passed to both the SDF
+	// generator and this SDF evaluator. The generator fills the thread map
+	// and the evaluator reads from it.
+	// Not ideal and kind of clunky, but a quick workaround for now.
+	void set_thread_map(std::vector<std::thread::id> *thread_map_) {
+		thread_map = thread_map_;
+	}
+
+private:
+
+	Eigen::Vector3d vertex_normal(unsigned int v) const;
+	Eigen::Vector3d edge_normal(Halfedge const& h) const;
+	Eigen::Vector3d face_normal(unsigned int f) const;
+
+	void callback(unsigned int node_index, TriangleMeshBSH const& bsh,
+		Eigen::Vector3d const& x, 
+		double& dist) const;
+
+	bool predicate(unsigned int node_index, TriangleMeshBSH const& bsh, 
+		Eigen::Vector3d const& x, double& dist) const;
+
+	int get_thread_num() const;
+
+private:
+
+	TriangleMeshBSH m_bsh;
+	TriangleMesh const& m_mesh;
+	mutable std::vector<std::thread::id> *thread_map;
+
+	using FunctionValueCache = LRUCache<Eigen::Vector3d, double>;
+	mutable std::vector<TriangleMeshBSH::TraversalQueue> m_queues;
+	mutable std::vector<unsigned int> m_nearest_face;
+	mutable std::vector<FunctionValueCache> m_cache;
+	mutable std::vector<FunctionValueCache> m_ucache;
+	
+	std::vector<Eigen::Vector3d> m_face_normals;
+	std::vector<Eigen::Vector3d> m_vertex_normals;
+	bool m_precomputed_normals;
+};
+
+}
+

extern/discregrid/discregrid/include/Discregrid/mesh/entity_containers.hpp

@@ -0,0 +1,111 @@
+#pragma once
+
+#include "entity_iterators.hpp"
+
+namespace Discregrid
+{
+
+class TriangleMesh;
+
+class FaceContainer
+{
+
+public:
+
+	FaceIterator begin() const
+	{
+		return FaceIterator(0, m_mesh);
+	}
+	FaceIterator end() const;
+
+private:
+
+	friend class TriangleMesh;
+	FaceContainer(TriangleMesh* mesh) : m_mesh(mesh) {}
+
+	TriangleMesh* m_mesh;
+};
+
+class FaceConstContainer
+{
+
+public:
+
+	FaceConstIterator begin() const
+	{
+		return FaceConstIterator(0, m_mesh);
+	}
+	FaceConstIterator end() const;
+
+private:
+
+	friend class TriangleMesh;
+	FaceConstContainer(TriangleMesh const* mesh) : m_mesh(mesh) {}
+
+	TriangleMesh const* m_mesh;
+};
+
+
+class IncidentFaceContainer
+{
+
+public:
+
+	IncidentFaceIterator begin() const
+	{
+		return IncidentFaceIterator(m_v, m_mesh);
+	}
+	IncidentFaceIterator end() const
+	{
+		return IncidentFaceIterator();
+	}
+
+private:
+
+	friend class TriangleMesh;
+	IncidentFaceContainer(unsigned int v, TriangleMesh const* mesh) 
+		: m_mesh(mesh), m_v(v) {}
+
+	TriangleMesh const* m_mesh;
+	unsigned int m_v;
+};
+
+class VertexContainer
+{
+
+public:
+
+	VertexIterator begin() const
+	{
+		return VertexIterator(0, m_mesh);
+	}
+	VertexIterator end() const;
+
+private:
+
+	friend class TriangleMesh;
+	VertexContainer(TriangleMesh* mesh) : m_mesh(mesh) {}
+
+	TriangleMesh* m_mesh;
+};
+
+class VertexConstContainer
+{
+
+public:
+
+	VertexConstIterator begin() const
+	{
+		return VertexConstIterator(0, m_mesh);
+	}
+	VertexConstIterator end() const;
+
+private:
+
+	friend class TriangleMesh;
+	VertexConstContainer(TriangleMesh const* mesh) : m_mesh(mesh) {}
+
+	TriangleMesh const* m_mesh;
+};
+}
+

extern/discregrid/discregrid/include/Discregrid/mesh/entity_iterators.hpp

@@ -0,0 +1,264 @@
+#pragma once
+
+#include "halfedge.hpp"
+
+#include <iterator>
+#include <array>
+#include <Eigen/Core>
+
+namespace Discregrid
+{
+
+class TriangleMesh;
+
+class FaceContainer;
+class FaceIterator : public
+	std::iterator<std::random_access_iterator_tag, std::array<unsigned int, 3>>
+{
+public:
+
+	typedef FaceIterator _Mytype;
+
+	FaceIterator() = delete;
+
+	reference operator*();
+
+	bool operator<(_Mytype const& other) const
+	{
+		return m_index < other.m_index;
+	}
+	bool operator==(_Mytype const& other) const
+	{
+		return m_index == other.m_index;
+	}
+
+	bool operator!=(_Mytype const& other) const
+	{
+		return !(*this == other);
+	}
+
+	inline _Mytype& operator++() { ++m_index; return *this; }
+	inline _Mytype& operator--() { --m_index; return *this; }
+
+	inline _Mytype operator+(_Mytype const& rhs)
+	{
+		return _Mytype(m_index + rhs.m_index, m_mesh);
+	}
+	inline difference_type operator-(_Mytype const& rhs)
+	{
+		return m_index - rhs.m_index;
+	}
+	inline _Mytype operator-(int const& rhs)
+	{
+		return _Mytype(m_index - rhs, m_mesh);
+	}
+
+	unsigned int vertex(unsigned int i) const;
+	unsigned int& vertex(unsigned int i);
+
+private:
+
+	friend class FaceContainer;
+	FaceIterator(unsigned int index, TriangleMesh* mesh)
+		: m_index(index), m_mesh(mesh) {}
+
+	unsigned int m_index;
+	TriangleMesh* m_mesh;
+};
+class FaceConstIterator : public
+	std::iterator<std::random_access_iterator_tag, std::array<unsigned int, 3> const>
+{
+
+public:
+
+	typedef FaceConstIterator _Mytype;
+
+	FaceConstIterator() = delete;
+
+	reference operator*();
+
+	bool operator<(_Mytype const& other) const
+	{
+		return m_index < other.m_index;
+	}
+	bool operator==(_Mytype const& other) const
+	{
+		return m_index == other.m_index;
+	}
+
+	bool operator!=(_Mytype const& other) const
+	{
+		return !(*this == other);
+	}
+
+	inline _Mytype& operator++() { ++m_index; return *this; }
+	inline _Mytype& operator--() { --m_index; return *this; }
+
+	inline _Mytype operator+(_Mytype const& rhs) const
+	{
+		return _Mytype(m_index + rhs.m_index, m_mesh);
+	}
+	inline difference_type operator-(_Mytype const& rhs) const
+	{
+		return m_index - rhs.m_index;
+	}
+	inline _Mytype operator-(int const& rhs) const
+	{
+		return _Mytype(m_index - rhs, m_mesh);
+	}
+
+	unsigned int vertex(unsigned int i) const;
+	unsigned int& vertex(unsigned int i);
+
+private:
+
+	friend class FaceConstContainer;
+	FaceConstIterator(unsigned int index, TriangleMesh const* mesh)
+		: m_index(index), m_mesh(mesh) {}
+
+	unsigned int m_index;
+	TriangleMesh const* m_mesh;
+};
+
+class IncidentFaceContainer;
+class IncidentFaceIterator : public std::iterator<std::forward_iterator_tag, Halfedge>
+{
+
+public:
+
+	typedef IncidentFaceIterator _Mytype;
+
+	value_type operator*() { return m_h; }
+	_Mytype& operator++();
+	bool operator==(_Mytype const& other) const
+	{
+		return m_h == other.m_h;
+	}
+
+	bool operator!=(_Mytype const& other) const
+	{
+		return !(*this == other);
+	}
+
+private:
+
+	friend class IncidentFaceContainer;
+	IncidentFaceIterator(unsigned int v, TriangleMesh const* mesh);
+	IncidentFaceIterator() : m_h(), m_begin(), m_mesh(nullptr) {}
+
+	Halfedge m_h, m_begin;
+	TriangleMesh const* m_mesh;
+};
+
+
+class VertexContainer;
+class VertexIterator : public std::iterator<std::random_access_iterator_tag, Eigen::Vector3d>
+{
+
+public:
+
+	typedef VertexIterator _Mytype;
+
+	VertexIterator() = delete;
+
+	reference operator*();
+
+	bool operator<(_Mytype const& other) const
+	{
+		return m_index < other.m_index;
+	}
+	bool operator==(_Mytype const& other) const
+	{
+		return m_index == other.m_index;
+	}
+
+	bool operator!=(_Mytype const& other) const
+	{
+		return !(*this == other);
+	}
+
+	inline _Mytype& operator++() { ++m_index; return *this; }
+	inline _Mytype& operator--() { --m_index; return *this; }
+
+	inline _Mytype operator+(_Mytype const& rhs) const
+	{ 
+		return _Mytype(m_index + rhs.m_index, m_mesh); 
+	}
+	inline difference_type operator-(_Mytype const& rhs) const
+	{ 
+		return m_index - rhs.m_index; 
+	}
+	inline _Mytype operator-(int const& rhs) const
+	{ 
+		return _Mytype(m_index - rhs, m_mesh); 
+	}
+
+	unsigned int index() const;
+
+private:
+
+	friend class VertexContainer;
+	VertexIterator(unsigned int index, TriangleMesh* mesh)
+		: m_index(index), m_mesh(mesh) {}
+
+	unsigned int m_index;
+	TriangleMesh* m_mesh;
+};
+
+
+class VertexConstContainer;
+class VertexConstIterator : 
+	public std::iterator<std::random_access_iterator_tag, Eigen::Vector3d const>
+{
+
+public:
+
+	typedef VertexConstIterator _Mytype;
+
+	VertexConstIterator() = delete;
+
+	reference operator*();
+
+	bool operator<(_Mytype const& other) const
+	{
+		return m_index < other.m_index;
+	}
+	bool operator==(_Mytype const& other) const
+	{
+		return m_index == other.m_index;
+	}
+
+	bool operator!=(_Mytype const& other) const
+	{
+		return !(*this == other);
+	}
+
+	inline _Mytype& operator++() { ++m_index; return *this; }
+	inline _Mytype& operator--() { --m_index; return *this; }
+
+	inline _Mytype operator+(_Mytype const& rhs) const
+	{ 
+		return _Mytype(m_index + rhs.m_index, m_mesh); 
+	}
+	inline difference_type operator-(_Mytype const& rhs) const
+	{ 
+		return m_index - rhs.m_index; 
+	}
+	inline _Mytype operator-(int const& rhs) const
+	{ 
+		return _Mytype(m_index - rhs, m_mesh); 
+	}
+
+	unsigned int index() const;
+
+private:
+
+	friend class VertexConstContainer;
+	VertexConstIterator(unsigned int index, TriangleMesh const* mesh)
+		: m_index(index), m_mesh(mesh) {}
+
+	unsigned int m_index;
+	TriangleMesh const* m_mesh;
+};
+}
+

extern/discregrid/discregrid/include/Discregrid/mesh/halfedge.hpp

@@ -0,0 +1,46 @@
+#pragma once
+
+#include <cassert>
+
+namespace Discregrid
+{
+
+class Halfedge
+{
+public:
+
+    Halfedge() : m_code(3) {}
+    Halfedge(Halfedge const&) = default;
+    Halfedge(unsigned int f, unsigned char e)
+        : m_code((f << 2) | e)
+    {
+        //assert(e < 3);
+    }
+
+    Halfedge next() const
+    {
+        return Halfedge(face(), (edge() + 1) % 3);
+    }
+
+    Halfedge previous() const
+    {
+        return Halfedge(face(), (edge() + 2) % 3);
+    }
+
+    bool operator==(Halfedge const& other) const
+    {
+        return m_code == other.m_code;
+    }
+
+    unsigned int face() const { return m_code >> 2; }
+    unsigned char edge() const { return m_code & 0x3; }
+    bool isBoundary() const { return edge() == 3; }
+
+private:
+
+    Halfedge(unsigned int code) : m_code(code) {}
+    unsigned int m_code;
+};
+}
+
+

extern/discregrid/discregrid/include/Discregrid/mesh/triangle_mesh.hpp

@@ -0,0 +1,111 @@
+#pragma once
+
+#include "halfedge.hpp"
+#include "entity_containers.hpp"
+
+#include <vector>
+#include <array>
+#include <cassert>
+#include <string>
+
+#include <Eigen/Dense>
+
+namespace Discregrid
+{
+
+class TriangleMesh
+{
+
+public:
+
+	TriangleMesh(std::vector<Eigen::Vector3d> const& vertices, 
+		std::vector<std::array<unsigned int, 3>> const& faces);
+
+	TriangleMesh(double const* vertices,
+		unsigned int const* faces, 
+		std::size_t nv, std::size_t nf);
+
+	TriangleMesh(std::string const& filename);
+
+	bool is_closed() const { return m_is_closed; }
+
+	void exportOBJ(std::string const& filename) const;
+
+	// Halfedge modifiers.
+	unsigned int source(Halfedge const h) const
+	{
+		if (h.isBoundary()) return target(opposite(h));
+		return m_faces[h.face()][h.edge()];
+	}
+	unsigned int target(Halfedge const h) const
+	{
+		if (h.isBoundary()) return source(opposite(h));
+		return source(h.next());
+	}
+	Halfedge opposite(Halfedge const h) const
+	{
+		if (h.isBoundary()) return m_b2e[h.face()];
+		return m_e2e[h.face()][h.edge()];
+	}
+
+	// Container getters.
+	FaceContainer faces() { return FaceContainer(this); }
+	FaceConstContainer faces() const { return FaceConstContainer(this); }
+	IncidentFaceContainer incident_faces(unsigned int v) const { 
+		return IncidentFaceContainer(v, this); }
+	VertexContainer vertices() { return VertexContainer(this); }
+	VertexConstContainer vertices() const { return VertexConstContainer(this); }
+
+	// Entity size getters.
+	std::size_t nFaces() const { return m_faces.size(); }
+	std::size_t nVertices() const { return m_v2e.size(); }
+	std::size_t nBorderEdges() const { return m_b2e.size(); }
+
+	// Entity getters.
+	unsigned int const& faceVertex(unsigned int f, unsigned int i) const 
+	{
+		assert(i < 3);
+		assert(f < m_faces.size());
+		return m_faces[f][i];
+	}
+	unsigned int& faceVertex(unsigned int f, unsigned int i)
+	{
+		assert(i < 3);
+		assert(f < m_faces.size());
+		return m_faces[f][i];
+	}
+
+	Eigen::Vector3d const& vertex(unsigned int i) const { return m_vertices[i]; }
+	Eigen::Vector3d& vertex(unsigned int i) { return m_vertices[i]; }
+	std::array<unsigned int, 3> const& face(unsigned int i) const { 
+		return m_faces[i]; }
+	std::array<unsigned int, 3>& face(unsigned int i) {
+		return m_faces[i];
+	}
+	Halfedge incident_halfedge(unsigned int v) const { return m_v2e[v]; }
+
+	// Data getters.
+	std::vector<Eigen::Vector3d> const& vertex_data() const { 
+		return m_vertices; }
+	std::vector<Eigen::Vector3d>& vertex_data() { return m_vertices; }    
+	std::vector<std::array<unsigned int, 3>> const& face_data() const { 
+		return m_faces; }
+	std::vector<std::array<unsigned int, 3>>& face_data() { return m_faces; }
+
+	Eigen::Vector3d computeFaceNormal(unsigned int f) const;
+
+private:
+
+	void construct();
+
+private:
+
+	std::vector<Eigen::Vector3d> m_vertices;
+	std::vector<std::array<unsigned int, 3>> m_faces;
+	std::vector<std::array<Halfedge, 3>> m_e2e;
+	std::vector<Halfedge> m_v2e;
+	std::vector<Halfedge> m_b2e;
+	bool m_is_closed;
+};
+}
+

extern/discregrid/discregrid/include/Discregrid/utility/lru_cache.hpp

@@ -0,0 +1,137 @@
+#pragma once
+
+#include <cassert> 
+#include <list> 
+#include <map>
+ 
+// Class providing fixed-size (by number of records) 
+// LRU-replacement cache of a function with signature 
+// V f(K). 
+// MAP should be one of std::map or std::unordered_map. 
+// Variadic template args used to deal with the 
+// different type argument signatures of those 
+// containers; the default comparator/hash/allocator 
+// will be used. 
+template <typename K, typename V>
+class LRUCache
+{
+
+public: 
+ 
+    using key_type = K;
+    using value_type = V;
+ 
+    // Key access history, most recent at back 
+    using key_tracker_type = std::list<key_type>; 
+ 
+    // Key to value and key history iterator 
+    //using key_to_value_type = MAP<key_type, std::pair<value_type, 
+    //    typename key_tracker_type::iterator>>;
+    using key_to_value_type = std::map<key_type, std::pair<value_type, 
+        typename key_tracker_type::iterator>>;
+
+ 
+    using eval_func_type = std::function<value_type(const key_type&)>;
+
+    // Constuctor specifies the cached function and 
+    // the maximum number of records to be stored 
+    LRUCache(eval_func_type  const& f, std::size_t c) 
+    : _fn(f), _capacity(c) 
+    { 
+        assert(_capacity!=0); 
+    } 
+ 
+    // Obtain value of the cached function for k 
+    value_type operator()(const key_type& k)
+    {
+        // Attempt to find existing record 
+        auto it =_key_to_value.find(k); 
+ 
+        if (it == _key_to_value.end())
+        { 
+            // We don't have it: 
+ 
+            // Evaluate function and create new record 
+            auto v = _fn(k); 
+            insert(k,v); 
+ 
+            // Return the freshly computed value 
+            return v; 
+ 
+        }
+        else
+        {
+            // We do have it:  
+            // Update access record by moving 
+            // accessed key to back of list 
+            _key_tracker.splice(_key_tracker.end(), _key_tracker, (*it).second.second);
+ 
+            // Return the retrieved value 
+            return (*it).second.first; 
+        } 
+    } 
+ 
+    // Obtain the cached keys, most recently used element 
+    // at head, least recently used at tail. 
+    // This method is provided purely to support testing. 
+    template <typename IT>
+    void getKeys(IT dst) const
+    {
+        auto src =_key_tracker.rbegin(); 
+        while (src!=_key_tracker.rend())
+        {
+            *dst++ = *src++; 
+        }
+    } 
+ 
+private: 
+ 
+    // Record a fresh key-value pair in the cache 
+    void insert(key_type const& k, value_type const& v)
+    { 
+ 
+        // Method is only called on cache misses 
+        //assert(_key_to_value.find(k)==_key_to_value.end()); 
+ 
+        // Make space if necessary 
+        if (_key_to_value.size() == _capacity) 
+            evict(); 
+ 
+        // Record k as most-recently-used key 
+        auto it =_key_tracker.insert(_key_tracker.end(),k); 
+ 
+        // Create the key-value entry, 
+        // linked to the usage record. 
+        _key_to_value.insert(std::make_pair(k, std::make_pair(v,it))); 
+        // No need to check return, 
+        // given previous assert. 
+    } 
+ 
+    // Purge the least-recently-used element in the cache 
+    void evict() 
+    {
+        // Assert method is never called when cache is empty 
+        assert(!_key_tracker.empty()); 
+ 
+        // Identify least recently used key 
+        auto it =_key_to_value.find(_key_tracker.front()); 
+        assert(it!=_key_to_value.end()); 
+ 
+        // Erase both elements to completely purge record 
+        _key_to_value.erase(it); 
+        _key_tracker.pop_front(); 
+    } 
+ 
+    // The function to be cached 
+    eval_func_type _fn; 
+ 
+    // Maximum number of key-value pairs to be retained 
+    //const size_t _capacity; 
+    size_t _capacity; 
+ 
+    // Key access history 
+    key_tracker_type _key_tracker; 
+ 
+    // Key-to-value lookup 
+    key_to_value_type _key_to_value; 
+};

extern/discregrid/discregrid/include/Discregrid/utility/serialize.hpp

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <streambuf>
+
+namespace Discregrid
+{
+
+namespace serialize
+{
+namespace details
+{
+template<class T>
+bool write(std::streambuf& buf, const T& val)
+{
+	static_assert( std::is_standard_layout<T>{}, "data is not standard layout" );
+	auto bytes = sizeof(T);
+	return buf.sputn(reinterpret_cast<const char*>(&val), bytes) == bytes;
+}
+template<class T>
+bool read(std::streambuf& buf, T& val)
+{
+	static_assert( std::is_standard_layout<T>{}, "data is not standard layout" );
+	auto bytes = sizeof(T);
+	return buf.sgetn(reinterpret_cast<char*>(&val), bytes) == bytes;
+}
+}
+
+template<class T>
+bool read(std::streambuf& buf, T& val)
+{
+	using details::read;
+	return read(buf, val);
+}
+template<class T>
+bool write(std::streambuf& buf, T const& val)
+{
+	using details::write;
+	return write(buf, val);
+}
+}
+}

extern/discregrid/discregrid/src/acceleration/bounding_sphere_hierarchy.cpp

@@ -0,0 +1,118 @@
+
+#include <acceleration/bounding_sphere_hierarchy.hpp>
+
+#include <iostream>
+#include <set>
+
+using namespace Eigen;
+
+namespace Discregrid
+{
+
+TriangleMeshBSH::TriangleMeshBSH(
+	std::vector<Vector3d> const& vertices,
+	std::vector<std::array<unsigned int, 3>> const& faces)
+	: super(faces.size()), m_faces(faces), m_vertices(vertices), 
+		m_tri_centers(faces.size())
+{
+	std::transform(m_faces.begin(), m_faces.end(), m_tri_centers.begin(),
+		[&](std::array<unsigned int, 3> const& f)
+		{
+			return 1.0 / 3.0 * (m_vertices[f[0]] + m_vertices[f[1]] +
+				m_vertices[f[2]]);
+		});
+}
+
+Vector3d const&
+TriangleMeshBSH::entityPosition(unsigned int i) const
+{
+	return m_tri_centers[i];
+}
+
+void
+TriangleMeshBSH::computeHull(unsigned int b, unsigned int n, BoundingSphere& hull) const
+{
+	auto vertices_subset = std::vector<Vector3d>(3 * n);
+	for (unsigned int i(0); i < n; ++i)
+	{
+		auto const& f = m_faces[m_lst[b + i]];
+		{
+			vertices_subset[3 * i + 0] = m_vertices[f[0]];
+			vertices_subset[3 * i + 1] = m_vertices[f[1]];
+			vertices_subset[3 * i + 2] = m_vertices[f[2]];
+		}
+	}
+
+	const BoundingSphere s(vertices_subset);
+
+	hull.x() = s.x();
+	hull.r() = s.r();
+}
+
+TriangleMeshBBH::TriangleMeshBBH(
+	std::vector<Vector3d> const& vertices,
+	std::vector<std::array<unsigned int, 3>> const& faces)
+	: super(faces.size()), m_faces(faces), m_vertices(vertices), 
+		m_tri_centers(faces.size())
+{
+	std::transform(m_faces.begin(), m_faces.end(), m_tri_centers.begin(),
+		[&](std::array<unsigned int, 3> const& f)
+		{
+			return 1.0 / 3.0 * (m_vertices[f[0]] + m_vertices[f[1]] +
+				m_vertices[f[2]]);
+		});
+}
+
+Vector3d const&
+TriangleMeshBBH::entityPosition(unsigned int i) const
+{
+	return m_tri_centers[i];
+}
+
+void
+TriangleMeshBBH::computeHull(unsigned int b, unsigned int n, AlignedBox3d& hull) const
+{
+	for (auto i = 0u; i < n; ++i)
+	{
+		auto const& f = m_faces[m_lst[b + i]];
+		for (auto v : f)
+		{
+			hull.extend(m_vertices[v]);
+		}
+	}
+}
+
+
+PointCloudBSH::PointCloudBSH()
+	: super(0)
+{
+}
+
+PointCloudBSH::PointCloudBSH(std::vector<Vector3d> const& vertices)
+	: super(vertices.size()), m_vertices(&vertices)
+{
+
+}
+
+Vector3d const&
+PointCloudBSH::entityPosition(unsigned int i) const
+{
+	return (*m_vertices)[i];
+}
+
+void
+PointCloudBSH::computeHull(unsigned int b, unsigned int n, BoundingSphere& hull) const
+{
+	auto vertices_subset = std::vector<Vector3d>(n);
+	for (unsigned int i = b; i < n + b; ++i)
+		vertices_subset[i - b] = (*m_vertices)[m_lst[i]];
+
+	const BoundingSphere s(vertices_subset);
+
+	hull.x() = s.x();
+	hull.r() = s.r();
+}
+
+
+
+}

extern/discregrid/discregrid/src/data/z_sort_table.hpp

@@ -0,0 +1,136 @@
+
+#ifndef Z_SORT_TABLE_H__
+#define Z_SORT_TABLE_H__
+
+#include <cstdint>
+#include <array>
+
+static const uint32_t morton256_x[256] =
+{
+	0x00000000,
+	0x00000001, 0x00000008, 0x00000009, 0x00000040, 0x00000041, 0x00000048, 0x00000049, 0x00000200,
+	0x00000201, 0x00000208, 0x00000209, 0x00000240, 0x00000241, 0x00000248, 0x00000249, 0x00001000,
+	0x00001001, 0x00001008, 0x00001009, 0x00001040, 0x00001041, 0x00001048, 0x00001049, 0x00001200,
+	0x00001201, 0x00001208, 0x00001209, 0x00001240, 0x00001241, 0x00001248, 0x00001249, 0x00008000,
+	0x00008001, 0x00008008, 0x00008009, 0x00008040, 0x00008041, 0x00008048, 0x00008049, 0x00008200,
+	0x00008201, 0x00008208, 0x00008209, 0x00008240, 0x00008241, 0x00008248, 0x00008249, 0x00009000,
+	0x00009001, 0x00009008, 0x00009009, 0x00009040, 0x00009041, 0x00009048, 0x00009049, 0x00009200,
+	0x00009201, 0x00009208, 0x00009209, 0x00009240, 0x00009241, 0x00009248, 0x00009249, 0x00040000,
+	0x00040001, 0x00040008, 0x00040009, 0x00040040, 0x00040041, 0x00040048, 0x00040049, 0x00040200,
+	0x00040201, 0x00040208, 0x00040209, 0x00040240, 0x00040241, 0x00040248, 0x00040249, 0x00041000,
+	0x00041001, 0x00041008, 0x00041009, 0x00041040, 0x00041041, 0x00041048, 0x00041049, 0x00041200,
+	0x00041201, 0x00041208, 0x00041209, 0x00041240, 0x00041241, 0x00041248, 0x00041249, 0x00048000,
+	0x00048001, 0x00048008, 0x00048009, 0x00048040, 0x00048041, 0x00048048, 0x00048049, 0x00048200,
+	0x00048201, 0x00048208, 0x00048209, 0x00048240, 0x00048241, 0x00048248, 0x00048249, 0x00049000,
+	0x00049001, 0x00049008, 0x00049009, 0x00049040, 0x00049041, 0x00049048, 0x00049049, 0x00049200,
+	0x00049201, 0x00049208, 0x00049209, 0x00049240, 0x00049241, 0x00049248, 0x00049249, 0x00200000,
+	0x00200001, 0x00200008, 0x00200009, 0x00200040, 0x00200041, 0x00200048, 0x00200049, 0x00200200,
+	0x00200201, 0x00200208, 0x00200209, 0x00200240, 0x00200241, 0x00200248, 0x00200249, 0x00201000,
+	0x00201001, 0x00201008, 0x00201009, 0x00201040, 0x00201041, 0x00201048, 0x00201049, 0x00201200,
+	0x00201201, 0x00201208, 0x00201209, 0x00201240, 0x00201241, 0x00201248, 0x00201249, 0x00208000,
+	0x00208001, 0x00208008, 0x00208009, 0x00208040, 0x00208041, 0x00208048, 0x00208049, 0x00208200,
+	0x00208201, 0x00208208, 0x00208209, 0x00208240, 0x00208241, 0x00208248, 0x00208249, 0x00209000,
+	0x00209001, 0x00209008, 0x00209009, 0x00209040, 0x00209041, 0x00209048, 0x00209049, 0x00209200,
+	0x00209201, 0x00209208, 0x00209209, 0x00209240, 0x00209241, 0x00209248, 0x00209249, 0x00240000,
+	0x00240001, 0x00240008, 0x00240009, 0x00240040, 0x00240041, 0x00240048, 0x00240049, 0x00240200,
+	0x00240201, 0x00240208, 0x00240209, 0x00240240, 0x00240241, 0x00240248, 0x00240249, 0x00241000,
+	0x00241001, 0x00241008, 0x00241009, 0x00241040, 0x00241041, 0x00241048, 0x00241049, 0x00241200,
+	0x00241201, 0x00241208, 0x00241209, 0x00241240, 0x00241241, 0x00241248, 0x00241249, 0x00248000,
+	0x00248001, 0x00248008, 0x00248009, 0x00248040, 0x00248041, 0x00248048, 0x00248049, 0x00248200,
+	0x00248201, 0x00248208, 0x00248209, 0x00248240, 0x00248241, 0x00248248, 0x00248249, 0x00249000,
+	0x00249001, 0x00249008, 0x00249009, 0x00249040, 0x00249041, 0x00249048, 0x00249049, 0x00249200,
+	0x00249201, 0x00249208, 0x00249209, 0x00249240, 0x00249241, 0x00249248, 0x00249249
+};
+
+// pre-shifted table for Y coordinates (1 bit to the left)
+static const uint32_t morton256_y[256] = {
+	0x00000000,
+	0x00000002, 0x00000010, 0x00000012, 0x00000080, 0x00000082, 0x00000090, 0x00000092, 0x00000400,
+	0x00000402, 0x00000410, 0x00000412, 0x00000480, 0x00000482, 0x00000490, 0x00000492, 0x00002000,
+	0x00002002, 0x00002010, 0x00002012, 0x00002080, 0x00002082, 0x00002090, 0x00002092, 0x00002400,
+	0x00002402, 0x00002410, 0x00002412, 0x00002480, 0x00002482, 0x00002490, 0x00002492, 0x00010000,
+	0x00010002, 0x00010010, 0x00010012, 0x00010080, 0x00010082, 0x00010090, 0x00010092, 0x00010400,
+	0x00010402, 0x00010410, 0x00010412, 0x00010480, 0x00010482, 0x00010490, 0x00010492, 0x00012000,
+	0x00012002, 0x00012010, 0x00012012, 0x00012080, 0x00012082, 0x00012090, 0x00012092, 0x00012400,
+	0x00012402, 0x00012410, 0x00012412, 0x00012480, 0x00012482, 0x00012490, 0x00012492, 0x00080000,
+	0x00080002, 0x00080010, 0x00080012, 0x00080080, 0x00080082, 0x00080090, 0x00080092, 0x00080400,
+	0x00080402, 0x00080410, 0x00080412, 0x00080480, 0x00080482, 0x00080490, 0x00080492, 0x00082000,
+	0x00082002, 0x00082010, 0x00082012, 0x00082080, 0x00082082, 0x00082090, 0x00082092, 0x00082400,
+	0x00082402, 0x00082410, 0x00082412, 0x00082480, 0x00082482, 0x00082490, 0x00082492, 0x00090000,
+	0x00090002, 0x00090010, 0x00090012, 0x00090080, 0x00090082, 0x00090090, 0x00090092, 0x00090400,
+	0x00090402, 0x00090410, 0x00090412, 0x00090480, 0x00090482, 0x00090490, 0x00090492, 0x00092000,
+	0x00092002, 0x00092010, 0x00092012, 0x00092080, 0x00092082, 0x00092090, 0x00092092, 0x00092400,
+	0x00092402, 0x00092410, 0x00092412, 0x00092480, 0x00092482, 0x00092490, 0x00092492, 0x00400000,
+	0x00400002, 0x00400010, 0x00400012, 0x00400080, 0x00400082, 0x00400090, 0x00400092, 0x00400400,
+	0x00400402, 0x00400410, 0x00400412, 0x00400480, 0x00400482, 0x00400490, 0x00400492, 0x00402000,
+	0x00402002, 0x00402010, 0x00402012, 0x00402080, 0x00402082, 0x00402090, 0x00402092, 0x00402400,
+	0x00402402, 0x00402410, 0x00402412, 0x00402480, 0x00402482, 0x00402490, 0x00402492, 0x00410000,
+	0x00410002, 0x00410010, 0x00410012, 0x00410080, 0x00410082, 0x00410090, 0x00410092, 0x00410400,
+	0x00410402, 0x00410410, 0x00410412, 0x00410480, 0x00410482, 0x00410490, 0x00410492, 0x00412000,
+	0x00412002, 0x00412010, 0x00412012, 0x00412080, 0x00412082, 0x00412090, 0x00412092, 0x00412400,
+	0x00412402, 0x00412410, 0x00412412, 0x00412480, 0x00412482, 0x00412490, 0x00412492, 0x00480000,
+	0x00480002, 0x00480010, 0x00480012, 0x00480080, 0x00480082, 0x00480090, 0x00480092, 0x00480400,
+	0x00480402, 0x00480410, 0x00480412, 0x00480480, 0x00480482, 0x00480490, 0x00480492, 0x00482000,
+	0x00482002, 0x00482010, 0x00482012, 0x00482080, 0x00482082, 0x00482090, 0x00482092, 0x00482400,
+	0x00482402, 0x00482410, 0x00482412, 0x00482480, 0x00482482, 0x00482490, 0x00482492, 0x00490000,
+	0x00490002, 0x00490010, 0x00490012, 0x00490080, 0x00490082, 0x00490090, 0x00490092, 0x00490400,
+	0x00490402, 0x00490410, 0x00490412, 0x00490480, 0x00490482, 0x00490490, 0x00490492, 0x00492000,
+	0x00492002, 0x00492010, 0x00492012, 0x00492080, 0x00492082, 0x00492090, 0x00492092, 0x00492400,
+	0x00492402, 0x00492410, 0x00492412, 0x00492480, 0x00492482, 0x00492490, 0x00492492
+};
+
+// Pre-shifted table for z (2 bits to the left)
+static const uint32_t morton256_z[256] = {
+	0x00000000,
+	0x00000004, 0x00000020, 0x00000024, 0x00000100, 0x00000104, 0x00000120, 0x00000124, 0x00000800,
+	0x00000804, 0x00000820, 0x00000824, 0x00000900, 0x00000904, 0x00000920, 0x00000924, 0x00004000,
+	0x00004004, 0x00004020, 0x00004024, 0x00004100, 0x00004104, 0x00004120, 0x00004124, 0x00004800,
+	0x00004804, 0x00004820, 0x00004824, 0x00004900, 0x00004904, 0x00004920, 0x00004924, 0x00020000,
+	0x00020004, 0x00020020, 0x00020024, 0x00020100, 0x00020104, 0x00020120, 0x00020124, 0x00020800,
+	0x00020804, 0x00020820, 0x00020824, 0x00020900, 0x00020904, 0x00020920, 0x00020924, 0x00024000,
+	0x00024004, 0x00024020, 0x00024024, 0x00024100, 0x00024104, 0x00024120, 0x00024124, 0x00024800,
+	0x00024804, 0x00024820, 0x00024824, 0x00024900, 0x00024904, 0x00024920, 0x00024924, 0x00100000,
+	0x00100004, 0x00100020, 0x00100024, 0x00100100, 0x00100104, 0x00100120, 0x00100124, 0x00100800,
+	0x00100804, 0x00100820, 0x00100824, 0x00100900, 0x00100904, 0x00100920, 0x00100924, 0x00104000,
+	0x00104004, 0x00104020, 0x00104024, 0x00104100, 0x00104104, 0x00104120, 0x00104124, 0x00104800,
+	0x00104804, 0x00104820, 0x00104824, 0x00104900, 0x00104904, 0x00104920, 0x00104924, 0x00120000,
+	0x00120004, 0x00120020, 0x00120024, 0x00120100, 0x00120104, 0x00120120, 0x00120124, 0x00120800,
+	0x00120804, 0x00120820, 0x00120824, 0x00120900, 0x00120904, 0x00120920, 0x00120924, 0x00124000,
+	0x00124004, 0x00124020, 0x00124024, 0x00124100, 0x00124104, 0x00124120, 0x00124124, 0x00124800,
+	0x00124804, 0x00124820, 0x00124824, 0x00124900, 0x00124904, 0x00124920, 0x00124924, 0x00800000,
+	0x00800004, 0x00800020, 0x00800024, 0x00800100, 0x00800104, 0x00800120, 0x00800124, 0x00800800,
+	0x00800804, 0x00800820, 0x00800824, 0x00800900, 0x00800904, 0x00800920, 0x00800924, 0x00804000,
+	0x00804004, 0x00804020, 0x00804024, 0x00804100, 0x00804104, 0x00804120, 0x00804124, 0x00804800,
+	0x00804804, 0x00804820, 0x00804824, 0x00804900, 0x00804904, 0x00804920, 0x00804924, 0x00820000,
+	0x00820004, 0x00820020, 0x00820024, 0x00820100, 0x00820104, 0x00820120, 0x00820124, 0x00820800,
+	0x00820804, 0x00820820, 0x00820824, 0x00820900, 0x00820904, 0x00820920, 0x00820924, 0x00824000,
+	0x00824004, 0x00824020, 0x00824024, 0x00824100, 0x00824104, 0x00824120, 0x00824124, 0x00824800,
+	0x00824804, 0x00824820, 0x00824824, 0x00824900, 0x00824904, 0x00824920, 0x00824924, 0x00900000,
+	0x00900004, 0x00900020, 0x00900024, 0x00900100, 0x00900104, 0x00900120, 0x00900124, 0x00900800,
+	0x00900804, 0x00900820, 0x00900824, 0x00900900, 0x00900904, 0x00900920, 0x00900924, 0x00904000,
+	0x00904004, 0x00904020, 0x00904024, 0x00904100, 0x00904104, 0x00904120, 0x00904124, 0x00904800,
+	0x00904804, 0x00904820, 0x00904824, 0x00904900, 0x00904904, 0x00904920, 0x00904924, 0x00920000,
+	0x00920004, 0x00920020, 0x00920024, 0x00920100, 0x00920104, 0x00920120, 0x00920124, 0x00920800,
+	0x00920804, 0x00920820, 0x00920824, 0x00920900, 0x00920904, 0x00920920, 0x00920924, 0x00924000,
+	0x00924004, 0x00924020, 0x00924024, 0x00924100, 0x00924104, 0x00924120, 0x00924124, 0x00924800,
+	0x00924804, 0x00924820, 0x00924824, 0x00924900, 0x00924904, 0x00924920, 0x00924924
+};
+
+inline
+uint64_t morton_lut(std::array<unsigned int, 3> const& x)
+{
+	uint64_t answer = 0;
+	answer = morton256_z[(x[2] >> 16) & 0xFF] | // we start by shifting the third byte, since we only look at the first 21 bits
+		morton256_y[(x[1] >> 16) & 0xFF] |
+		morton256_x[(x[0] >> 16) & 0xFF];
+	answer = answer << 48 | morton256_z[(x[2] >> 8) & 0xFF] | // shifting second byte
+		morton256_y[(x[1] >> 8) & 0xFF] |
+		morton256_x[(x[0] >> 8) & 0xFF];
+	answer = answer << 24 |
+		morton256_z[(x[2]) & 0xFF] | // first byte
+		morton256_y[(x[1]) & 0xFF] |
+		morton256_x[(x[0]) & 0xFF];
+	return answer;
+}
+
+#endif // Z_SORT_TABLE_H__

extern/discregrid/discregrid/src/discrete_grid.cpp

@@ -0,0 +1,41 @@
+#include <discrete_grid.hpp>
+
+using namespace Eigen;
+
+namespace Discregrid
+{
+
+
+DiscreteGrid::MultiIndex
+DiscreteGrid::singleToMultiIndex(unsigned int l) const
+{
+	auto n01 = m_resolution[0] * m_resolution[1];
+	auto k = l / n01;
+	auto temp = l % n01;
+	auto j = temp / m_resolution[0];
+	auto i = temp % m_resolution[0];
+	return {{i, j, k}};
+}
+
+unsigned int
+DiscreteGrid::multiToSingleIndex(MultiIndex const & ijk) const
+{
+	return m_resolution[1] * m_resolution[0] * ijk[2] + m_resolution[0] * ijk[1] + ijk[0];
+}
+
+AlignedBox3d
+DiscreteGrid::subdomain(MultiIndex const& ijk) const
+{
+	auto origin = m_domain.min() + Map<Matrix<unsigned int, 3, 1> const>(
+		ijk.data()).cast<double>().cwiseProduct(m_cell_size);
+	return { origin, origin + m_cell_size};
+}
+
+AlignedBox3d
+DiscreteGrid::subdomain(unsigned int l) const
+{
+	return subdomain(singleToMultiIndex(l));
+}
+
+
+}

extern/discregrid/discregrid/src/geometry/mesh_distance.cpp

@@ -0,0 +1,326 @@
+
+#include <geometry/mesh_distance.hpp>
+#include <mesh/triangle_mesh.hpp>
+#include "point_triangle_distance.hpp"
+
+#include <limits>
+#include <functional>
+#include <thread>
+//#include <omp.h>
+
+using namespace Eigen;
+
+namespace Discregrid
+{
+
+MeshDistance::MeshDistance(TriangleMesh const& mesh, bool precompute_normals)
+	: m_bsh(mesh.vertex_data(), mesh.face_data()), m_mesh(mesh), thread_map(nullptr)
+	, m_precomputed_normals(precompute_normals)
+{
+	auto max_threads = std::max(1,(int)std::thread::hardware_concurrency());
+	m_queues.resize(max_threads);
+	m_nearest_face.resize(max_threads);
+	m_cache.resize(max_threads, FunctionValueCache([&](Vector3d const& xi){ return signedDistance(xi);}, 10000u));
+	m_ucache.resize(max_threads, FunctionValueCache([&](Vector3d const& xi){ return distance(xi);}, 10000u));
+
+	m_bsh.construct();
+
+	if (m_precomputed_normals)
+	{
+		m_face_normals.resize(m_mesh.nFaces());
+		m_vertex_normals.resize(mesh.nVertices(), Vector3d::Zero());
+		std::transform(m_mesh.faces().begin(), m_mesh.faces().end(),
+			m_face_normals.begin(),
+			[&](std::array<unsigned int, 3> const& face)
+			{
+				auto const& x0 = m_mesh.vertex(face[0]);
+				auto const& x1 = m_mesh.vertex(face[1]);
+				auto const& x2 = m_mesh.vertex(face[2]);
+
+				auto n = (x1 - x0).cross(x2 - x0).normalized();
+
+				auto e1 = (x1 - x0).normalized();
+				auto e2 = (x2 - x1).normalized();
+				auto e3 = (x0 - x2).normalized();
+
+				auto alpha = Vector3d{
+					std::acos(e1.dot(-e3)), 
+					std::acos(e2.dot(-e1)),
+					std::acos(e3.dot(-e2)) };
+				m_vertex_normals[face[0]] += alpha[0] * n;
+				m_vertex_normals[face[1]] += alpha[1] * n;
+				m_vertex_normals[face[2]] += alpha[2] * n;
+
+				return n;
+			}
+		);
+	}
+}
+
+int MeshDistance::get_thread_num() const {
+	//return omp_get_thread_num();
+	if (thread_map == nullptr) {
+		throw std::runtime_error("**Discregride::MeshDistance Error: No thread map set!\n");
+	}
+	int nt = thread_map->size();
+	if (nt == 0) {
+		throw std::runtime_error("**Discregride::MeshDistance Error: Thread map empty!\n");
+	}
+	for (int i=0; i<nt; ++i)
+	{
+		if (thread_map->at(i)==std::this_thread::get_id()) {
+			return i;
+		}
+	}
+	throw std::runtime_error("**Discregride::MeshDistance Error: Thread index not found!\n");
+	return 0;
+}
+
+// Thread-safe.
+double
+MeshDistance::distance(Vector3d const& x, Vector3d* nearest_point, 
+	unsigned int* nearest_face, NearestEntity* ne) const
+{
+	using namespace std::placeholders;
+
+	int thread_num = get_thread_num();
+
+	auto dist_candidate = std::numeric_limits<double>::max();
+	auto f = m_nearest_face[thread_num];
+	if (f < m_mesh.nFaces())
+	{
+		auto t = std::array<Vector3d const*, 3>{
+			&m_mesh.vertex(m_mesh.faceVertex(f, 0)),
+			&m_mesh.vertex(m_mesh.faceVertex(f, 1)),
+			&m_mesh.vertex(m_mesh.faceVertex(f, 2))
+		};
+		dist_candidate = std::sqrt(point_triangle_sqdistance(x, t));
+	}
+
+	auto pred = [&](unsigned int node_index, unsigned int)
+	{
+		return predicate(node_index, m_bsh, x, dist_candidate);
+	};
+
+	auto cb = [&](unsigned int node_index, unsigned int)
+	{
+		return callback(node_index, m_bsh, x, dist_candidate);
+	};
+
+	auto pless = [&](std::array<int, 2> const& c)
+	{
+		//return true;
+		auto const& hull0 = m_bsh.hull(c[0]);
+		auto const& hull1 = m_bsh.hull(c[1]);
+		auto d0_2 = (x - hull0.x()).norm() - hull0.r();
+		auto d1_2 = (x - hull1.x()).norm() - hull1.r();
+		return d0_2 < d1_2;
+	};
+
+	while (!m_queues[thread_num].empty())
+		m_queues[thread_num].pop();
+	m_bsh.traverseDepthFirst(pred, cb, pless);
+
+	f = m_nearest_face[thread_num];
+	if (nearest_point)
+	{
+		auto t = std::array<Vector3d const*, 3>{
+			&m_mesh.vertex(m_mesh.faceVertex(f, 0)),
+			&m_mesh.vertex(m_mesh.faceVertex(f, 1)),
+			&m_mesh.vertex(m_mesh.faceVertex(f, 2))
+		};
+		auto np = Vector3d{};
+		auto ne_ = NearestEntity{};
+		auto dist2_ = point_triangle_sqdistance(x, t, &np, &ne_);
+		dist_candidate = std::sqrt(dist2_);
+		if (ne)
+			*ne = ne_;
+		if (nearest_point)
+			*nearest_point = np;
+	}
+	if (nearest_face)
+		*nearest_face = f;
+	return dist_candidate;
+}
+
+bool
+MeshDistance::predicate(unsigned int node_index, 
+	TriangleMeshBSH const& bsh,
+	Vector3d const& x, 
+	double& dist_candidate) const
+{
+	// If the furthest point on the current candidate hull is closer than the closest point on the next hull then we can skip it
+	auto const& hull = bsh.hull(node_index);
+	auto const& hull_radius = hull.r();
+	auto const& hull_center = hull.x();
+
+	const auto dist_sq_to_center = (x - hull_center).squaredNorm();
+
+	if (dist_candidate > hull_radius) {
+		const auto l = dist_candidate - hull_radius;
+		if (l * l > dist_sq_to_center)
+			dist_candidate = std::sqrt(dist_sq_to_center) + hull_radius;
+	}
+
+	const auto d = dist_candidate + hull_radius;
+	return dist_sq_to_center <= d * d;
+}
+
+void
+MeshDistance::callback(unsigned int node_index, 
+	TriangleMeshBSH const& bsh,
+	Vector3d const& x, 
+	double& dist_candidate) const
+{
+	auto const& node = m_bsh.node(node_index);
+	auto const& hull = m_bsh.hull(node_index);
+
+	if (!node.isLeaf())
+		return;
+
+	auto r = hull.r();
+
+	auto temp = (x - hull.x()).eval();
+	auto d_center2 = temp[0] * temp[0] + temp[1] * temp[1] + temp[2] * temp[2];
+
+	auto temp_ = dist_candidate + r;
+	if (d_center2 > temp_ * temp_)
+		return;
+
+	auto dist_candidate_2 = dist_candidate * dist_candidate;
+	auto changed = false;
+	for (auto i = node.begin; i < node.begin + node.n; ++i)
+	{
+		auto f = m_bsh.entity(i);
+		auto t = std::array<Vector3d const*, 3>{
+			&m_mesh.vertex(m_mesh.faceVertex(f, 0)),
+			&m_mesh.vertex(m_mesh.faceVertex(f, 1)),
+			&m_mesh.vertex(m_mesh.faceVertex(f, 2))
+		};
+		auto dist2_ = point_triangle_sqdistance(x, t);
+		if (dist_candidate_2 > dist2_)
+		{
+			dist_candidate_2 = dist2_;
+			changed = true;
+			m_nearest_face[get_thread_num()] = f;
+		}
+	}
+	if (changed)
+	{
+		dist_candidate = std::sqrt(dist_candidate_2);
+	}
+}
+
+double
+MeshDistance::signedDistance(Vector3d const& x) const
+{
+	unsigned int nf;
+	auto ne = NearestEntity{};
+	auto np = Vector3d{};
+	auto dist = distance(x, &np, &nf, &ne);
+	
+	auto n = Vector3d{};
+	switch (ne)
+	{
+	case NearestEntity::VN0:
+		n = vertex_normal(m_mesh.faceVertex(nf, 0));
+		break;
+	case NearestEntity::VN1:
+		n = vertex_normal(m_mesh.faceVertex(nf, 1));
+		break;
+	case NearestEntity::VN2:
+		n = vertex_normal(m_mesh.faceVertex(nf, 2));
+		break;
+	case NearestEntity::EN0:
+		n = edge_normal({nf, 0});
+		break;
+	case NearestEntity::EN1:
+		n = edge_normal({nf, 1});
+		break;
+	case NearestEntity::EN2:
+		n = edge_normal({nf, 2});
+		break;
+	case NearestEntity::FN:
+		n = face_normal(nf);
+		break;
+	default:
+		n.setZero();
+		break;
+	}
+
+	if ((x - np).dot(n) < 0.0)
+		dist *= -1.0;
+
+	return dist;
+}
+
+double
+MeshDistance::signedDistanceCached(Vector3d const & x) const
+{
+	return m_cache[get_thread_num()](x);
+}
+
+double
+MeshDistance::unsignedDistance(Vector3d const & x) const
+{
+	return distance(x);
+}
+
+double
+MeshDistance::unsignedDistanceCached(Vector3d const & x) const
+{
+	return m_ucache[get_thread_num()](x);
+}
+
+Vector3d
+MeshDistance::face_normal(unsigned int f) const
+{
+	if (m_precomputed_normals)
+		return m_face_normals[f];
+
+	auto const& x0 = m_mesh.vertex(m_mesh.faceVertex(f, 0));
+	auto const& x1 = m_mesh.vertex(m_mesh.faceVertex(f, 1));
+	auto const& x2 = m_mesh.vertex(m_mesh.faceVertex(f, 2));
+
+	return (x1 - x0).cross(x2 - x0).normalized();
+}
+
+Vector3d
+MeshDistance::edge_normal(Halfedge const& h) const
+{
+	auto o = m_mesh.opposite(h);
+
+	if (m_precomputed_normals)
+	{
+		if (o.isBoundary()) return m_face_normals[h.face()];
+		return m_face_normals[h.face()] + m_face_normals[o.face()];
+	}
+
+	if (o.isBoundary()) return face_normal(h.face());
+	return face_normal(h.face()) + face_normal(o.face());
+}
+
+Vector3d
+MeshDistance::vertex_normal(unsigned int v) const
+{
+	if (m_precomputed_normals)
+		return m_vertex_normals[v];
+
+	auto const& x0 = m_mesh.vertex(v);
+	auto n = Vector3d{}; n.setZero();
+	for (auto h : m_mesh.incident_faces(v))
+	{
+		assert(m_mesh.source(h) == v);
+		auto ve0 = m_mesh.target(h);
+		auto e0 = (m_mesh.vertex(ve0) - x0).eval();
+		e0.normalize();
+		auto ve1 = m_mesh.target(h.next());
+		auto e1 = (m_mesh.vertex(ve1) - x0).eval();
+		e1.normalize();
+		auto alpha = std::acos((e0.dot(e1)));
+		n += alpha * e0.cross(e1);
+	}
+	return n;
+}
+
+}

extern/discregrid/discregrid/src/geometry/point_triangle_distance.cpp

@@ -0,0 +1,289 @@
+
+#include "point_triangle_distance.hpp"
+
+#include <iostream>
+
+using namespace Eigen;
+
+namespace Discregrid
+{
+
+double
+point_triangle_sqdistance(Vector3d const& point, 
+    std::array<Vector3d const*, 3> const& triangle, 
+    Vector3d* nearest_point,
+    NearestEntity* ne)
+{
+    Vector3d diff = *triangle[0] - point;
+    Vector3d edge0 = *triangle[1] - *triangle[0];
+    Vector3d edge1 = *triangle[2] - *triangle[0];
+    double a00 = edge0.dot(edge0);
+    double a01 = edge0.dot(edge1);
+    double a11 = edge1.dot(edge1);
+    double b0 = diff.dot(edge0);
+    double b1 = diff.dot(edge1);
+    double c = diff.dot(diff);
+    double det = std::abs(a00*a11 - a01*a01);
+    double s = a01*b1 - a11*b0;
+    double t = a01*b0 - a00*b1;
+
+    double d2 = -1.0;
+
+    if (s + t <= det)
+    {
+        if (s < 0)
+        {
+            if (t < 0)  // region 4
+            {
+                if (b0 < 0)
+                {
+                    t = 0;
+                    if (-b0 >= a00)
+                    {   // VN1
+                        if (ne) *ne = NearestEntity::VN1;
+                        s = 1;
+                        d2 = a00 + (2)*b0 + c;
+                    }
+                    else
+                    {
+                        // EN0
+                        if (ne) *ne = NearestEntity::EN0;
+                        s = -b0 / a00;
+                        d2 = b0*s + c;
+                    }
+                }
+                else
+                {
+                    s = 0;
+                    if (b1 >= 0)
+                    {   // VN0
+                        if (ne) *ne = NearestEntity::VN0;
+                        t = 0;
+                        d2 = c;
+                    }
+                    else if (-b1 >= a11)
+                    {
+                        // VN2
+                        if (ne) *ne = NearestEntity::VN2;
+                        t = 1;
+                        d2 = a11 + (2)*b1 + c;
+                    }
+                    else
+                    {
+                        // EN2
+                        if (ne) *ne = NearestEntity::EN2;
+                        t = -b1 / a11;
+                        d2 = b1*t + c;
+                    }
+                }
+            }
+            else  // region 3
+            {
+                s = 0;
+                if (b1 >= 0)
+                {   // VN0
+                    if (ne) *ne = NearestEntity::VN0;
+                    t = 0;
+                    d2 = c;
+                }
+                else if (-b1 >= a11)
+                {   // VN2
+                    if (ne) *ne = NearestEntity::VN2;
+                    t = 1;
+                    d2 = a11 + (2)*b1 + c;
+                }
+                else
+                {   // EN2
+                    if (ne) *ne = NearestEntity::EN2;
+                    t = -b1 / a11;
+                    d2 = b1*t + c;
+                }
+            }
+        }
+        else if (t < 0)  // region 5
+        {
+            t = 0;
+            if (b0 >= 0)
+            {   // VN0
+                if (ne) *ne = NearestEntity::VN0;
+                s = 0;
+                d2 = c;
+            }
+            else if (-b0 >= a00)
+            {   // VN1
+                if (ne) *ne = NearestEntity::VN1;
+                s = 1;
+                d2 = a00 + (2)*b0 + c;
+            }
+            else
+            {   // EN0
+                if (ne) *ne = NearestEntity::EN0;
+                s = -b0 / a00;
+                d2 = b0*s + c;
+            }
+        }
+        else  // region 0 
+        {   // FN
+            if (ne) *ne = NearestEntity::FN;
+            // minimum at interior point
+            double invDet = (1) / det;
+            s *= invDet;
+            t *= invDet;
+            d2 = s*(a00*s + a01*t + (2)*b0) +
+                t*(a01*s + a11*t + (2)*b1) + c;
+        }
+    }
+    else
+    {
+        double tmp0, tmp1, numer, denom;
+
+        if (s < 0)  // region 2
+        {
+            tmp0 = a01 + b0;
+            tmp1 = a11 + b1;
+            if (tmp1 > tmp0)
+            {
+                numer = tmp1 - tmp0;
+                denom = a00 - (2)*a01 + a11;
+                if (numer >= denom)
+                {   // VN1
+                    if (ne) *ne = NearestEntity::VN1;
+                    s = 1;
+                    t = 0;
+                    d2 = a00 + (2)*b0 + c;
+                }
+                else
+                {
+                    // EN1
+                    if (ne) *ne = NearestEntity::EN1;
+                    s = numer / denom;
+                    t = 1 - s;
+                    d2 = s*(a00*s + a01*t + (2)*b0) +
+                        t*(a01*s + a11*t + (2)*b1) + c;
+                }
+            }
+            else
+            {
+                s = 0;
+                if (tmp1 <= 0)
+                {   // VN2
+                    if (ne) *ne = NearestEntity::VN2;
+                    t = 1;
+                    d2 = a11 + (2)*b1 + c;
+                }
+                else if (b1 >= 0)
+                {   // VN0
+                    if (ne) *ne = NearestEntity::VN0;
+                    t = 0;
+                    d2 = c;
+                }
+                else
+                {
+                    // EN2
+                    if (ne) *ne = NearestEntity::EN2;
+                    t = -b1 / a11;
+                    d2 = b1*t + c;
+                }
+            }
+        }
+        else if (t < 0)  // region 6
+        {
+            tmp0 = a01 + b1;
+            tmp1 = a00 + b0;
+            if (tmp1 > tmp0)
+            {
+                numer = tmp1 - tmp0;
+                denom = a00 - (2)*a01 + a11;
+                if (numer >= denom)
+                {   // VN2
+                    if (ne) *ne = NearestEntity::VN2;
+                    t = 1;
+                    s = 0;
+                    d2 = a11 + (2)*b1 + c;
+                }
+                else
+                {
+                    // EN1
+                    if (ne) *ne = NearestEntity::EN1;
+                    t = numer / denom;
+                    s = 1 - t;
+                    d2 = s*(a00*s + a01*t + (2)*b0) +
+                        t*(a01*s + a11*t + (2)*b1) + c;
+                }
+            }
+            else
+            {
+                t = 0;
+                if (tmp1 <= 0)
+                {   // VN1
+                    if (ne) *ne = NearestEntity::VN1;
+                    s = 1;
+                    d2 = a00 + (2)*b0 + c;
+                }
+                else if (b0 >= 0)
+                {   // VN0
+                    if (ne) *ne = NearestEntity::VN0;
+                    s = 0;
+                    d2 = c;
+                }
+                else
+                {
+                    // EN0
+                    if (ne) *ne = NearestEntity::EN0;
+                    s = -b0 / a00;
+                    d2 = b0*s + c;
+                }
+            }
+        }
+        else  // region 1
+        {
+            numer = a11 + b1 - a01 - b0;
+            if (numer <= 0)
+            {   // VN2
+                if (ne) *ne = NearestEntity::VN2;
+                s = 0;
+                t = 1;
+                d2 = a11 + (2)*b1 + c;
+            }
+            else
+            {
+                denom = a00 - (2)*a01 + a11;
+                if (numer >= denom)
+                {   // VN1
+                    if (ne) *ne = NearestEntity::VN1;
+                    s = 1;
+                    t = 0;
+                    d2 = a00 + (2)*b0 + c;
+                }
+                else
+                {   // EN1
+                    if (ne) *ne = NearestEntity::EN1;
+                    s = numer / denom;
+                    t = 1 - s;
+                    d2 = s*(a00*s + a01*t + (2)*b0) +
+                        t*(a01*s + a11*t + (2)*b1) + c;
+                }
+            }
+        }
+    }
+
+    // Account for numerical round-off error.
+    if (d2 < 0)
+    {
+        d2 = 0;
+    }
+
+    if (nearest_point)
+        *nearest_point = *triangle[0] + s*edge0 + t*edge1;
+
+    return d2;
+
+    //result.distance = sqrt(d2);
+    //result.triangleClosestPoint = triangle.v[0] + s*edge0 + t*edge1;
+    //result.triangleParameter[1] = s;
+    //result.triangleParameter[2] = t;
+    //result.triangleParameter[0] = 1 - s - t;
+    //return result;
+}
+
+}

extern/discregrid/discregrid/src/geometry/point_triangle_distance.hpp

@@ -0,0 +1,21 @@
+
+#pragma once
+
+#include <array>
+#include <Eigen/Core>
+
+namespace Discregrid
+{
+
+enum class NearestEntity
+{
+	VN0, VN1, VN2, EN0, EN1, EN2, FN
+};
+
+double point_triangle_sqdistance(Eigen::Vector3d const& point, 
+	std::array<Eigen::Vector3d const*, 3> const& triangle,
+	Eigen::Vector3d* nearest_point = nullptr,
+	NearestEntity* ne = nullptr);
+
+}
+

extern/discregrid/discregrid/src/mesh/entity_containers.cpp

@@ -0,0 +1,38 @@
+#include <mesh/entity_containers.hpp>
+#include <mesh/triangle_mesh.hpp>
+
+namespace Discregrid
+{
+
+
+FaceIterator
+FaceContainer::end() const
+{
+    return FaceIterator(static_cast<unsigned int>(m_mesh->nFaces())
+        , m_mesh);
+}
+
+FaceConstIterator
+FaceConstContainer::end() const
+{
+    return FaceConstIterator(static_cast<unsigned int>(m_mesh->nFaces())
+        , m_mesh);
+}
+
+VertexIterator
+VertexContainer::end() const
+{
+    return VertexIterator(static_cast<unsigned int>(m_mesh->nVertices()),
+        m_mesh);
+}
+
+
+VertexConstIterator
+VertexConstContainer::end() const
+{
+    return VertexConstIterator(static_cast<unsigned int>(m_mesh->nVertices()),
+        m_mesh);
+}
+
+}
+

extern/discregrid/discregrid/src/mesh/entity_iterators.cpp

@@ -0,0 +1,81 @@
+
+#include <mesh/entity_iterators.hpp>
+#include <mesh/triangle_mesh.hpp>
+
+namespace Discregrid
+{
+
+
+unsigned int
+FaceIterator::vertex(unsigned int i) const
+{
+    return m_mesh->faceVertex(m_index, i);
+}
+
+FaceIterator::reference
+FaceIterator::operator*()
+{
+    return m_mesh->face(m_index);
+}
+
+FaceConstIterator::reference
+FaceConstIterator::operator*()
+{
+    return m_mesh->face(m_index);
+}
+
+unsigned int&
+FaceIterator::vertex(unsigned int i)
+{
+    return m_mesh->faceVertex(m_index, i);
+}
+
+VertexIterator::reference
+VertexIterator::operator*()
+{
+    return m_mesh->vertex(m_index);
+}
+
+
+VertexConstIterator::reference
+VertexConstIterator::operator*()
+{
+    return m_mesh->vertex(m_index);
+}
+
+
+
+unsigned int
+VertexIterator::index() const
+{
+    return m_index;
+}
+
+IncidentFaceIterator::_Mytype&
+IncidentFaceIterator::operator++()
+{
+    Halfedge o = m_mesh->opposite(m_h);
+    if (o.isBoundary())
+    {
+        m_h = Halfedge();
+        return *this;
+    }
+    m_h = o.next();
+    if (m_h == m_begin)
+    {
+        m_h = Halfedge();
+    }
+    return *this;
+}
+
+
+IncidentFaceIterator::IncidentFaceIterator(unsigned int v, TriangleMesh const* mesh) 
+    : m_mesh(mesh), m_h(mesh->incident_halfedge(v))
+, m_begin(mesh->incident_halfedge(v))
+{
+    if (m_h.isBoundary())
+        m_h = mesh->opposite(m_h).next();
+}
+
+}
+

extern/discregrid/discregrid/src/mesh/triangle_mesh.cpp

@@ -0,0 +1,225 @@
+
+#include <mesh/triangle_mesh.hpp>
+
+#include <unordered_set>
+#include <cassert>
+#include <fstream>
+#include <iostream>
+
+using namespace Eigen;
+
+namespace
+{
+template <class T>
+inline void hash_combine(std::size_t& seed, const T& v)
+{
+	std::hash<T> hasher;
+	seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2);
+}
+}
+
+namespace Discregrid
+{
+
+struct HalfedgeHasher
+{
+	HalfedgeHasher(std::vector<std::array<unsigned int, 3>> const& faces_)
+		: faces(&faces_){}
+
+	std::size_t operator()(Halfedge const& he) const
+	{
+		unsigned int f = he.face();
+		unsigned int e = he.edge();
+		std::array<unsigned int, 2> v = { (*faces)[f][e], (*faces)[f][(e + 1) % 3] };
+		if (v[0] > v[1])
+			std::swap(v[0], v[1]);
+
+		std::size_t seed(0);
+		hash_combine(seed, v[0]);
+		hash_combine(seed, v[1]);
+		return seed;
+	}
+
+	std::vector<std::array<unsigned int, 3>> const* faces;
+};
+
+struct HalfedgeEqualTo
+{
+	HalfedgeEqualTo(std::vector<std::array<unsigned int, 3>> const& faces_)
+		: faces(&faces_){}
+
+	bool operator()(Halfedge const& a, Halfedge const& b) const
+	{
+		unsigned int fa = a.face();
+		unsigned int ea = a.edge();
+		std::array<unsigned int, 2> va = { (*faces)[fa][ea], (*faces)[fa][(ea + 1) % 3] };
+
+		unsigned int fb = b.face();
+		unsigned int eb = b.edge();
+		std::array<unsigned int, 2> vb = { (*faces)[fb][eb], (*faces)[fb][(eb + 1) % 3] };
+
+		return va[0] == vb[1] && va[1] == vb[0];
+	}
+
+	std::vector<std::array<unsigned int, 3>> const* faces;
+};
+
+typedef std::unordered_set<Halfedge, HalfedgeHasher, HalfedgeEqualTo>
+	FaceSet;
+
+TriangleMesh::TriangleMesh(
+	std::vector<Vector3d> const& vertices,
+	std::vector<std::array<unsigned int, 3>> const& faces)
+	: m_faces(faces), m_e2e(3 * faces.size()), m_vertices(vertices)
+	, m_v2e(vertices.size())
+	, m_is_closed(false)
+{
+	construct();
+}
+
+TriangleMesh::TriangleMesh(double const* vertices,
+	unsigned int const* faces,
+	std::size_t nv, std::size_t nf)
+	: m_faces(nf), m_vertices(nv), m_e2e(3 * nf), m_v2e(nv), m_is_closed(false)
+{
+	std::copy(vertices, vertices + 3 * nv, m_vertices[0].data());
+	std::copy(faces, faces + 3 * nf, m_faces[0].data());
+	construct();
+}
+
+
+
+TriangleMesh::TriangleMesh(std::string const& path)
+{
+	m_is_closed = false;
+
+	std::ifstream in(path, std::ios::in);
+	if (!in) 
+	{ 
+	    std::cerr << "Cannot open " << path << std::endl; 
+	    return;
+	}
+
+	std::string line;
+	while (getline(in, line)) {
+	    if (line.substr(0, 2) == "v ") {
+	        std::istringstream s(line.substr(2));
+	        Vector3d v; s >> v.x(); s >> v.y(); s >> v.z();
+	        m_vertices.push_back(v);
+	    }
+	    else if (line.substr(0, 2) == "f ") {
+	        std::istringstream s(line.substr(2));
+	        std::array<unsigned int, 3> f;
+	        for (unsigned int j(0); j < 3; ++j)
+	        {
+	            std::string buf;
+	            s >> buf;
+	            buf = buf.substr(0, buf.find_first_of('/'));
+	            f[j] = std::stoi(buf) - 1;
+	        }
+	        m_faces.push_back(f);
+	    }
+	    else if (line[0] == '#') { /* ignoring this line */ }
+	    else { /* ignoring this line */ }
+	}
+
+	construct();
+}
+
+void
+TriangleMesh::exportOBJ(std::string const& filename) const
+{
+	auto outfile = std::ofstream(filename.c_str());
+	auto str_stream = std::stringstream(std::stringstream::in);
+
+	outfile << "g default" << std::endl;
+	for (auto const& pos : m_vertices)
+	{
+		outfile << "v " << pos[0] << " " << pos[1] << " " << pos[2] << "\n";
+	}
+	
+	for (auto const& f : m_faces)
+	{
+		outfile << "f";
+		for (auto v : f)
+			outfile << " " << v + 1;
+		outfile << std::endl;
+	}
+
+	outfile.close();
+}
+
+void
+TriangleMesh::construct()
+{
+
+	m_e2e.resize(3 * m_faces.size());
+	m_v2e.resize(m_vertices.size());
+
+	// Build adjacencies for mesh faces.
+	FaceSet face_set(
+		(m_faces.size() * 3) / 2,
+		HalfedgeHasher(m_faces),
+		HalfedgeEqualTo(m_faces));
+	for (unsigned int i(0); i < m_faces.size(); ++i)
+		for (unsigned char j(0); j < 3; ++j)
+		{
+		Halfedge he(i, j);
+		auto ret = face_set.insert(he);
+		if (!ret.second)
+		{
+			m_e2e[he.face()][he.edge()] = *(ret.first);
+			m_e2e[ret.first->face()][ret.first->edge()] = he;
+
+			face_set.erase(ret.first);
+		}
+
+		m_v2e[m_faces[i][j]] = he;
+		}
+
+	m_b2e.reserve(face_set.size());
+
+	for (Halfedge const he : face_set)
+	{
+		m_b2e.push_back(he);
+		Halfedge b(static_cast<unsigned int>(m_b2e.size()) - 1u, 3);
+		m_e2e[he.face()][he.edge()] = b;
+		m_v2e[target(he)] = b;
+
+		assert(source(b) == target(he));
+	}
+
+#ifdef _DEBUG
+	for (unsigned int i(0); i < nFaces(); ++i)
+	{
+		Halfedge h(i, 0);
+		for (unsigned int j(0); j < 3; ++j)
+		{
+			assert(faceVertex(i, j) == source(h));
+			h = h.next();
+		}
+	}
+#endif
+
+	if (!m_b2e.empty())
+	{
+		m_is_closed = false;
+	}
+	else
+	{
+		m_is_closed = true;
+	}
+}
+
+Vector3d
+TriangleMesh::computeFaceNormal(unsigned int f) const
+{
+	Vector3d const& x0 = vertex(faceVertex(f, 0));
+	Vector3d const& x1 = vertex(faceVertex(f, 1));
+	Vector3d const& x2 = vertex(faceVertex(f, 2));
+
+	return (x1 - x0).cross(x2 - x0).normalized();
+}
+
+}
+

extern/discregrid/discregrid/src/utility/spinlock.hpp

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <atomic>
+
+namespace Discregrid
+{
+class SpinLock
+{
+public:
+	void lock()
+	{
+		while(m_flag.test_and_set(std::memory_order_acquire)){}
+	}
+
+	void unlock()
+	{
+		m_flag.clear(std::memory_order_release);
+	}
+ 
+private:
+	std::atomic_flag m_flag = ATOMIC_FLAG_INIT;
+};
+}

extern/discregrid/discregrid/src/utility/timing.cpp

@@ -0,0 +1,10 @@
+#include "timing.hpp"
+
+using namespace Discregrid;
+
+std::unordered_map<int, AverageTime> Timing::m_averageTimes;
+std::stack<TimingHelper> Timing::m_timingStack;
+bool Timing::m_dontPrintTimes = false;
+unsigned int Timing::m_startCounter = 0;
+unsigned int Timing::m_stopCounter = 0;
+unsigned int Timing::m_id_counter = 0;

extern/discregrid/discregrid/src/utility/timing.hpp

@@ -0,0 +1,162 @@
+#pragma once
+
+#include <iostream>
+#include <stack>
+#include <unordered_map>
+#include <chrono>
+
+namespace Discregrid
+{
+
+#define START_TIMING(timerName) \
+		Timing::startTiming(timerName);
+
+#define STOP_TIMING \
+		Timing::stopTiming(false);
+
+#define STOP_TIMING_PRINT \
+		Timing::stopTiming(true);
+
+#define STOP_TIMING_AVG \
+		{ \
+		static int timing_timerId = -1; \
+		Timing::stopTiming(false, timing_timerId); \
+		}
+
+#define STOP_TIMING_AVG_PRINT \
+		{ \
+		static int timing_timerId = -1; \
+		Timing::stopTiming(true, timing_timerId); \
+		}
+
+
+	struct TimingHelper
+	{
+		std::chrono::time_point<std::chrono::high_resolution_clock> start;
+		std::string name;
+	};
+
+	struct AverageTime
+	{
+		double totalTime;
+		unsigned int counter;
+		std::string name;
+	};
+
+	class Timing
+	{
+	public:
+		static bool m_dontPrintTimes;
+		static unsigned int m_startCounter;
+		static unsigned int m_stopCounter;
+		static std::stack<TimingHelper> m_timingStack;
+		static std::unordered_map<int, AverageTime> m_averageTimes;
+		static unsigned int m_id_counter;
+
+		static void reset()
+		{
+			while (!m_timingStack.empty())
+				m_timingStack.pop();
+			m_averageTimes.clear();
+			m_startCounter = 0;
+			m_stopCounter = 0;
+		}
+
+		static void startTiming(const std::string& name = std::string(""))
+		{
+			TimingHelper h;
+			h.start = std::chrono::high_resolution_clock::now();
+			h.name = name;
+			Timing::m_timingStack.push(h);
+			Timing::m_startCounter++;
+		}
+
+		static double stopTiming(bool print = true)
+		{
+			if (!Timing::m_timingStack.empty())
+			{
+				Timing::m_stopCounter++;
+				std::chrono::time_point<std::chrono::high_resolution_clock> stop = std::chrono::high_resolution_clock::now();
+				TimingHelper h = Timing::m_timingStack.top();
+				Timing::m_timingStack.pop();
+				std::chrono::duration<double> elapsed_seconds = stop - h.start;
+				double t = elapsed_seconds.count() * 1000.0;
+
+				if (print)
+					std::cout << "time " << h.name.c_str() << ": " << t << " ms\n" << std::flush;
+				return t;
+		}
+			return 0;
+	}
+
+		static double stopTiming(bool print, int &id)
+		{
+			if (id == -1)
+				id = m_id_counter++;
+			if (!Timing::m_timingStack.empty())
+			{
+				Timing::m_stopCounter++;
+				std::chrono::time_point<std::chrono::high_resolution_clock> stop = std::chrono::high_resolution_clock::now();
+				TimingHelper h = Timing::m_timingStack.top();
+				Timing::m_timingStack.pop();
+
+				std::chrono::duration<double> elapsed_seconds = stop - h.start;
+				double t = elapsed_seconds.count() * 1000.0;
+
+				if (print && !Timing::m_dontPrintTimes)
+					std::cout << "time " << h.name.c_str() << ": " << t << " ms\n" << std::flush;
+
+				if (id >= 0)
+				{
+					std::unordered_map<int, AverageTime>::iterator iter;
+					iter = Timing::m_averageTimes.find(id);
+					if (iter != Timing::m_averageTimes.end())
+					{
+						Timing::m_averageTimes[id].totalTime += t;
+						Timing::m_averageTimes[id].counter++;
+					}
+					else
+					{
+						AverageTime at;
+						at.counter = 1;
+						at.totalTime = t;
+						at.name = h.name;
+						Timing::m_averageTimes[id] = at;
+					}
+				}
+				return t;
+			}
+			return 0;
+		}
+
+		static void printAverageTimes()
+		{
+			std::unordered_map<int, AverageTime>::iterator iter;
+			for (iter = Timing::m_averageTimes.begin(); iter != Timing::m_averageTimes.end(); iter++)
+			{
+				AverageTime &at = iter->second;
+				const double avgTime = at.totalTime / at.counter;
+				std::cout << "Average time " << at.name.c_str() << ": " << avgTime << " ms\n" << std::flush;
+			}
+			if (Timing::m_startCounter != Timing::m_stopCounter)
+				std::cout << "Problem: " << Timing::m_startCounter << " calls of startTiming and " << Timing::m_stopCounter << " calls of stopTiming.\n " << std::flush;
+			std::cout << "---------------------------------------------------------------------------\n\n";
+		}
+
+		static void printTimeSums()
+		{
+			std::unordered_map<int, AverageTime>::iterator iter;
+			for (iter = Timing::m_averageTimes.begin(); iter != Timing::m_averageTimes.end(); iter++)
+			{
+				AverageTime &at = iter->second;
+				const double timeSum = at.totalTime;
+				std::cout << "Time sum " << at.name.c_str() << ": " << timeSum << " ms\n" << std::flush;
+			}
+			if (Timing::m_startCounter != Timing::m_stopCounter)
+				std::cout << "Problem: " << Timing::m_startCounter << " calls of startTiming and " << Timing::m_stopCounter << " calls of stopTiming.\n " << std::flush;
+			std::cout << "---------------------------------------------------------------------------\n\n";
+		}
+	};
+
+}
+

extern/softbody/CMakeLists.txt

@@ -0,0 +1,61 @@
+# ***** BEGIN GPL LICENSE BLOCK *****
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software Foundation,
+# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+#
+# The Original Code is Copyright (C) 2006, Blender Foundation
+# All rights reserved.
+# ***** END GPL LICENSE BLOCK *****
+
+set(INC
+	src
+)
+
+set(INC_SYS
+	${EIGEN3_INCLUDE_DIRS}
+	../../source/blender/blenlib
+	../../source/blender/makesdna # BLI_math_geom requires DNA
+	../discregrid/discregrid/include
+)
+
+set(SRC
+	src/admmpd_bvh.h
+	src/admmpd_bvh.cpp
+	src/admmpd_bvh_traverse.h
+	src/admmpd_bvh_traverse.cpp
+	src/admmpd_collision.h
+	src/admmpd_collision.cpp
+	src/admmpd_energy.h
+	src/admmpd_energy.cpp
+	src/admmpd_geom.h
+	src/admmpd_geom.cpp
+	src/admmpd_linsolve.h
+	src/admmpd_linsolve.cpp
+	src/admmpd_solver.h
+	src/admmpd_solver.cpp
+	src/admmpd_log.h
+	src/admmpd_log.cpp
+	src/admmpd_mesh.h
+	src/admmpd_mesh.cpp
+	src/admmpd_timer.h
+	src/admmpd_types.h
+	src/svd/ImplicitQRSVD.h
+	src/svd/Tools.h
+)
+
+set(LIB
+	extern_discregrid
+)
+
+blender_add_lib(extern_admmpd "${SRC}" "${INC}" "${INC_SYS}" "${LIB}")

extern/softbody/LICENSE

@@ -0,0 +1,21 @@
+The MIT License (MIT)
+
+Copyright (c) 2020 Matthew Overby
+
+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.

extern/softbody/README.md

@@ -0,0 +1,5 @@
+# SoftBody #
+
+TODO
+
+

extern/softbody/src/admmpd_bvh.cpp

@@ -0,0 +1,381 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#include "admmpd_bvh.h"
+#include <numeric> // iota
+
+// Adapted from:
+// https://github.com/mattoverby/mclscene/blob/master/include/MCL/BVH.hpp
+
+namespace admmpd {
+
+template <typename T, int DIM>
+AABBTree<T,DIM>::AABBTree(const AABBTree<T,DIM> &other_tree)
+{
+    m_root = std::make_shared<Node>();
+    std::function<void(const Node*, Node*)> copy_children;
+    copy_children = [&copy_children](const Node *src, Node *dest)->void
+    {
+        if (src==nullptr)
+        {
+            dest = nullptr;
+            return;
+        }
+        dest->aabb = src->aabb;
+        dest->prims = src->prims;
+        copy_children(dest->left, src->left);
+        copy_children(dest->right, src->right);
+    };
+    copy_children(other_tree.m_root.get(), m_root.get());
+}
+
+template <typename T, int DIM>
+void AABBTree<T,DIM>::clear()
+{
+    m_root = std::make_shared<Node>();
+}
+
+template <typename T, int DIM>
+void AABBTree<T,DIM>::init(const std::vector<AABB> &leaves)
+{
+    m_root = std::make_shared<Node>();
+    int np = leaves.size();
+    if (np==0)
+        return;
+    std::vector<int> queue(np);
+    std::iota(queue.begin(), queue.end(), 0);
+    create_children(m_root.get(), queue, leaves);
+}
+
+template <typename T, int DIM>
+void AABBTree<T,DIM>::update(const std::vector<AABB> &leaves)
+{
+    if (!m_root || (int)leaves.size()==0)
+        return;
+    update_children(m_root.get(), leaves);
+}
+
+template <typename T, int DIM>
+bool AABBTree<T,DIM>::traverse(Traverser<T,DIM> &traverser) const
+{
+    if (!m_root)
+        return false;
+    return traverse_children(m_root.get(), traverser);
+}
+
+// If we are traversing with function pointers, we'll just
+// wrap our own traverser that calls these functions to
+// avoid duplicate traverse_children code.
+template <typename T, int DIM>
+class TraverserFromFunctionPtrs : public Traverser<T,DIM>
+{
+using typename Traverser<T,DIM>::AABB;
+public:
+    std::function<void(const AABB&, bool&, const AABB&, bool&, bool&)> t;
+    std::function<bool(const AABB&, int)> s;
+    void traverse(
+		const AABB &left_aabb, bool &go_left,
+		const AABB &right_aabb, bool &go_right,
+		bool &go_left_first)
+    {
+        t(left_aabb, go_left, right_aabb, go_right, go_left_first);
+    }
+	bool stop_traversing(const AABB &aabb, int prim)
+    {
+        return s(aabb,prim);
+    }
+};
+
+template <typename T, int DIM>
+bool AABBTree<T,DIM>::traverse(
+    std::function<void(const AABB&, bool&, const AABB&, bool&, bool&)> t,
+    std::function<bool(const AABB&, int)> s) const
+{
+    if (!m_root)
+        return false;
+    TraverserFromFunctionPtrs<T,DIM> traverser;
+    traverser.t = t;
+    traverser.s = s;
+    return traverse_children(m_root.get(), traverser);
+}
+
+template <typename T, int DIM>
+void AABBTree<T,DIM>::create_children(
+    Node *node,
+    std::vector<int> &queue,
+    const std::vector<AABB> &leaves)
+{
+	node->aabb.setEmpty();
+    int n_queue = queue.size();
+	if (n_queue == 1)
+    {
+	    node->prims.emplace_back(queue[0]);
+		node->aabb = leaves[queue[0]];
+		return;
+	}
+
+    for (int i=0; i<n_queue; ++i)
+    {
+        int q = queue[i];
+        node->aabb.extend(leaves[q]);
+    }
+
+    struct SortByAxis
+    {
+        int axis;
+        const std::vector<AABB> &aabbs;
+        SortByAxis(int axis_, const std::vector<AABB> &aabbs_) :
+            axis(axis_), aabbs(aabbs_) {}
+        bool operator()(size_t l, size_t r) const
+        {
+            return aabbs[l].center()[axis] < aabbs[r].center()[axis];
+        }
+    };
+
+    // Sort tree and split queue
+    int sort_axis = 0;
+    VecType sizes = node->aabb.sizes();
+    sizes.maxCoeff(&sort_axis);
+    std::sort(queue.begin(), queue.end(), SortByAxis(sort_axis,leaves));
+	std::vector<int> left_queue(queue.begin(), queue.begin()+(n_queue/2));
+	std::vector<int> right_queue(queue.begin()+(n_queue/2), queue.end());
+
+    // Recursive top-down constructrion
+    node->left = new Node();
+	create_children(node->left, left_queue, leaves);
+    node->right = new Node();
+	create_children(node->right, right_queue, leaves);
+
+} // end create children
+
+template <typename T, int DIM>
+void AABBTree<T,DIM>::update_children(
+    Node *node,
+    const std::vector<AABB> &leaves)
+{
+	node->aabb.setEmpty();
+
+    if (node->is_leaf())
+    {
+        int np = node->prims.size();
+        for (int i=0; i<np; ++i)
+        {
+            node->aabb.extend(leaves[node->prims[i]]);
+        }
+        return;
+    }
+
+	if (node->left != nullptr)
+    {
+		update_children(node->left, leaves);
+		node->aabb.extend(node->left->aabb);
+	}
+	if (node->right != nullptr)
+    {
+		update_children(node->right, leaves);
+		node->aabb.extend(node->right->aabb);
+	}
+
+} // end update children
+
+template <typename T, int DIM>
+bool AABBTree<T,DIM>::traverse_children(
+    const Node *node,
+    Traverser<T,DIM> &traverser ) const
+{
+	if( node->is_leaf() ){
+		int np = node->prims.size();
+		for(int i=0; i<np; ++i)
+        {
+			if(traverser.stop_traversing(node->aabb, node->prims[i]))
+                return true;
+		}
+		return false;
+	}
+
+	bool go_left = true;
+	bool go_right = true;
+	bool go_left_first = true;
+    const AABB &left_aabb = (node->left == nullptr ? AABB() : node->left->aabb);
+    const AABB &right_aabb = (node->right == nullptr ? AABB() : node->right->aabb);
+	traverser.traverse(
+		left_aabb, go_left,
+		right_aabb, go_right,
+		go_left_first );
+
+	if (go_left && go_right)
+    {
+        if (go_left_first)
+        {
+            if (traverse_children(node->left, traverser)) { return true; }
+		    else { return traverse_children(node->right, traverser); }
+        }
+        else
+        {
+            if (traverse_children(node->right, traverser)) { return true; }
+		    else { return traverse_children(node->left, traverser); }
+        }
+	}
+	if (go_left && !go_right)
+    {
+		return traverse_children(node->left, traverser);
+	}
+	if (!go_left && go_right)
+    {
+		return traverse_children(node->right, traverser);
+	}
+
+	return false;
+
+} // end traverse children
+
+template<typename T, int DIM>
+void Octree<T,DIM>::clear()
+{
+    m_root = std::make_shared<Node>();
+}
+
+template<typename T, int DIM>
+typename Octree<T,DIM>::AABB Octree<T,DIM>::bounds() const
+{
+    if (m_root)
+        return m_root->bounds();
+    return AABB();
+}
+
+template<typename T, int DIM>
+void Octree<T,DIM>::init(const MatrixXT *V, const Eigen::MatrixXi *F, int stopdepth)
+{
+    BLI_assert(V != nullptr);
+    BLI_assert(F != nullptr);
+    BLI_assert(F->cols()==3);
+    m_root = std::make_shared<Node>();
+
+    if (DIM !=3 || F->cols()!=3)
+    {
+        return;
+    }
+
+    int nf = F->rows();
+    AABB global_box;
+    std::vector<AABB> boxes(nf);
+    std::vector<int> queue(nf);
+    for (int i=0; i<nf; ++i)
+    {
+        Eigen::RowVector3i f = F->row(i);
+		queue[i]=i;
+		boxes[i].extend(V->row(f[0]).transpose());
+		boxes[i].extend(V->row(f[1]).transpose());
+		boxes[i].extend(V->row(f[2]).transpose());
+        boxes[i].extend(boxes[i].min()-VecType::Ones()*1e-4);
+        boxes[i].extend(boxes[i].max()+VecType::Ones()*1e-4);
+		global_box.extend(boxes[i]);
+    }
+
+    global_box.extend(global_box.min()-VecType::Ones()*1e-2);
+    global_box.extend(global_box.max()+VecType::Ones()*1e-2);
+
+	T halfwidth = global_box.sizes().maxCoeff()*0.5;
+    m_root.reset(
+        create_children(global_box.center(),halfwidth,stopdepth,V,F,queue,boxes)
+    );
+}
+
+template<typename T, int DIM>
+typename Octree<T,DIM>::Node* Octree<T,DIM>::create_children(
+    const VecType &center, T halfwidth, int stopdepth,
+    const MatrixXT *V, const Eigen::MatrixXi *F,
+    const std::vector<int> &queue,
+    const std::vector<AABB> &boxes)
+{
+    int nchild = std::pow(2,DIM);
+    BLI_assert((int)queue.size()>0);
+    BLI_assert((int)boxes.size()>0);
+    BLI_assert(F != nullptr);
+    BLI_assert(V != nullptr);
+    BLI_assert(F->cols()==3);
+    BLI_assert(V->cols()==3);
+    if (stopdepth >= 0)
+    {
+        Node *node = new Node();
+        node->center = center;
+        node->halfwidth = halfwidth;
+        node->prims.clear();
+        AABB box = node->bounds();
+        // Set list of intersected prims
+        int nq = queue.size();
+        for (int i=0; i<nq; ++i)
+        {
+            int p_idx = queue[i];
+            if (box.intersects(boxes[p_idx]))
+                node->prims.emplace_back(p_idx);
+        }
+        // Create children only if prims intersect
+        bool has_prims = node->prims.size()>0;
+        for (int i=0; i<nchild && has_prims; ++i)
+        {
+
+            T step = node->halfwidth * 0.5;
+            VecType offset = VecType::Zero();
+            offset[0] = ((i & 1) ? step : -step);
+            offset[1] = ((i & 2) ? step : -step);
+            if (DIM==3) offset[2] = ((i & 4) ? step : -step);
+
+            node->children[i] = create_children(
+                node->center+offset, step, stopdepth-1,
+                V, F, node->prims, boxes);
+        }
+        return node;
+    }
+    return nullptr;
+}
+
+template<typename T, int DIM>
+Octree<T,DIM>::Node::Node() :
+    center(VecType::Zero()),
+    halfwidth(0)
+{
+    int nchild = std::pow(2,DIM);
+	for (int i=0; i<nchild; ++i)
+		children[i] = nullptr;
+}
+
+template<typename T, int DIM>
+Octree<T,DIM>::Node::~Node()
+{
+    int nchild = std::pow(2,DIM);
+    for (int i=0; i<nchild; ++i)
+	    if (children[i] != nullptr)
+			delete children[i];
+}
+
+template<typename T, int DIM>
+bool Octree<T,DIM>::Node::is_leaf() const
+{
+    return children[0] == nullptr;
+}
+
+template<typename T, int DIM>
+typename Octree<T,DIM>::AABB Octree<T,DIM>::Node::bounds() const
+{
+    AABB box;
+    box.extend(center + VecType::Ones()*halfwidth);
+    box.extend(center - VecType::Ones()*halfwidth);
+    return box;
+}
+
+// Compile types
+template class admmpd::AABBTree<double,2>;
+template class admmpd::AABBTree<double,3>;
+template class admmpd::AABBTree<float,2>;
+template class admmpd::AABBTree<float,3>;
+template class admmpd::TraverserFromFunctionPtrs<double,2>;
+template class admmpd::TraverserFromFunctionPtrs<double,3>;
+template class admmpd::TraverserFromFunctionPtrs<float,2>;
+template class admmpd::TraverserFromFunctionPtrs<float,3>;
+//template class admmpd::Octree<double,2>;
+template class admmpd::Octree<double,3>;
+//template class admmpd::Octree<float,2>;
+template class admmpd::Octree<float,3>;
+
+} // namespace admmpd

extern/softbody/src/admmpd_bvh.h

@@ -0,0 +1,145 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_BVH_H_
+#define ADMMPD_BVH_H_ 1
+
+#include "admmpd_bvh_traverse.h"
+#include <vector>
+#include <memory>
+#include <functional>
+
+namespace admmpd {
+
+template <typename T, int DIM>
+class AABBTree
+{
+protected:
+	typedef Eigen::AlignedBox<T,DIM> AABB;
+	typedef Eigen::Matrix<T,DIM,1> VecType;
+public:
+	AABBTree() {}
+
+	// Performs a deep copy of another tree
+	AABBTree(const AABBTree<T,DIM> &other_tree);
+
+	// Removes all BVH data
+	void clear();
+
+	// Initializes the BVH with a list of leaf bounding boxes.
+	// Sorts each split by largest axis.
+	void init(const std::vector<AABB> &leaves);
+
+	// Recomputes the bounding boxes of leaf and parent
+	// nodes but does not sort the tree.
+	void update(const std::vector<AABB> &leaves);
+
+	// Traverse the tree. Returns result of traverser
+	bool traverse(Traverser<T,DIM> &traverser) const;
+
+	// Traverse the tree with function pointers instead of class:
+	// void traverse(
+	//	const AABB &left_aabb, bool &go_left,
+	//	const AABB &right_aabb, bool &go_right,
+	//	bool &go_left_first);
+	// bool stop_traversing( const AABB &aabb, int prim );
+	bool traverse(
+		std::function<void(const AABB&, bool&, const AABB&, bool&, bool&)> t,
+		std::function<bool(const AABB&, int)> s) const;
+
+	struct Node
+	{
+		AABB aabb;
+		Node *left, *right;
+		std::vector<int> prims;
+		bool is_leaf() const { return prims.size()>0; }
+		Node() : left(nullptr), right(nullptr) {}
+		~Node()
+		{
+			if (left) { delete left; }
+			if (right) { delete right; }
+		}
+	};
+
+	// Return AABB of root
+	Eigen::AlignedBox<T,DIM> bounds() const {
+		if (m_root) { return m_root->aabb; }
+		return Eigen::AlignedBox<T,DIM>();
+	}
+
+	// Return ptr to the root node
+	// Becomes invalidated after init()
+	std::shared_ptr<Node> root() { return m_root; }
+
+protected:
+
+	std::shared_ptr<Node> m_root;
+
+	void create_children(
+		Node *node,
+		std::vector<int> &queue,
+		const std::vector<AABB> &leaves);
+
+	void update_children(
+		Node *node,
+		const std::vector<AABB> &leaves);
+
+	bool traverse_children(
+		const Node *node,
+		Traverser<T,DIM> &traverser ) const;
+
+}; // class AABBtree
+
+
+// Octree is actually a quadtree if DIM=2
+template<typename T, int DIM>
+class Octree
+{
+protected:
+	typedef Eigen::AlignedBox<T,DIM> AABB;
+	typedef Eigen::Matrix<T,DIM,1> VecType;
+	typedef Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> MatrixXT;
+public:
+
+	// Removes all BVH data
+	void clear();
+
+	// Creates the Octree up to stopdepth. Only boxes containing
+	// faces are subdivided up to the depth.
+	void init(const MatrixXT *V, const Eigen::MatrixXi *F, int stopdepth);
+
+	// Returns bounding box of the root node
+	AABB bounds() const;
+
+	struct Node
+	{
+		VecType center;
+		T halfwidth;
+		Node *children[4*DIM];
+		std::vector<int> prims; // includes childen
+		bool is_leaf() const;
+		AABB bounds() const;
+		Node();
+		~Node();
+	};
+
+	// Return ptr to the root node
+	// Becomes invalidated after init()
+	std::shared_ptr<Node> root() { return m_root; }
+
+protected:
+
+	std::shared_ptr<Node> m_root;
+
+	Node* create_children(
+		const VecType &center, T halfwidth, int stopdepth,
+		const MatrixXT *V, const Eigen::MatrixXi *F,
+		const std::vector<int> &queue,
+		const std::vector<AABB> &boxes);
+
+}; // class Octree
+
+} // namespace admmpd
+
+#endif // ADMMPD_BVH_H_
+

extern/softbody/src/admmpd_bvh_traverse.cpp

@@ -0,0 +1,215 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_BVH_H_
+#define ADMMPD_BVH_H_ 1
+
+#include "admmpd_bvh_traverse.h"
+#include "admmpd_geom.h"
+#include "BLI_assert.h"
+#include "BLI_math_geom.h"
+
+namespace admmpd {
+using namespace Eigen;
+
+template <typename T>
+void PointInTetMeshTraverse<T>::traverse(
+	const AABB &left_aabb, bool &go_left,
+	const AABB &right_aabb, bool &go_right,
+	bool &go_left_first )
+{
+	if (left_aabb.contains(point))
+		go_left = true;
+	if (right_aabb.contains(point))
+		go_right = true;
+	(void)(go_left_first); // doesn't matter for point-in-tet
+}
+
+template <typename T>
+bool PointInTetMeshTraverse<T>::stop_traversing(
+		const AABB &aabb, int prim )
+{
+	BLI_assert(prim_verts->cols()==3);
+	BLI_assert(prim_inds->cols()==4);
+
+	if (!aabb.contains(point))
+		return false;
+
+	int n_tet_skip = skip_tet_inds.size();
+	for (int i=0; i<n_tet_skip; ++i)
+	{
+		if (skip_tet_inds[i]==prim) return false;
+	}
+
+	RowVector4i t = prim_inds->row(prim);
+	int n_skip = skip_vert_inds.size();
+	for (int i=0; i<n_skip; ++i)
+	{
+		if (skip_vert_inds[i]==t[0]) return false;
+		if (skip_vert_inds[i]==t[1]) return false;
+		if (skip_vert_inds[i]==t[2]) return false;
+		if (skip_vert_inds[i]==t[3]) return false;
+	}
+
+	VecType v[4] = {
+		prim_verts->row(t[0]),
+		prim_verts->row(t[1]),
+		prim_verts->row(t[2]),
+		prim_verts->row(t[3])
+	};
+
+	bool hit = geom::point_in_tet<T>(point, v[0], v[1], v[2], v[3]);
+
+	if (hit)
+		output.prim = prim;
+
+	return hit; // stop traversing if hit
+}
+
+template <typename T>
+PointInTriangleMeshTraverse<T>::PointInTriangleMeshTraverse(
+	const VecType &point_,
+	const MatrixXType *prim_verts_,
+	const Eigen::MatrixXi *prim_inds_,
+	const std::vector<int> &skip_inds_) :
+	point(point_),
+	dir(0,0,1),
+	prim_verts(prim_verts_),
+	prim_inds(prim_inds_),
+	skip_inds(skip_inds_)
+{
+	//dir = VecType::Random();
+	BLI_assert(prim_verts->rows()>=0);
+	BLI_assert(prim_inds->rows()>=0);
+	BLI_assert(prim_inds->cols()==3);
+	dir.normalize(); // TODO random unit vector
+	for (int i=0; i<3; ++i)
+	{
+		o[i] = (float)point[i];
+		d[i] = (float)dir[i];
+	}
+	isect_ray_tri_watertight_v3_precalc(&isect_precalc, d);
+}
+
+template <typename T>
+void PointInTriangleMeshTraverse<T>::traverse(
+	const AABB &left_aabb, bool &go_left,
+	const AABB &right_aabb, bool &go_right,
+	bool &go_left_first )
+{
+	const T t_min = 0;
+	const T t_max = std::numeric_limits<T>::max();
+	go_left = geom::ray_aabb<T>(point,dir,left_aabb,t_min,t_max);
+	go_right = geom::ray_aabb<T>(point,dir,right_aabb,t_min,t_max);
+	go_left_first = go_left;
+} // end point in mesh traverse
+
+template <typename T>
+bool PointInTriangleMeshTraverse<T>::stop_traversing(
+		const AABB &aabb, int prim )
+{
+	const T t_min = 0;
+	T t_max = std::numeric_limits<T>::max();
+
+	// Check if the tet box doesn't intersect the triangle box
+	if (!geom::ray_aabb<T>(point,dir,aabb,t_min,t_max))
+		return false;
+
+	// Get the vertices of the face in float arrays
+	// to interface with Blender kernels.
+	BLI_assert(prim >= 0 && prim < prim_inds->rows());
+	RowVector3i q_f = prim_inds->row(prim);
+	int n_skip = skip_inds.size();
+	for (int i=0; i<n_skip; ++i)
+	{
+		if (skip_inds[i]==q_f[0]) return false;
+		if (skip_inds[i]==q_f[1]) return false;
+		if (skip_inds[i]==q_f[2]) return false;
+	}
+	BLI_assert(q_f[0] < prim_verts->rows());
+	BLI_assert(q_f[1] < prim_verts->rows());
+	BLI_assert(q_f[2] < prim_verts->rows());
+	float q0[3], q1[3], q2[3];
+	for (int i=0; i<3; ++i)
+	{
+		q0[i] = (float)prim_verts->operator()(q_f[0],i);
+		q1[i] = (float)prim_verts->operator()(q_f[1],i);
+		q2[i] = (float)prim_verts->operator()(q_f[2],i);
+	}
+
+	// If we didn't have a triangle-triangle intersection
+	// then record if it was a ray-hit.
+	float lambda = 0;
+	float uv[2] = {0,0};
+	bool hit = isect_ray_tri_watertight_v3(o, &isect_precalc, q0, q1, q2, &lambda, uv);
+
+	if (hit)
+		output.hits.emplace_back(std::make_pair(prim,lambda));
+
+	return false; // multi-hit, so keep traversing
+
+} // end point in mesh stop traversing
+
+template <typename T>
+void NearestTriangleTraverse<T>::traverse(
+	const AABB &left_aabb, bool &go_left,
+	const AABB &right_aabb, bool &go_right,
+	bool &go_left_first)
+{
+	T l_d = left_aabb.exteriorDistance(point);
+	go_left = l_d < output.dist;
+	T r_d = right_aabb.exteriorDistance(point);
+	go_right = r_d < output.dist;
+	go_left_first = go_left <= go_right;
+}
+
+template <typename T>
+bool NearestTriangleTraverse<T>::stop_traversing(const AABB &aabb, int prim)
+{
+	BLI_assert(prim >= 0);
+	BLI_assert(prim < prim_inds->rows());
+	BLI_assert(prim_inds->cols()==3);
+
+	T b_dist = aabb.exteriorDistance(point);
+	if (b_dist > output.dist)
+		return false;
+
+	RowVector3i tri = prim_inds->row(prim);
+
+	int n_skip = skip_inds.size();
+	for (int i=0; i<n_skip; ++i)
+	{
+		if (skip_inds[i]==tri[0]) return false;
+		if (skip_inds[i]==tri[1]) return false;
+		if (skip_inds[i]==tri[2]) return false;
+	}
+
+	VecType v[3] = {
+		prim_verts->row(tri[0]),
+		prim_verts->row(tri[1]),
+		prim_verts->row(tri[2])
+	};
+	VecType pt_on_tri = geom::point_on_triangle<T>(point,v[0],v[1],v[2]);
+	double dist = (point-pt_on_tri).norm();
+	if (dist < output.dist)
+	{
+		output.prim = prim;
+		output.dist = dist;
+		output.pt_on_tri = pt_on_tri;		
+	}
+
+	return false;
+}
+
+// Compile template types
+template class admmpd::PointInTetMeshTraverse<double>;
+template class admmpd::PointInTetMeshTraverse<float>;
+template class admmpd::PointInTriangleMeshTraverse<double>;
+template class admmpd::PointInTriangleMeshTraverse<float>;
+template class admmpd::NearestTriangleTraverse<double>;
+template class admmpd::NearestTriangleTraverse<float>;
+
+} // namespace admmpd
+
+#endif // ADMMPD_BVH_H_
+

extern/softbody/src/admmpd_bvh_traverse.h

@@ -0,0 +1,168 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_BVH_TRAVERSE_H_
+#define ADMMPD_BVH_TRAVERSE_H_ 1
+
+#include <Eigen/Geometry>
+#include <vector>
+#include "BLI_math_geom.h"
+
+namespace admmpd {
+
+
+// Traverse class for traversing the structures.
+template <typename T, int DIM>
+class Traverser
+{
+protected:
+	typedef Eigen::AlignedBox<T,DIM> AABB;
+public:
+	// Set the boolean flags if we should go left, right, or both.
+	// Default for all booleans is true if left unchanged.
+	// Note that if stop_traversing ever returns true, it may not
+	// go left/right, even if you set go_left/go_right.
+	virtual void traverse(
+		const AABB &left_aabb, bool &go_left,
+		const AABB &right_aabb, bool &go_right,
+		bool &go_left_first) = 0;
+
+	// Return true to stop traversing.
+	// I.e., returning true is equiv to "hit anything stop checking",
+	// finding a closest object should return false (continue traversing).
+	virtual bool stop_traversing(const AABB &aabb, int prim) = 0;
+};
+
+// Point in tet mesh traversal
+template <typename T>
+class PointInTetMeshTraverse : public Traverser<T,3>
+{
+protected:
+	using typename Traverser<T,3>::AABB;
+	typedef Eigen::Matrix<T,3,1> VecType;
+	typedef Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> MatrixXType;
+
+	VecType point;
+	const MatrixXType *prim_verts;
+	const Eigen::MatrixXi *prim_inds;
+	std::vector<int> skip_vert_inds; // if tet contains these verts, skip test
+	std::vector<int> skip_tet_inds; // if tet is this index, skip test
+
+public:
+	struct Output {
+		int prim; // -1 if no intersections
+		Output() : prim(-1) {}
+	} output;
+
+	PointInTetMeshTraverse(
+		const VecType &point_,
+		const MatrixXType *prim_verts_,
+		const Eigen::MatrixXi *prim_inds_,
+		const std::vector<int> &skip_vert_inds_=std::vector<int>(),
+		const std::vector<int> &skip_tet_inds_=std::vector<int>()) :
+		point(point_),
+		prim_verts(prim_verts_),
+		prim_inds(prim_inds_),
+		skip_vert_inds(skip_vert_inds_),
+		skip_tet_inds(skip_tet_inds_)
+		{}
+
+	void traverse(
+		const AABB &left_aabb, bool &go_left,
+		const AABB &right_aabb, bool &go_right,
+		bool &go_left_first);
+
+	bool stop_traversing(const AABB &aabb, int prim);
+};
+
+// Point in triangle mesh traversal.
+// Determined by launching a ray in a random direction from
+// the point and counting the number of (watertight) intersections. If
+// the number of intersections is odd, the point is inside th mesh.
+template <typename T>
+class PointInTriangleMeshTraverse : public Traverser<T,3>
+{
+protected:
+	using typename Traverser<T,3>::AABB;
+	typedef Eigen::Matrix<T,3,1> VecType;
+	typedef Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> MatrixXType;
+
+	VecType point, dir;
+	const MatrixXType *prim_verts; // triangle mesh verts
+	const Eigen::MatrixXi *prim_inds; // triangle mesh inds
+	float o[3], d[3]; // pt and dir casted to float for Blender kernels
+	struct IsectRayPrecalc isect_precalc;
+	std::vector<int> skip_inds;
+
+public:
+	struct Output {
+		std::vector< std::pair<int,T> > hits; // [prim,t]
+		int num_hits() const { return hits.size(); }
+		bool is_inside() const { return hits.size()%2==1; }
+	} output;
+
+	PointInTriangleMeshTraverse(
+		const VecType &point_,
+		const MatrixXType *prim_verts_,
+		const Eigen::MatrixXi *prim_inds_,
+		const std::vector<int> &skip_inds_=std::vector<int>());
+
+	void traverse(
+		const AABB &left_aabb, bool &go_left,
+		const AABB &right_aabb, bool &go_right,
+		bool &go_left_first);
+
+	// Always returns false (multi-hit, so it doesn't stop)
+	bool stop_traversing(const AABB &aabb, int prim);
+};
+
+// Search for the nearest triangle to a given point
+template <typename T>
+class NearestTriangleTraverse : public Traverser<T,3>
+{
+protected:
+	using typename Traverser<T,3>::AABB;
+	typedef Eigen::Matrix<T,3,1> VecType;
+	typedef Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> MatrixXType;
+
+	VecType point;
+	const MatrixXType *prim_verts; // triangle mesh verts
+	const Eigen::MatrixXi *prim_inds; // triangle mesh inds
+	std::vector<int> skip_inds;
+
+public:
+	struct Output {
+		int prim;
+		T dist;
+		VecType pt_on_tri;
+		Output() :
+			prim(-1),
+			dist(std::numeric_limits<T>::max()),
+			pt_on_tri(0,0,0)
+			{}
+	} output;
+
+	NearestTriangleTraverse(
+		const VecType &point_,
+		const MatrixXType *prim_verts_,
+		const Eigen::MatrixXi *prim_inds_,
+		const std::vector<int> &skip_inds_=std::vector<int>()) :
+		point(point_),
+		prim_verts(prim_verts_),
+		prim_inds(prim_inds_),
+		skip_inds(skip_inds_)
+		{}
+
+	void traverse(
+		const AABB &left_aabb, bool &go_left,
+		const AABB &right_aabb, bool &go_right,
+		bool &go_left_first);
+
+	// Always returns false
+	bool stop_traversing(const AABB &aabb, int prim);
+};
+
+} // namespace admmpd
+
+#endif // ADMMPD_BVH_TRAVERSE_H_
+

extern/softbody/src/admmpd_collision.cpp

@@ -0,0 +1,737 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#include "admmpd_collision.h"
+#include "admmpd_bvh_traverse.h"
+#include "admmpd_geom.h"
+
+#include "BLI_assert.h"
+#include "BLI_task.h"
+#include "BLI_threads.h"
+#include <thread>
+
+#include <iostream>
+#include <sstream>
+
+namespace admmpd {
+using namespace Eigen;
+
+VFCollisionPair::VFCollisionPair() :
+    p_idx(-1),
+    p_is_obs(0),
+    q_idx(-1),
+    q_is_obs(0),
+	q_pt(0,0,0),
+	q_n(0,0,0),
+	q_bary(0,0,0)
+	{}
+
+bool Collision::ObstacleData::compute_sdf(int idx)
+{
+	if (idx < 0 || idx >x1.size()) {
+		return false;
+	}
+
+	// There was an error in init
+	if (box[idx].isEmpty()) {
+		return false;
+	}
+
+	// Test that the mesh is closed
+	Discregrid::TriangleMesh tm(
+		(double const*)x1[idx].data(),
+		(unsigned int const*)F[idx].data(),
+		x1[idx].rows(), F[idx].rows());
+	if (!tm.is_closed()) {
+		return false;
+	}
+
+	// Generate signed distance field
+	Discregrid::MeshDistance md(tm);
+	std::array<unsigned int, 3> resolution;
+	resolution[0] = 30; resolution[1] = 30; resolution[2] = 30;
+	sdf[idx] = Discregrid::CubicLagrangeDiscreteGrid(box[idx], resolution);
+	auto func = Discregrid::DiscreteGrid::ContinuousFunction{};
+	std::vector<std::thread::id> thread_map;
+	md.set_thread_map(&thread_map);
+	func = [&md](Eigen::Vector3d const& xi) {
+		return md.signedDistanceCached(xi);
+	};
+	sdf[idx].addFunction(func, &thread_map, false);
+
+	if (sdf[idx].nCells()==0) {
+		return false;
+	}
+	return true;
+}
+
+bool Collision::set_obstacles(
+	std::vector<Eigen::MatrixXd> &v0,
+	std::vector<Eigen::MatrixXd> &v1,
+	std::vector<Eigen::MatrixXi> &F,
+	std::string *err)
+{
+	if (v0.size() != v1.size() || v0.size() != F.size()) {
+		if (err) { *err = "Bad dimensions on obstacle input"; }
+		return false;
+	}
+
+	// Copy the obstacle data from the input to the stored
+	// data container. If the vertex locations have changed,
+	// we need to recompute the SDF. Otherwise, leave it as is.
+	int n_obs_new = v0.size();
+	int n_obs_old = obsdata.x0.size();
+    obsdata.sdf.resize(n_obs_new);
+	obsdata.x0.resize(n_obs_new);
+	obsdata.x1.resize(n_obs_new);
+	obsdata.F.resize(n_obs_new);
+	obsdata.box.resize(n_obs_new);
+
+	// We can use isApprox for testing if the obstacle has
+	// moved from the last call to set_obstacles. The SDF
+	// has limited accuracy anyway...
+	double approx_eps = 1e-6;
+	for (int i=0; i<n_obs_new; ++i) {
+
+		bool reset_obs = false;
+		if (i >= n_obs_old) {
+			reset_obs=true; // is new obs
+		}
+		else if (!obsdata.x1[i].isApprox(v1[i],approx_eps) ||
+				!obsdata.x0[i].isApprox(v0[i],approx_eps)) {
+			reset_obs = true; // is different than before
+		}
+
+		if (reset_obs) {
+
+			obsdata.box[i].setEmpty();
+			int nv = v1[i].rows();
+			for (int j=0; j<nv; ++j) {
+				obsdata.box[i].extend(v1[i].row(j).transpose());
+			}
+			obsdata.box[i].max() += 1e-3 * obsdata.box[i].diagonal().norm() * Eigen::Vector3d::Ones();
+			obsdata.box[i].min() -= 1e-3 * obsdata.box[i].diagonal().norm() * Eigen::Vector3d::Ones();
+
+			obsdata.sdf[i] = SDFType(); // clear old sdf
+			obsdata.x0[i] = v0[i];
+			obsdata.x1[i] = v1[i];
+			obsdata.F[i] = F[i].cast<unsigned int>();
+
+			// Determine if the triangle mesh is closed or not.
+			// We want to provide a warning if it is.
+			Discregrid::TriangleMesh tm(
+				(double const*)obsdata.x1[i].data(),
+				(unsigned int const*)obsdata.F[i].data(),
+				obsdata.x1[i].rows(), obsdata.F[i].rows());
+			if (!tm.is_closed()) {
+				obsdata.box[i].setEmpty();
+				if (err) { *err = "Collision obstacle not a closed mesh - ignoring"; }
+			}
+		}
+	}
+
+	return true;
+
+} // end add obstacle
+
+std::pair<bool,VFCollisionPair>
+Collision::detect_against_obs(
+	    const admmpd::Mesh *mesh,
+	    const admmpd::Options *options,
+        const admmpd::SolverData *data,
+        const Eigen::Vector3d &pt_t0,
+		const Eigen::Vector3d &pt_t1,
+        const ObstacleData *obs) const
+{
+	(void)(mesh);
+	(void)(options);
+	(void)(data);
+	(void)(pt_t0);
+
+	int n_obs = obs->num_obs();
+	if (n_obs==0) {
+		return std::make_pair(false, VFCollisionPair());
+	}
+
+	for (int i=0; i<n_obs; ++i)
+	{
+		if (obs->sdf[i].nCells()==0) {
+			continue; // not initialized
+		}
+		Vector3d n;
+		double dist = obs->sdf[i].interpolate(0, pt_t1, &n);
+		if (dist > 0) { continue; } // not colliding
+
+		std::pair<bool,VFCollisionPair> ret = 
+			std::make_pair(true, VFCollisionPair());
+		ret.first = true;
+		ret.second.q_idx = -1;
+		ret.second.q_is_obs = true;
+		ret.second.q_bary.setZero();
+		ret.second.q_pt = pt_t1 - dist*n;
+		ret.second.q_n = n.normalized();
+		return ret;
+	}
+	return std::make_pair(false, VFCollisionPair());
+}
+
+int EmbeddedMeshCollision::detect(
+	const admmpd::Mesh *mesh,
+	const admmpd::Options *options,
+	const admmpd::SolverData *data,
+	const Eigen::MatrixXd *x0,
+	const Eigen::MatrixXd *x1)
+{
+	if (!mesh) {
+		return 0;
+	}
+
+	if (mesh->type() != MESHTYPE_EMBEDDED) {
+		return 0;
+	}
+
+	// Compute SDFs if the mesh is intersecting
+	// the associated obstacle. The sdf generation is internally threaded,
+	// but it might be faster to thread the different SDFs.
+	bool has_obs_intersection = false;
+	int n_obs = obsdata.num_obs();
+	AlignedBox<double,3> mesh_box = data->col.prim_tree.bounds();
+	for (int i=0; i<n_obs; ++i) {
+		AlignedBox<double,3> &box = obsdata.box[i];
+		if (box.isEmpty()) { continue; }
+		if (!box.intersects(mesh_box)) { continue; }
+		has_obs_intersection = true;
+		// Do we need to generate a new SDF?
+		if (obsdata.sdf[i].nCells()==0) {
+			obsdata.compute_sdf(i);
+		}
+	}
+
+	// Do we even need to process collisions and launch
+	// the per-vertex threads?
+	if (!has_obs_intersection && !options->self_collision) {
+		if (x1->col(2).minCoeff() > options->floor) {
+			return 0;
+		}
+	}
+
+	// We store the results of the collisions in a per-vertex buffer.
+	// This is a workaround so we can create them in threads.
+	int nev = mesh->rest_facet_verts()->rows();
+	if ((int)per_vertex_pairs.size() != nev) {
+		per_vertex_pairs.resize(nev, std::vector<VFCollisionPair>());
+	}
+
+	//
+	// Thread data for detection
+	//
+	typedef struct {
+		const Mesh *mesh;
+		const Options *options;
+		const SolverData *data;
+		const Collision *collision;
+		const Collision::ObstacleData *obsdata;
+		const Eigen::MatrixXd *x0;
+		const Eigen::MatrixXd *x1;
+		std::vector<std::vector<VFCollisionPair> > *per_vertex_pairs;
+		std::vector<std::vector<Eigen::Vector2i> > *per_thread_results;
+	} DetectThreadData;
+
+	//
+	// Detection function for a single embedded vertex
+	// This function is very poorly optimized in terms of
+	// cache friendlyness.
+	// Some refactoring would greatly improve run time.
+	//
+//	auto per_embedded_vertex_detect = [](
+//		void *__restrict userdata,
+//		const int vi,
+//		const TaskParallelTLS *__restrict tls)->void
+	auto per_embedded_vertex_detect = [](
+		DetectThreadData *td,
+		int thread_idx,
+		int vi)->void
+	{
+//		(void)(tls);
+//		DetectThreadData *td = (DetectThreadData*)userdata;
+		if (!td->mesh)
+			return;
+
+		std::vector<Eigen::Vector2i> &pt_res = td->per_thread_results->at(thread_idx);
+		std::vector<VFCollisionPair> &vi_pairs = td->per_vertex_pairs->at(vi);
+		vi_pairs.clear();
+		Vector3d pt_t0 = td->mesh->get_mapped_facet_vertex(td->x0,vi);
+		Vector3d pt_t1 = td->mesh->get_mapped_facet_vertex(td->x1,vi);
+
+//		if (td->options->log_level >= LOGLEVEL_DEBUG) {
+//			printf("\tDetecting collisions for emb vertex %d: %f %f %f\n", vi, pt_t1[0], pt_t1[1], pt_t1[2]);
+//		}
+
+		// Special case, check if we are below the floor
+		if (pt_t1[2] < td->options->floor)
+		{
+			pt_res.emplace_back(vi,vi_pairs.size());
+			vi_pairs.emplace_back();
+			VFCollisionPair &pair = vi_pairs.back();
+			pair.p_idx = vi;
+			pair.p_is_obs = false;
+			pair.q_idx = -1;
+			pair.q_is_obs = 1;
+			pair.q_bary.setZero();
+			pair.q_pt = Vector3d(pt_t1[0],pt_t1[1],td->options->floor);
+			pair.q_n = Vector3d(0,0,1);
+		}
+
+		// Detect against obstacles
+		bool had_obstacle_collision = false;
+		if (td->obsdata->num_obs()>0)
+		{
+			std::pair<bool,VFCollisionPair> pt_hit_obs =
+				td->collision->detect_against_obs(
+					td->mesh,
+					td->options,
+					td->data,
+					pt_t0,
+					pt_t1,
+					td->obsdata);
+			if (pt_hit_obs.first)
+			{
+				pt_hit_obs.second.p_idx = vi;
+				pt_hit_obs.second.p_is_obs = false;
+				pt_res.emplace_back(vi,vi_pairs.size());
+				vi_pairs.emplace_back(pt_hit_obs.second);
+				had_obstacle_collision = true;
+			}
+		}
+
+		// We perform self collision if the self_collision flag is true and:
+		// a) there was no obstacle collision
+		// b) the vertex is in the set of self collision vertices (or its empty)
+		bool do_self_collision = !had_obstacle_collision && td->options->self_collision;
+		if (do_self_collision) {
+			if (td->data->col.selfcollision_verts.size()>0) {
+				do_self_collision = td->data->col.selfcollision_verts.count(vi)>0;
+			}
+		}
+
+		// Detect against self
+		if (do_self_collision)
+		{
+			std::pair<bool,VFCollisionPair> pt_hit_self =
+				td->collision->detect_against_self(
+					td->mesh,
+					td->options,
+					td->data,
+					vi,
+					pt_t0,
+					pt_t1,
+					td->x0,
+					td->x1);
+			if (pt_hit_self.first)
+			{
+				pt_res.emplace_back(vi,vi_pairs.size());
+				vi_pairs.emplace_back(pt_hit_self.second);
+			}
+		}
+
+	}; // end detect for a single embedded vertex
+
+	std::vector<std::vector<Eigen::Vector2i> > per_thread_results;
+	DetectThreadData thread_data = {
+		mesh,
+		options,
+		data,
+		this,
+		&obsdata,
+		x0,
+		x1,
+		&per_vertex_pairs,
+		&per_thread_results
+	};
+
+	// The pooling is a little unusual here.
+	// Collisions are processed per-vertex. If one vertex is colliding, it's
+	// likely that adjacent vertices are also colliding.
+	// Because of this, it may be better to interlace/offset the pooling so that
+	// vertices next to eachother are on different threads to provide
+	// better concurrency. Otherwise a standard slice may end up doing
+	// all of the BVH traversals and the other threads do none.
+	// I haven't actually profiled this, so maybe I'm wrong.
+	int max_threads = std::max(1, std::min(nev, admmpd::get_max_threads(options)));
+	if (options->log_level >= LOGLEVEL_DEBUG) {
+		max_threads = 1;
+	}
+	const auto & per_thread_function = [&per_embedded_vertex_detect,&max_threads,&nev]
+		(DetectThreadData *td, int thread_idx)
+	{
+		float slice_f = float(nev+1) / float(max_threads);
+    	int slice = std::max((int)std::ceil(slice_f),1);
+		for (int i=0; i<slice; ++i)
+		{
+			int vi = i*max_threads + thread_idx;
+
+			// Yeah okay I know this is dumb and I can just do a better job
+			// of calculating the slice. We can save thread optimization
+			// for the future, since this will be written different anyway.
+			if (vi >= nev) {
+				break;
+			}
+
+			per_embedded_vertex_detect(td,thread_idx,vi);
+		}
+	};
+
+	// Launch threads
+	std::vector<std::thread> pool;
+	per_thread_results.resize(max_threads, std::vector<Vector2i>());
+	for (int i=0; i<max_threads; ++i) {
+		per_thread_results[i].reserve(std::max(1,nev/max_threads));
+	}
+	for (int i=0; i<max_threads; ++i) {
+		pool.emplace_back(per_thread_function,&thread_data,i);
+	}
+
+	// Combine parallel results
+	vf_pairs.clear();
+	for (int i=0; i<max_threads; ++i)
+	{
+		if (pool[i].joinable())
+			pool[i].join(); // wait for thread to finish
+
+		// Other threads may be finishing while we insert results
+		// into the global buffer. That's okay!
+		vf_pairs.insert(vf_pairs.end(),
+			per_thread_results[i].begin(), per_thread_results[i].end());
+	}
+
+//	TaskParallelSettings thrd_settings;
+//	BLI_parallel_range_settings_defaults(&thrd_settings);
+//	BLI_task_parallel_range(0, nev, &thread_data, per_embedded_vertex_detect, &thrd_settings);
+//	vf_pairs.clear();
+//	for (int i=0; i<nev; ++i)
+//	{
+//		int pvp = per_vertex_pairs[i].size();
+//		for (int j=0; j<pvp; ++j)
+//			vf_pairs.emplace_back(Vector2i(i,j));
+//	}
+
+	return vf_pairs.size();
+} // end detect
+
+void EmbeddedMeshCollision::update_bvh(
+	const admmpd::Mesh *mesh,
+	const admmpd::Options *options,
+	admmpd::SolverData *data,
+	const Eigen::MatrixXd *x0,
+	const Eigen::MatrixXd *x1,
+	bool sort)
+{
+	(void)(options);
+	(void)(x0);
+
+	if (!mesh)
+		return;
+
+	if (mesh->type() != MESHTYPE_EMBEDDED)
+		return;
+
+	int nt = mesh->prims()->rows();
+	if ((int)data->col.prim_boxes.size() != nt)
+		data->col.prim_boxes.resize(nt);
+
+	for (int i=0; i<nt; ++i)
+	{
+		RowVector4i tet = mesh->prims()->row(i);
+		AlignedBox<double,3> &box = data->col.prim_boxes[i];
+		box.setEmpty();
+		box.extend(x1->row(tet[0]).transpose());
+		box.extend(x1->row(tet[1]).transpose());
+		box.extend(x1->row(tet[2]).transpose());
+		box.extend(x1->row(tet[3]).transpose());
+	}
+
+	if (!data->col.prim_tree.root() || sort)
+		{ data->col.prim_tree.init(data->col.prim_boxes); } // sort
+	else
+		{ data->col.prim_tree.update(data->col.prim_boxes); } // grow
+
+} // end update bvh
+
+// Self collisions
+std::pair<bool,VFCollisionPair>
+EmbeddedMeshCollision::detect_against_self(
+	const admmpd::Mesh *mesh_,
+	const admmpd::Options *options,
+	const admmpd::SolverData *data,
+	int pt_idx,
+	const Eigen::Vector3d &pt_t0,
+	const Eigen::Vector3d &pt_t1,
+	const Eigen::MatrixXd *x0,
+	const Eigen::MatrixXd *x1) const
+{
+	(void)(pt_t0);
+	(void)(x0);
+	(void)(options);
+	(void)(data);
+
+	std::pair<bool,VFCollisionPair> ret = 
+		std::make_pair(false, VFCollisionPair());
+
+	if (!mesh_)
+		return ret;
+
+	if (mesh_->type() != MESHTYPE_EMBEDDED)
+		return ret;
+
+	const EmbeddedMesh* mesh = dynamic_cast<const EmbeddedMesh*>(mesh_);
+
+	// Are we in the (deforming) tet mesh?
+	int self_tet_idx = mesh->emb_vtx_to_tet()->operator[](pt_idx);
+	std::vector<int> skip_tet_inds = {self_tet_idx};
+	PointInTetMeshTraverse<double> pt_in_tet(
+		pt_t1,
+		x1,
+		mesh->prims(), // tets
+		std::vector<int>(), // skip tets that contain these verts
+		skip_tet_inds); // skip tet that is this index
+	bool in_mesh = data->col.prim_tree.traverse(pt_in_tet);
+	if (!in_mesh)
+		return ret;
+
+	// Transform point to rest shape
+	int tet_idx = pt_in_tet.output.prim;
+	RowVector4i tet = mesh->prims()->row(tet_idx);
+	Vector4d barys = geom::point_tet_barys<double>(pt_t1,
+		x1->row(tet[0]),
+		x1->row(tet[1]),
+		x1->row(tet[2]),
+		x1->row(tet[3]));
+	if (barys.minCoeff()<-1e-8 || barys.sum() > 1+1e-8) {
+		throw std::runtime_error("EmbeddedMeshCollision: Bad tet barys");
+	}
+
+	const MatrixXd *rest_V0 = mesh->rest_prim_verts();
+	Vector3d rest_pt =
+		barys[0]*rest_V0->row(tet[0])+
+		barys[1]*rest_V0->row(tet[1])+
+		barys[2]*rest_V0->row(tet[2])+
+		barys[3]*rest_V0->row(tet[3]);
+
+	// Verify we are in the surface mesh, not just the lattice tet mesh
+	const SDFType *rest_emb_sdf = mesh->rest_facet_sdf();
+	if (rest_emb_sdf)
+	{
+		double dist = rest_emb_sdf->interpolate(0, rest_pt);
+		if (dist > 0) {
+			return ret; // nope
+		}
+	}
+
+	// Find triangle surface projection that doesn't
+	// include the penetrating vertex
+	const MatrixXd *emb_V0 = mesh->rest_facet_verts();
+	std::vector<int> skip_tri_inds = {pt_idx};
+	NearestTriangleTraverse<double> nearest_tri(
+		rest_pt,
+		emb_V0,
+		mesh->facets(), // triangles
+		skip_tri_inds);
+	mesh->emb_rest_tree()->traverse(nearest_tri);
+
+	if (nearest_tri.output.prim<0) {
+		throw std::runtime_error("EmbeddedMeshCollision: Failed to find triangle");
+	}
+
+	ret.first = true;
+	ret.second.p_idx = pt_idx;
+	ret.second.p_is_obs = false;
+	ret.second.q_idx = nearest_tri.output.prim;
+	ret.second.q_is_obs = false;
+
+	// Compute barycoords of projection
+	RowVector3i f = mesh->facets()->row(nearest_tri.output.prim);
+	Vector3d v3[3] = { emb_V0->row(f[0]), emb_V0->row(f[1]), emb_V0->row(f[2]) };
+	ret.second.q_bary = geom::point_triangle_barys<double>(
+		nearest_tri.output.pt_on_tri, v3[0], v3[1], v3[2]);
+	if (ret.second.q_bary.minCoeff()<-1e-8 || ret.second.q_bary.sum() > 1+1e-8) {
+		throw std::runtime_error("EmbeddedMeshCollision: Bad triangle barys");
+	}
+
+	// q_pt is not used for self collisions, but we'll use it
+	// to define the tet constraint stencil.
+	ret.second.q_pt = pt_t1;
+
+	return ret;
+}
+
+
+void EmbeddedMeshCollision::graph(
+	const admmpd::Mesh *mesh_,
+	std::vector<std::set<int> > &g)
+{
+	if (!mesh_)
+		return;
+
+	if (mesh_->type() != MESHTYPE_EMBEDDED)
+		return;
+
+	const EmbeddedMesh *mesh = dynamic_cast<const EmbeddedMesh*>(mesh_);
+
+	int np = vf_pairs.size();
+	if (np==0)
+		return;
+
+	int nv = mesh->rest_prim_verts()->rows();
+	if ((int)g.size() < nv)
+		g.resize(nv, std::set<int>());
+
+	for (int i=0; i<np; ++i)
+	{
+		Vector2i pair_idx = vf_pairs[i];
+		VFCollisionPair &pair = per_vertex_pairs[pair_idx[0]][pair_idx[1]];
+		std::set<int> stencil;
+
+		if (!pair.p_is_obs)
+		{
+			int tet_idx = mesh->emb_vtx_to_tet()->operator[](pair.p_idx);
+			RowVector4i tet = mesh->prims()->row(tet_idx);
+			stencil.emplace(tet[0]);
+			stencil.emplace(tet[1]);
+			stencil.emplace(tet[2]);
+			stencil.emplace(tet[3]);
+		}
+		if (!pair.q_is_obs)
+		{
+			RowVector3i emb_face = mesh->facets()->row(pair.q_idx);
+			for (int j=0; j<3; ++j)
+			{
+				int tet_idx = mesh->emb_vtx_to_tet()->operator[](emb_face[j]);
+				RowVector4i tet = mesh->prims()->row(tet_idx);
+				stencil.emplace(tet[0]);
+				stencil.emplace(tet[1]);
+				stencil.emplace(tet[2]);
+				stencil.emplace(tet[3]);	
+			}
+		}
+
+		for (std::set<int>::iterator it = stencil.begin();
+			it != stencil.end(); ++it)
+		{
+			for (std::set<int>::iterator it2 = stencil.begin();
+				it2 != stencil.end(); ++it2)
+			{
+				if (*it == *it2)
+					continue;
+				g[*it].emplace(*it2);
+			}
+		}
+	}
+} // end graph
+
+void EmbeddedMeshCollision::linearize(
+	const admmpd::Mesh *mesh_,
+	const admmpd::Options *options,
+	const admmpd::SolverData *data,
+	const Eigen::MatrixXd *x,
+	std::vector<Eigen::Triplet<double> > *trips,
+	std::vector<double> *d) const
+{
+	(void)(data);
+	BLI_assert(x != NULL);
+	BLI_assert(x->cols() == 3);
+
+	if (!mesh_)
+		return;
+
+	if (mesh_->type() != MESHTYPE_EMBEDDED)
+		return;
+
+	const EmbeddedMesh *mesh = dynamic_cast<const EmbeddedMesh*>(mesh_);
+
+	int np = vf_pairs.size();
+	if (np==0)
+		return;
+
+	//int nx = x->rows();
+	d->reserve((int)d->size() + np);
+	trips->reserve((int)trips->size() + np*3*4);
+	double eta = 0;//std::max(0.0,options->collision_thickness);
+
+	for (int i=0; i<np; ++i)
+	{
+		const Vector2i &pair_idx = vf_pairs[i];
+		const VFCollisionPair &pair = per_vertex_pairs[pair_idx[0]][pair_idx[1]];
+		int emb_p_idx = pair.p_idx;
+//		Vector3d p_pt = meshdata.mesh->get_mapped_facet_vertex(x,emb_p_idx);
+
+		//
+		// Obstacle collision
+		//
+		if (pair.q_is_obs)
+		{
+			// Get the four deforming verts that embed
+			// the surface vertices, and add constraints on those.
+			RowVector4d bary = mesh->emb_barycoords()->row(emb_p_idx);
+			int tet_idx = mesh->emb_vtx_to_tet()->operator[](emb_p_idx);
+			RowVector4i tet = mesh->prims()->row(tet_idx);
+			int c_idx = d->size();
+			d->emplace_back(pair.q_n.dot(pair.q_pt) + eta);
+			for (int j=0; j<4; ++j)
+			{
+				trips->emplace_back(c_idx, tet[j]*3+0, bary[j]*pair.q_n[0]);
+				trips->emplace_back(c_idx, tet[j]*3+1, bary[j]*pair.q_n[1]);
+				trips->emplace_back(c_idx, tet[j]*3+2, bary[j]*pair.q_n[2]);
+			}
+
+		} // end q is obs
+		//
+		// Self collision
+		//
+		else
+		{
+			int c_idx = d->size();
+			d->emplace_back(eta);
+
+			// Compute the normal in the deformed space
+			RowVector3i q_face = mesh->facets()->row(pair.q_idx);
+			Vector3d q_v0 = mesh->get_mapped_facet_vertex(x,q_face[0]);
+			Vector3d q_v1 = mesh->get_mapped_facet_vertex(x,q_face[1]);
+			Vector3d q_v2 = mesh->get_mapped_facet_vertex(x,q_face[2]);
+			Vector3d q_n = (q_v1-q_v0).cross(q_v2-q_v0);
+			q_n.normalize();
+
+			// The penetrating vertex:
+			{
+				int tet_idx = mesh->emb_vtx_to_tet()->operator[](emb_p_idx);
+				RowVector4d bary = mesh->emb_barycoords()->row(emb_p_idx);
+				RowVector4i tet = mesh->prims()->row(tet_idx);
+				for (int j=0; j<4; ++j)
+				{
+					trips->emplace_back(c_idx, tet[j]*3+0, bary[j]*q_n[0]);
+					trips->emplace_back(c_idx, tet[j]*3+1, bary[j]*q_n[1]);
+					trips->emplace_back(c_idx, tet[j]*3+2, bary[j]*q_n[2]);
+				}
+			}
+
+			// The intersected face:
+			for (int j=0; j<3; ++j)
+			{
+				int emb_q_idx = q_face[j];
+				RowVector4d bary = mesh->emb_barycoords()->row(emb_q_idx);
+				int tet_idx = mesh->emb_vtx_to_tet()->operator[](emb_q_idx);
+				RowVector4i tet = mesh->prims()->row(tet_idx);
+				for (int k=0; k<4; ++k)
+				{
+					trips->emplace_back(c_idx, tet[k]*3+0, -pair.q_bary[j]*bary[k]*q_n[0]);
+					trips->emplace_back(c_idx, tet[k]*3+1, -pair.q_bary[j]*bary[k]*q_n[1]);
+					trips->emplace_back(c_idx, tet[k]*3+2, -pair.q_bary[j]*bary[k]*q_n[2]);
+				}
+			}
+
+		} // end q is obs
+	
+	} // end loop pairs
+
+} // end jacobian
+
+} // namespace admmpd

extern/softbody/src/admmpd_collision.h

@@ -0,0 +1,158 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_COLLISION_H_
+#define ADMMPD_COLLISION_H_
+
+#include "admmpd_types.h"
+#include "admmpd_mesh.h"
+
+namespace admmpd {
+
+struct VFCollisionPair {
+    int p_idx; // point
+    int p_is_obs; // 0 or 1
+    int q_idx; // idx of hit face, or -1 if obstacle
+    int q_is_obs; // 0 or 1
+    Eigen::Vector3d q_pt; // pt of collision (if q obs)
+    Eigen::Vector3d q_n; // normal of collision (if q obs)
+    Eigen::Vector3d q_bary; // barys of collision (if q !obs)
+    VFCollisionPair();
+};
+
+class Collision {
+public:
+    struct ObstacleData {
+        int num_obs() const { return sdf.size(); }
+        bool compute_sdf(int idx);
+        std::vector<SDFType> sdf;
+        // Obstacle data stored in custom matrix type to interop with DiscreGrid
+        std::vector<Eigen::Matrix<double,Eigen::Dynamic,3,Eigen::RowMajor> > x0;
+        std::vector<Eigen::Matrix<double,Eigen::Dynamic,3,Eigen::RowMajor> > x1;
+        std::vector<Eigen::Matrix<unsigned int,Eigen::Dynamic,3,Eigen::RowMajor> > F;
+        std::vector<Eigen::AlignedBox<double,3> > box;
+    } obsdata;
+
+    virtual ~Collision() {}
+
+    // Updates the BVH with or without sorting
+    virtual void update_bvh(
+        const admmpd::Mesh *mesh,
+        const admmpd::Options *options,
+        admmpd::SolverData *data,
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1,
+        bool sort) = 0;
+
+    // Performs collision detection.
+    // Returns the number of active constraints.
+    virtual int detect(
+        const admmpd::Mesh *mesh,
+        const admmpd::Options *options,
+        const admmpd::SolverData *data,
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1) = 0;
+
+    // Appends the per-vertex graph of dependencies
+    // for constraints (ignores obstacles).
+    virtual void graph(
+        const admmpd::Mesh *mesh,
+        std::vector<std::set<int> > &g) = 0;
+
+    // Updates the collision obstacles. If the
+    // obstacles are new or have moved, the SDF
+    // is recomputed on the next call to detect(...)
+    virtual bool set_obstacles(
+        std::vector<Eigen::MatrixXd> &v0,
+        std::vector<Eigen::MatrixXd> &v1,
+        std::vector<Eigen::MatrixXi> &F,
+        std::string *err=nullptr);
+
+    // Linearizes active collision pairs about x
+    // for the constraint Cx=d
+    virtual void linearize(
+        const admmpd::Mesh *mesh,
+        const Options *options,
+        const admmpd::SolverData *data,
+        const Eigen::MatrixXd *x,
+    	std::vector<Eigen::Triplet<double> > *trips,
+		std::vector<double> *d) const = 0;
+
+    // Given a point and a mesh, perform
+    // discrete collision detection.
+    virtual std::pair<bool,VFCollisionPair>
+    detect_against_obs(
+        const admmpd::Mesh *mesh,
+        const admmpd::Options *options,
+        const admmpd::SolverData *data,
+        const Eigen::Vector3d &pt_t0,
+        const Eigen::Vector3d &pt_t1,
+        const ObstacleData *obs) const;
+
+    // Perform self collision detection
+    virtual std::pair<bool,VFCollisionPair>
+    detect_against_self(
+        const admmpd::Mesh *mesh,
+        const admmpd::Options *options,
+        const admmpd::SolverData *data,
+        int pt_idx,
+        const Eigen::Vector3d &pt_t0,
+        const Eigen::Vector3d &pt_t1,
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1) const = 0;
+};
+
+// Collision detection against multiple meshes
+class EmbeddedMeshCollision : public Collision {
+public:
+    // Performs collision detection and stores pairs
+    int detect(
+        const admmpd::Mesh *mesh,
+        const admmpd::Options *options,
+        const admmpd::SolverData *data,
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1);
+
+    void graph(
+        const admmpd::Mesh *mesh,
+        std::vector<std::set<int> > &g);
+
+    // Linearizes the collision pairs about x
+    // for the constraint Cx=d
+    void linearize(
+        const admmpd::Mesh *mesh,
+        const admmpd::Options *options,
+        const admmpd::SolverData *data,
+        const Eigen::MatrixXd *x,
+    	std::vector<Eigen::Triplet<double> > *trips,
+		std::vector<double> *d) const;
+
+    // Updates the tetmesh BVH for self collisions.
+    void update_bvh(
+        const admmpd::Mesh *mesh,
+        const admmpd::Options *options,
+        admmpd::SolverData *data,
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1,
+        bool sort = false);
+
+protected:
+    // Pairs are compute on detect and considered temporary.
+    std::vector<Eigen::Vector2i> vf_pairs; // index into per_vertex_pairs
+    std::vector<std::vector<VFCollisionPair> > per_vertex_pairs;
+
+    std::pair<bool,VFCollisionPair>
+    detect_against_self(
+        const admmpd::Mesh *mesh,
+        const admmpd::Options *options,
+        const admmpd::SolverData *data,
+        int pt_idx,
+        const Eigen::Vector3d &pt_t0,
+        const Eigen::Vector3d &pt_t1,
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1) const;
+};
+
+} // namespace admmpd
+
+#endif // ADMMPD_COLLISION_H_

extern/softbody/src/admmpd_energy.cpp

@@ -0,0 +1,444 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#include "admmpd_energy.h"
+#include "admmpd_types.h"
+#include "svd/ImplicitQRSVD.h"
+#include <iostream>
+#include <Eigen/Eigenvalues> 
+
+namespace admmpd {
+using namespace Eigen;
+
+Lame::Lame()
+{
+	set_from_youngs_poisson(10000000,0.399);
+}
+
+void Lame::set_from_youngs_poisson(double youngs, double poisson)
+{
+	m_mu = youngs/(2.0*(1.0+poisson));
+	m_lambda = youngs*poisson/((1.0+poisson)*(1.0-2.0*poisson));
+	m_bulk_mod = m_lambda + (2.0/3.0)*m_mu;
+}
+
+void EnergyTerm::signed_svd(
+	const Eigen::Matrix<double,3,3>& A, 
+	Eigen::Matrix<double,3,3> &U, 
+	Eigen::Matrix<double,3,1> &S, 
+	Eigen::Matrix<double,3,3> &V)
+{
+    JIXIE::singularValueDecomposition(A,U,S,V);
+	// Should replace this with 
+//	JacobiSVD<Matrix3d> svd(A, ComputeFullU | ComputeFullV);
+//	S = svd.singularValues();
+//	U = svd.matrixU();
+//	V = svd.matrixV();
+//	Matrix3d J = Matrix3d::Identity();
+//	J(2,2) = -1.0;
+//	if (U.determinant() < 0.0)
+//	{
+//		U = U * J;
+//		S[2] = -S[2];
+//	}
+//	if (V.determinant() < 0.0)
+//	{
+//		Matrix3d Vt = V.transpose();
+//		Vt = J * Vt;
+//		V = Vt.transpose();
+//		S[2] = -S[2];
+//	}
+}
+
+void EnergyTerm::update(
+	const Options *options,
+	int index,
+	int energyterm_type,
+	double rest_volume,
+	double weight,
+	const Eigen::MatrixXd *x,
+	const Eigen::MatrixXd *Dx,
+	Eigen::MatrixXd *z,
+	Eigen::MatrixXd *u)
+{
+	switch (energyterm_type)
+	{
+		default: break;
+		case ENERGYTERM_TET: {
+			update_tet(options,index,rest_volume,weight,x,Dx,z,u);
+		} break;
+		case ENERGYTERM_TRIANGLE: {
+			update_tri(options,index,rest_volume,weight,x,Dx,z,u);
+		} break;
+	}
+} // end EnergyTerm::update
+
+void EnergyTerm::update_tet(
+	const Options *options,
+	int index,
+	double rest_volume,
+	double weight,
+	const Eigen::MatrixXd *x,
+	const Eigen::MatrixXd *Dx,
+	Eigen::MatrixXd *z,
+	Eigen::MatrixXd *u)
+{
+	Lame lame;
+	lame.set_from_youngs_poisson(options->youngs,options->poisson);
+
+	(void)(x);
+	Matrix3d Dix = Dx->block<3,3>(index,0);
+	Matrix3d ui = u->block<3,3>(index,0);
+	Matrix3d zi = Dix + ui;
+
+	Matrix3d U, V;
+	Vector3d S;
+	signed_svd(zi, U, S, V);
+	const Vector3d s0 = S;
+
+	switch (options->elastic_material)
+	{
+		default:
+		case ELASTIC_ARAP:
+		{
+			S = Vector3d::Ones();
+			double k = lame.m_bulk_mod;
+			double kv = k * rest_volume;
+			double w2 = weight*weight;
+			Matrix3d p = U * S.asDiagonal() * V.transpose();
+			zi = (kv*p + w2*zi) / (w2 + kv);
+		} break;
+		case ELASTIC_NH:
+		{
+			solve_prox(options,index,lame,s0,S);
+			zi = U * S.asDiagonal() * V.transpose();
+		} break;
+	}
+
+	ui += (Dix - zi);
+	u->block<3,3>(index,0) = ui;
+	z->block<3,3>(index,0) = zi;
+
+} // end EnergyTerm::update
+
+void EnergyTerm::update_tri(
+	const Options *options,
+	int index,
+	double rest_area,
+	double weight,
+	const Eigen::MatrixXd *x,
+	const Eigen::MatrixXd *Dx,
+	Eigen::MatrixXd *z,
+	Eigen::MatrixXd *u)
+{
+	Lame lame;
+	lame.set_from_youngs_poisson(options->youngs,options->poisson);
+	Vector2d limit = options->strain_limit;
+	limit[0] = std::min(limit[0], 1.0);
+	limit[1] = std::max(limit[1], 1.0);
+
+	(void)(options);
+	typedef Matrix<double,2,3> Matrix23d;
+	// For we'll assume ARAP energy. If we need nonlinear energies
+	// in the future that's totally doable.
+	(void)(x);
+	Matrix23d Dix = Dx->block<2,3>(index,0);
+	Matrix23d ui = u->block<2,3>(index,0);
+	Matrix<double,3,2> zi_T = (Dix + ui).transpose();
+	JacobiSVD<Matrix<double,3,2> > svd(zi_T, ComputeFullU | ComputeFullV);
+
+	Matrix<double,3,2> S = Matrix<double,3,2>::Zero();
+	S(0,0)=1; S(1,1)=1;
+	Matrix<double,3,2> p = svd.matrixU() * S * svd.matrixV().transpose();
+	double k = lame.m_bulk_mod;
+	double kv = k * rest_area;
+	double w2 = weight*weight;
+	zi_T = (kv*p + w2*zi_T) / (w2 + kv);
+
+	// Apply strain limiting
+	bool check_strain = limit[0] > 0.0 || limit[1] < 99.0;
+	if (check_strain)
+	{
+		double l_col0 = zi_T.col(0).norm();
+		double l_col1 = zi_T.col(1).norm();
+		if (l_col0 < limit[0]) { zi_T.col(0) *= ( limit[0]/l_col0 ); }
+		if (l_col1 < limit[0]) { zi_T.col(1) *= ( limit[0]/l_col1 ); }
+		if (l_col0 > limit[1]) { zi_T.col(0) *= ( limit[1]/l_col0 ); }
+		if (l_col1 > limit[1]) { zi_T.col(1) *= ( limit[1]/l_col1 ); }
+	}
+
+	ui += (Dix - zi_T.transpose());
+	u->block<2,3>(index,0) = ui;
+	z->block<2,3>(index,0) = zi_T.transpose();
+}
+
+int EnergyTerm::init_tet(
+	const Options *options,
+	int index,
+	const Eigen::Matrix<int,1,4> &prim,
+	const Eigen::MatrixXd *x,
+	double &volume,
+	double &weight,
+	std::vector< Eigen::Triplet<double> > &triplets )
+{
+	Lame lame;
+	lame.set_from_youngs_poisson(options->youngs,options->poisson);
+
+	Matrix<double,3,3> edges;
+	edges.col(0) = x->row(prim[1]) - x->row(prim[0]);
+	edges.col(1) = x->row(prim[2]) - x->row(prim[0]);
+	edges.col(2) = x->row(prim[3]) - x->row(prim[0]);
+	Matrix<double,3,3> edges_inv = edges.inverse();
+	volume = edges.determinant() / 6.0f;
+	if (volume < 0)
+	{
+		printf("**admmpd::EnergyTerm Error: Inverted initial tet: %f\n",volume);
+		return 0;
+	}
+	double k = lame.m_bulk_mod;
+	weight = std::sqrt(k*volume);
+	Matrix<double,4,3> S = Matrix<double,4,3>::Zero();
+	S(0,0) = -1; S(0,1) = -1; S(0,2) = -1;
+	S(1,0) =  1; S(2,1) =  1; S(3,2) =  1;
+	Eigen::Matrix<double,3,4> D = (S * edges_inv).transpose();
+	int rows[3] = { index+0, index+1, index+2 };
+	int cols[4] = { prim[0], prim[1], prim[2], prim[3] };
+	for( int r=0; r<3; ++r )
+	{
+		for( int c=0; c<4; ++c )
+		{
+			triplets.emplace_back(rows[r], cols[c], D(r,c));
+		}
+	}
+	return 3;
+}
+
+int EnergyTerm::init_triangle(
+	const Options *options,
+	int index,
+	const Eigen::RowVector3i &prim,
+	const Eigen::MatrixXd *x,
+	double &area,
+	double &weight,
+	std::vector< Eigen::Triplet<double> > &triplets)
+{
+	Lame lame;
+	lame.set_from_youngs_poisson(options->youngs,options->poisson);
+
+	Matrix<double,3,2> edges;
+	edges.col(0) = x->row(prim[1]) - x->row(prim[0]);
+	edges.col(1) = x->row(prim[2]) - x->row(prim[0]);
+	Matrix<double,3,2> basis;
+	basis.col(0) = edges.col(0).normalized();
+	double dot = edges.col(1).dot(basis.col(0));
+	basis.col(1) = (edges.col(1)-dot*basis.col(0)).normalized();
+	Matrix<double,2,2> rest_pose = (basis.transpose() * edges).inverse();
+	area = (basis.transpose() * edges).determinant() / 2.0f;
+	if (area < 0)
+	{
+		printf("**admmpd::TriEnergyTerm Error: Inverted initial tri: %f\n",area);
+		return 0;
+	}
+	double k = lame.m_bulk_mod;
+	weight = std::sqrt(k*area);
+	Matrix<double,3,2> S = Matrix<double,3,2>::Zero();
+	S(0,0) = -1; S(0,1) = -1;
+	S(1,0) =  1; S(2,1) =  1;
+	Eigen::Matrix<double,2,3> D = (S * rest_pose).transpose();
+	int rows[2] = { index+0, index+1 };
+	int cols[3] = { prim[0], prim[1], prim[2] };
+	for (int r=0; r<2; ++r)
+	{
+		for (int c=0; c<3; ++c)
+		{
+			triplets.emplace_back(rows[r], cols[c], D(r,c));
+		}
+	}
+	return 2;
+}
+
+void EnergyTerm::solve_prox(
+	const Options *options,
+	int index,
+	const Lame &lame,
+	const Eigen::Vector3d &s0,
+	Eigen::Vector3d &s)
+{
+	(void)(index);
+
+	// Always start prox solve at positive singular vals
+	if( s[2] < 0.0 ){ s[2] *= -1.0; }
+//	s = Vector3d::Ones();
+
+	Vector3d g; // gradient
+	Vector3d p; // descent
+	Vector3d s_prev;
+	Matrix3d H = Matrix3d::Identity();
+	double energy_k, energy_k1;
+	const bool add_admm_pen = true;
+	const double eps = 1e-6;
+	const int max_ls_iter = 20;
+
+	int iter = 0;
+	for(; iter<10; ++iter)
+	{
+		g.setZero();
+		energy_k = energy_density(options,lame,add_admm_pen,s0,s,&g); // e and g
+
+		// Converged because low gradient
+		if (g.norm() < eps)
+			break;
+
+		hessian_spd(options,lame,add_admm_pen,s,H);
+		p = H.ldlt().solve(-g); // Newton step direction
+
+		s_prev = s;
+		s = s_prev + p;
+		energy_k1 = energy_density(options,lame,add_admm_pen,s0,s);
+
+		// Backtracking line search
+		double alpha = 1;
+		int ls_iter = 0;
+		while(energy_k1>energy_k && ls_iter<max_ls_iter)
+		{
+			alpha *= 0.5;
+			s = s_prev + alpha*p;
+			energy_k1 = energy_density(options,lame,add_admm_pen,s0,s);
+			ls_iter++;
+		}
+
+		// Sometimes flattened tets will have a hard time
+		// uninverting, in which case they get linesearch
+		// blocked. There are ways around this, but for now
+		// simply quitting is fine. The global step will smooth
+		// things over and get us in a better state next time.
+		if (ls_iter>=max_ls_iter)
+		{
+			s = s_prev;
+			break;
+		}
+
+		// Stopped making progress
+		if ((s-s_prev).norm() < eps)
+			break;
+
+	} // end Newton iterations
+
+	if (std::isnan(s[0]) || std::isnan(s[1]) || std::isnan(s[2]))
+		throw std::runtime_error("*EnergyTerm::solve_prox: Got nans");
+
+} // end solve prox
+
+double EnergyTerm::energy_density(
+	const Options *options,
+	const Lame &lame,
+	bool add_admm_penalty,
+	const Eigen::Vector3d &s0,
+	const Eigen::Vector3d &s,
+	Eigen::Vector3d *g)
+{
+	double e = 0;
+
+	// Compute energy and gradient
+	// https://github.com/mattoverby/admm-elastic/blob/master/src/TetEnergyTerm.cpp
+	// I suppose I should add ARAP for completeness even though it's not used
+	switch (options->elastic_material)
+	{
+		default: {
+			if (g != nullptr) g->setZero();
+		} break;
+
+		case ELASTIC_NH: {
+			if (s.minCoeff() <= 0)
+			{ // barrier energy to prevent inversions
+				if (g != nullptr) g->setZero();
+				return std::numeric_limits<float>::max();
+			}
+			double J = s.prod();
+			double I_1 = s.dot(s);
+			double log_I3 = std::log(J*J);
+			double t1 = 0.5 * lame.m_mu * (I_1-log_I3-3.0);
+			double t2 = 0.125 * lame.m_lambda * log_I3 * log_I3;
+			e = t1 + t2;
+			if (g != nullptr)
+			{
+				Vector3d s_inv = s.cwiseInverse();
+				*g = lame.m_mu*(s-s_inv) + lame.m_lambda*std::log(J)*s_inv;
+			}
+		} break;
+	}
+
+	if (add_admm_penalty)
+	{
+		const double &k = lame.m_bulk_mod;
+		Vector3d s_min_s0 = s-s0;
+		e += (k*0.5) * s_min_s0.squaredNorm();
+		if (g != nullptr) *g = *g + k*s_min_s0;
+	}
+
+	return e;
+
+} // end energy
+
+void EnergyTerm::hessian_spd(
+	const Options *options,
+	const Lame &lame,
+	bool add_admm_penalty,
+	const Eigen::Vector3d &s,
+	Eigen::Matrix3d &H)
+{
+	static const Matrix3d I = Matrix3d::Identity();
+
+	// Compute specific Hessian
+	switch (options->elastic_material)
+	{
+		default:
+		{
+			H.setIdentity(); // We don't use ARAP hessian
+		} break;
+
+		case ELASTIC_NH:
+		{
+			double J = s.prod();
+			Vector3d s_inv = s.cwiseInverse();
+			Matrix3d P = Matrix3d::Zero();
+			P(0,0) = 1.0 / (s[0]*s[0]);
+			P(1,1) = 1.0 / (s[1]*s[1]);
+			P(2,2) = 1.0 / (s[2]*s[2]);
+			H =	lame.m_mu*(I - 2.0*P) +
+				lame.m_lambda * std::log(J) * P +
+				lame.m_lambda * s_inv * s_inv.transpose();
+		} break;
+	}
+
+	// ADMM penalty
+	if(add_admm_penalty)
+	{
+		const double &k = lame.m_bulk_mod;
+		for (int i=0; i<3; ++i)
+			H(i,i) += k;
+	}
+
+	// Projects a matrix to nearest SPD
+	// https://github.com/penn-graphics-research/DOT/blob/master/src/Utils/IglUtils.hpp
+	auto project_spd = [](Matrix3d &H_)
+	{
+		SelfAdjointEigenSolver<Matrix3d> eigenSolver(H_);
+		if (eigenSolver.eigenvalues()[0] >= 0.0)
+			return;
+		Eigen::DiagonalMatrix<double,3> D(eigenSolver.eigenvalues());
+		for (int i = 0; i<3; ++i)
+		{
+			if (D.diagonal()[i] < 0)
+				D.diagonal()[i] = 0;
+			else
+				break;
+		}
+		H_ = eigenSolver.eigenvectors()*D*eigenSolver.eigenvectors().transpose();
+	};
+
+	project_spd(H);
+
+} // end hessian
+
+} // end namespace mcl

extern/softbody/src/admmpd_energy.h

@@ -0,0 +1,121 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_ENERGY_H_
+#define ADMMPD_ENERGY_H_ 1
+
+#include <Eigen/Sparse>
+#include <Eigen/Geometry>
+#include <vector>
+#include "admmpd_types.h"
+
+namespace admmpd {
+
+class Lame {
+public:
+	double m_mu;
+	double m_lambda;
+	double m_bulk_mod;
+	void set_from_youngs_poisson(double youngs, double poisson);
+	Lame();
+};
+
+class EnergyTerm {
+public:
+
+	void signed_svd(
+		const Eigen::Matrix<double,3,3> &A, 
+		Eigen::Matrix<double,3,3> &U, 
+		Eigen::Matrix<double,3,1> &S, 
+		Eigen::Matrix<double,3,3> &V);
+
+	// Updates the z and u variables for an element energy.
+	void update(
+		const Options *options,
+		int index,
+		int energyterm_type,
+		double rest_volume,
+		double weight,
+		const Eigen::MatrixXd *x,
+		const Eigen::MatrixXd *Dx,
+		Eigen::MatrixXd *z,
+		Eigen::MatrixXd *u);
+
+	// Updates the z and u variables for a tet
+	void update_tet(
+		const Options *options,
+		int index,
+		double rest_volume,
+		double weight,
+		const Eigen::MatrixXd *x,
+		const Eigen::MatrixXd *Dx,
+		Eigen::MatrixXd *z,
+		Eigen::MatrixXd *u);
+
+	// Updates the z and u variables for a tri
+	void update_tri(
+		const Options *options,
+		int index,
+		double rest_area,
+		double weight,
+		const Eigen::MatrixXd *x,
+		const Eigen::MatrixXd *Dx,
+		Eigen::MatrixXd *z,
+		Eigen::MatrixXd *u);
+
+	// Initializes tet energy, returns num rows for D
+	int init_tet(
+		const Options *options,
+		int index,
+		const Eigen::RowVector4i &prim,
+		const Eigen::MatrixXd *x,
+		double &volume,
+		double &weight,
+		std::vector< Eigen::Triplet<double> > &triplets);
+
+	// Initializes facet energy, returns num rows for D
+	int init_triangle(
+		const Options *options,
+		int index,
+		const Eigen::RowVector3i &prim,
+		const Eigen::MatrixXd *x,
+		double &area,
+		double &weight,
+		std::vector< Eigen::Triplet<double> > &triplets);
+
+	// Solves proximal energy function
+	void solve_prox(
+		const Options *options,
+		int index,
+		const Lame &lame,
+		const Eigen::Vector3d &s0,
+		Eigen::Vector3d &s);
+
+	// Returns gradient and energy (+ADMM penalty)
+	// of a material evaluated at s (singular values).
+	double energy_density(
+		const Options *options,
+		const Lame &lame,
+		bool add_admm_penalty,
+		const Eigen::Vector3d &s0,
+		const Eigen::Vector3d &s,
+		Eigen::Vector3d *g=nullptr);
+
+	// Returns the Hessian of a material at S
+	// projected to nearest SPD
+	void hessian_spd(
+		const Options *options,
+		const Lame &lame,
+		bool add_admm_penalty,
+		const Eigen::Vector3d &s,
+		Eigen::Matrix3d &H);
+
+};
+
+} // end namespace admmpd
+
+#endif // ADMMPD_ENERGY_H_
+
+
+
+

extern/softbody/src/admmpd_geom.cpp

@@ -0,0 +1,641 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#include "admmpd_geom.h"
+
+namespace admmpd {
+using namespace Eigen;
+
+template <typename T>
+Matrix<T,4,1> geom::point_tet_barys(
+	const Matrix<T,3,1> &p,
+	const Matrix<T,3,1> &a,
+	const Matrix<T,3,1> &b,
+	const Matrix<T,3,1> &c,
+	const Matrix<T,3,1> &d)
+{
+	typedef Matrix<T,3,1> VecType;
+	auto scalar_triple_product = [](
+		const VecType &u,
+		const VecType &v,
+		const VecType &w )
+	{
+		return u.dot(v.cross(w));
+	};
+	VecType ap = p - a;
+	VecType bp = p - b;
+	VecType ab = b - a;
+	VecType ac = c - a;
+	VecType ad = d - a;
+	VecType bc = c - b;
+	VecType bd = d - b;
+	T va6 = scalar_triple_product(bp, bd, bc);
+	T vb6 = scalar_triple_product(ap, ac, ad);
+	T vc6 = scalar_triple_product(ap, ad, ab);
+	T vd6 = scalar_triple_product(ap, ab, ac);
+	T v6 = 1.0 / scalar_triple_product(ab, ac, ad);
+	return Matrix<T,4,1>(va6*v6, vb6*v6, vc6*v6, vd6*v6);
+} // end point tet barycoords
+
+// Checks that it's on the "correct" side of the normal
+// for each face of the tet. Assumes winding points inward.
+template <typename T>
+bool geom::point_in_tet(
+	const Matrix<T,3,1> &p,
+	const Matrix<T,3,1> &a,
+	const Matrix<T,3,1> &b,
+	const Matrix<T,3,1> &c,
+	const Matrix<T,3,1> &d)
+{
+	auto check_face = [](
+		const Matrix<T,3,1> &point,
+		const Matrix<T,3,1> &p0,
+		const Matrix<T,3,1> &p1,
+		const Matrix<T,3,1> &p2,
+		const Matrix<T,3,1> &p3 )
+	{
+		Matrix<T,3,1> n = (p1-p0).cross(p2-p0);
+		double dp3 = n.dot(p3-p0);
+		double dp = n.dot(point-p0);
+		return (dp3*dp >= 0);
+	};
+	return
+		check_face(p, a, b, c, d) &&
+		check_face(p, b, c, d, a) &&
+		check_face(p, c, d, a, b) &&
+		check_face(p, d, a, b, c);
+}
+
+template <typename T>
+void geom::create_tets_from_box(
+    const Eigen::Matrix<T,3,1> &bmin,
+    const Eigen::Matrix<T,3,1> &bmax,
+    std::vector<Eigen::Matrix<T,3,1> > &verts,
+    std::vector<Eigen::RowVector4i> &tets)
+{
+	std::vector<Matrix<T,3,1>> v = {
+		// Top plane, clockwise looking down
+		bmax,
+		Matrix<T,3,1>(bmin[0], bmax[1], bmax[2]),
+		Matrix<T,3,1>(bmin[0], bmax[1], bmin[2]),
+		Matrix<T,3,1>(bmax[0], bmax[1], bmin[2]),
+		// Bottom plane, clockwise looking down
+		Matrix<T,3,1>(bmax[0], bmin[1], bmax[2]),
+		Matrix<T,3,1>(bmin[0], bmin[1], bmax[2]),
+		bmin,
+		Matrix<T,3,1>(bmax[0], bmin[1], bmin[2])
+	};
+	// Add vertices and get indices of the box
+	std::vector<int> b;
+	for(int i=0; i<8; ++i)
+	{
+		b.emplace_back(verts.size());
+		verts.emplace_back(v[i]);
+	}
+	// From the box, create five new tets
+	std::vector<RowVector4i> new_tets = {
+		RowVector4i( b[0], b[5], b[7], b[4] ),
+		RowVector4i( b[5], b[7], b[2], b[0] ),
+		RowVector4i( b[5], b[0], b[2], b[1] ),
+		RowVector4i( b[7], b[2], b[0], b[3] ),
+		RowVector4i( b[5], b[2], b[7], b[6] )
+	};
+	for(int i=0; i<5; ++i)
+		tets.emplace_back(new_tets[i]);
+}
+
+// From Real-Time Collision Detection by Christer Ericson
+template<typename T>
+Eigen::Matrix<T,3,1> geom::point_triangle_barys(
+	const Eigen::Matrix<T,3,1> &p,
+	const Eigen::Matrix<T,3,1> &a,
+	const Eigen::Matrix<T,3,1> &b,
+	const Eigen::Matrix<T,3,1> &c)
+{
+	Eigen::Matrix<T,3,1> v0 = b - a;
+	Eigen::Matrix<T,3,1> v1 = c - a;
+	Eigen::Matrix<T,3,1> v2 = p - a;
+	T d00 = v0.dot(v0);
+	T d01 = v0.dot(v1);
+	T d11 = v1.dot(v1);
+	T d20 = v2.dot(v0);
+	T d21 = v2.dot(v1);
+	T denom = d00 * d11 - d01 * d01;
+	if (std::abs(denom)<=0)
+		return Eigen::Matrix<T,3,1>::Zero();
+	Eigen::Matrix<T,3,1> r;
+	r[1] = (d11 * d20 - d01 * d21) / denom;
+	r[2] = (d00 * d21 - d01 * d20) / denom;
+	r[0] = 1.0 - r[1] - r[2];
+	return r;
+} // end point triangle barycoords
+
+// From Real-Time Collision Detection by Christer Ericson
+template<typename T>
+Eigen::Matrix<T,3,1> geom::point_on_triangle_ce(
+		const Eigen::Matrix<T,3,1> &p,
+		const Eigen::Matrix<T,3,1> &a,
+		const Eigen::Matrix<T,3,1> &b,
+		const Eigen::Matrix<T,3,1> &c)
+{
+	typedef Eigen::Matrix<T,3,1> VecType;
+	auto Dot = [](const VecType &v0, const VecType &v1)
+	{
+		return v0.dot(v1);
+	};
+	auto Cross = [](const VecType &v0, const VecType &v1)
+	{
+		return v0.cross(v1);
+	};
+	VecType ab = b - a;
+	VecType ac = c - a;
+	VecType bc = c - b;
+	// Compute parametric position s for projection P’ of P on AB,
+	// P’ = A + s*AB, s = snom/(snom+sdenom)
+	T snom = Dot(p - a, ab), sdenom = Dot(p - b, a - b);
+	// Compute parametric position t for projection P’ of P on AC,
+	// P’ = A + t*AC, s = tnom/(tnom+tdenom)
+	T tnom = Dot(p - a, ac), tdenom = Dot(p - c, a - c);
+	if (snom <= 0.0f && tnom <= 0.0f) return a;
+	// Vertex region early out
+	// Compute parametric position u for projection P’ of P on BC,
+	// P’ = B + u*BC, u = unom/(unom+udenom)
+	T unom = Dot(p - b, bc), udenom = Dot(p - c, b - c);
+	if (sdenom <= 0.0f && unom <= 0.0f) return b; // Vertex region early out
+	if (tdenom <= 0.0f && udenom <= 0.0f) return c; // Vertex region early out
+	// P is outside (or on) AB if the triple scalar product [N PA PB] <= 0
+	VecType n = Cross(b - a, c - a);
+	T vc = Dot(n, Cross(a - p, b - p));
+	// If P outside AB and within feature region of AB,
+	// return projection of P onto AB
+	if (vc <= 0.0f && snom >= 0.0f && sdenom >= 0.0f)
+	return a + snom / (snom + sdenom) * ab;
+	// P is outside (or on) BC if the triple scalar product [N PB PC] <= 0
+	T va = Dot(n, Cross(b - p, c - p));
+	// If P outside BC and within feature region of BC,
+	// return projection of P onto BC
+	if (va <= 0.0f && unom >= 0.0f && udenom >= 0.0f)
+	return b + unom / (unom + udenom) * bc;
+	// P is outside (or on) CA if the triple scalar product [N PC PA] <= 0
+	T vb = Dot(n, Cross(c - p, a - p));
+	// If P outside CA and within feature region of CA,
+	// return projection of P onto CA
+	if (vb <= 0.0f && tnom >= 0.0f && tdenom >= 0.0f)
+	return a + tnom / (tnom + tdenom) * ac;
+	// P must project inside face region. Compute Q using barycentric coordinates
+	T u = va / (va + vb + vc);
+	T v = vb / (va + vb + vc);
+	T w = 1.0f - u - v; // = vc / (va + vb + vc)
+	return u * a + v * b + w * c;
+}
+
+// https://github.com/mattoverby/mclscene/blob/master/include/MCL/Projection.hpp
+template<typename T>
+Eigen::Matrix<T,3,1> geom::point_on_triangle(
+		const Eigen::Matrix<T,3,1> &point,
+		const Eigen::Matrix<T,3,1> &p1,
+		const Eigen::Matrix<T,3,1> &p2,
+		const Eigen::Matrix<T,3,1> &p3)
+{
+	typedef Matrix<T,3,1> VecType;
+	auto myclamp = [](const T &x){ return x<0 ? 0 : (x>1 ? 1 : x); };
+
+	VecType edge0 = p2 - p1;
+	VecType edge1 = p3 - p1;
+	VecType v0 = p1 - point;
+	T a = edge0.dot( edge0 );
+	T b = edge0.dot( edge1 );
+	T c = edge1.dot( edge1 );
+	T d = edge0.dot( v0 );
+	T e = edge1.dot( v0 );
+	T det = a*c - b*b;
+	T s = b*e - c*d;
+	T t = b*d - a*e;
+
+	const T zero(0);
+	const T one(1);
+	if ( s + t < det ) {
+		if ( s < zero ) {
+		    if ( t < zero ) {
+				if ( d < zero ) {
+					s = myclamp( -d/a );
+					t = zero;
+				}
+				else {
+					s = zero;
+					t = myclamp( -e/c );
+				}
+			}
+			else {
+				s = zero;
+				t = myclamp( -e/c );
+		    }
+		}
+		else if ( t < zero ) {
+		    s = myclamp( -d/a );
+		    t = zero;
+		}
+		else {
+		    T invDet = one / det;
+		    s *= invDet;
+		    t *= invDet;
+		}
+	}
+	else {
+		if ( s < zero ) {
+		    T tmp0 = b+d;
+		    T tmp1 = c+e;
+		    if ( tmp1 > tmp0 ) {
+				T numer = tmp1 - tmp0;
+				T denom = a-T(2)*b+c;
+				s = myclamp( numer/denom );
+				t = one-s;
+		    }
+		    else {
+				t = myclamp( -e/c );
+				s = zero;
+		    }
+		}
+		else if ( t < zero ) {
+		    if ( a+d > b+e ) {
+				T numer = c+e-b-d;
+				T denom = a-T(2)*b+c;
+				s = myclamp( numer/denom );
+				t = one-s;
+		    }
+		    else {
+				s = myclamp( -e/c );
+				t = zero;
+		    }
+		}
+		else {
+		    T numer = c+e-b-d;
+		    T denom = a-T(2)*b+c;
+		    s = myclamp( numer/denom );
+		    t = one - s;
+		}
+	}
+
+	return ( p1 + edge0*s + edge1*t );
+}
+
+// From Real-Time Collision Detection by Christer Ericson
+template<typename T>
+bool geom::aabb_plane_intersect(
+		const Eigen::Matrix<T,3,1> &bmin,
+		const Eigen::Matrix<T,3,1> &bmax,
+		const Eigen::Matrix<T,3,1> &p, // pt on plane
+		const Eigen::Matrix<T,3,1> &n) // normal
+{
+	typedef Eigen::Matrix<T,3,1> VecType;
+	T d = p.dot(n);
+	// These two lines not necessary with a (center, extents) AABB representation
+	VecType c = (bmax+bmin)*0.5; // Compute AABB center
+	VecType e = bmax-c; // Compute positive extents
+	// Compute the projection interval radius of b onto L(t) = b.c + t * p.n
+	T r = e[0]*std::abs(n[0])+e[1]*std::abs(n[1])+e[2]*std::abs(n[2]);
+	// Compute distance of box center from plane
+	T s = n.dot(c)-d;
+	// Intersection occurs when distance s falls within [-r,+r] interval
+	return std::abs(s) <= r;
+}
+
+// From Real-Time Collision Detection by Christer Ericson
+template<typename T>
+bool geom::aabb_triangle_intersect(
+    const Eigen::Matrix<T,3,1> &bmin,
+    const Eigen::Matrix<T,3,1> &bmax,
+    const Eigen::Matrix<T,3,1> &a,
+    const Eigen::Matrix<T,3,1> &b,
+    const Eigen::Matrix<T,3,1> &c)
+{
+	Eigen::AlignedBox<T,3> box;
+	box.extend(bmin);
+	box.extend(bmax);
+	if (box.contains(a))
+		return true;
+	if (box.contains(b))
+		return true;
+	if (box.contains(c))
+		return true;
+
+	typedef Eigen::Matrix<T,3,1> VecType;
+	auto Max = [](T x, T y, T z){ return std::max(std::max(x,y),z); };
+	auto Min = [](T x, T y, T z){ return std::min(std::min(x,y),z); };
+    T p0, p1, p2, r, minp, maxp;
+    // Compute box center and extents (if not already given in that format)
+    VecType cent = (bmin+bmax)*0.5;
+    T e0 = (bmax[0]-bmin[0])*0.5;
+    T e1 = (bmax[1]-bmin[1])*0.5;
+    T e2 = (bmax[2]-bmin[2])*0.5;
+    // Translate triangle as conceptually moving AABB to origin
+    VecType v0 = a-cent;
+    VecType v1 = b-cent;
+    VecType v2 = c-cent;
+	const VecType box_normals[3] = { VecType(1,0,0), VecType(0,1,0), VecType(0,0,1) };
+    // Compute edge vectors for triangle
+	const VecType edge_vectors[3] = { v1-v0, v2-v1, v0-v2 }; // f0,f1,f2
+    // Test axes a00..a22 (category 3)
+	VecType aij;
+	for (int i=0; i<3; ++i)
+	{
+		const VecType &u = box_normals[i];
+		for (int j=0; j<3; ++j)
+		{
+			const VecType &f = edge_vectors[j];
+			aij = u.cross(f);
+			// Axis is separating axis
+			p0 = v0.dot(aij);
+			p1 = v1.dot(aij);
+			p2 = v2.dot(aij);
+			r = e0*std::abs(box_normals[0].dot(aij)) +
+				e1*std::abs(box_normals[1].dot(aij)) +
+				e2*std::abs(box_normals[2].dot(aij));
+			minp = Min(p0, p1, p2);
+			maxp = Max(p0, p1, p2);
+			if (maxp < -r || minp > r)
+				return false;
+		}
+	}
+	// Test the three axes corresponding to the face normals of AABB b (category 1).
+    // Exit if...
+    // ... [-e0, e0] and [min(v0[0],v1[0],v2[0]), max(v0[0],v1[0],v2[0])] do not overlap
+    if (Max(v0[0], v1[0], v2[0]) < -e0 || Min(v0[0], v1[0], v2[0]) > e0) return false;
+    // ... [-e1, e1] and [min(v0[1],v1[1],v2[1]), max(v0[1],v1[1],v2[1])] do not overlap
+    if (Max(v0[1], v1[1], v2[1]) < -e1 || Min(v0[1], v1[1], v2[1]) > e1) return false;
+    // ... [-e2, e2] and [min(v0[2],v1[2],v2[2]), max(v0[2],v1[2],v2[2])] do not overlap
+    if (Max(v0[2], v1[2], v2[2]) < -e2 || Min(v0[2], v1[2], v2[2]) > e2) return false;
+    // Test separating axis corresponding to triangle face normal (category 2)
+	VecType n = edge_vectors[0].cross(edge_vectors[1]);
+	T dot = v0.dot(n);
+	// These two lines not necessary with a (center, extents) AABB representation
+	VecType e = bmax-cent; // Compute positive extents
+	// Compute the projection interval radius of b onto L(t) = b.c + t * p.n
+	r = e[0]*std::abs(n[0])+e[1]*std::abs(n[1])+e[2]*std::abs(n[2]);
+	// Compute distance of box center from plane
+	T s = n.dot(cent)-dot;
+	// Intersection occurs when distance s falls within [-r,+r] interval
+	return std::abs(s) <= r;
+}
+
+// https://people.csail.mit.edu/amy/papers/box-jgt.pdf
+template<typename T>
+bool geom::ray_aabb(
+		const Eigen::Matrix<T,3,1> &o,
+		const Eigen::Matrix<T,3,1> &d,
+		const Eigen::AlignedBox<T,3> &aabb,
+		T t_min, T t_max)
+{
+	if (aabb.contains(o))
+		return true;
+	const Matrix<T,3,1> &bmin = aabb.min();
+	const Matrix<T,3,1> &bmax = aabb.max();
+	T tmin, tymin, tzmin;
+	T tmax, tymax, tzmax;
+	if (d[0] >= 0)
+	{
+		tmin = (bmin[0] - o[0]) / d[0];
+		tmax = (bmax[0] - o[0]) / d[0];
+	}
+	else
+	{
+		tmin = (bmax[0] - o[0]) / d[0];
+		tmax = (bmin[0] - o[0]) / d[0];
+	}
+	if (d[1] >= 0)
+	{
+		tymin = (bmin[1] - o[1]) / d[1];
+		tymax = (bmax[1] - o[1]) / d[1];
+	}
+	else
+	{
+		tymin = (bmax[1] - o[1]) / d[1];
+		tymax = (bmin[1] - o[1]) / d[1];
+	}
+	if ( (tmin > tymax) || (tymin > tmax) )
+		return false;
+	if (tymin > tmin)
+		tmin = tymin;
+	if (tymax < tmax)
+		tmax = tymax;
+	if (d[2] >= 0)
+	{
+		tzmin = (bmin[2] - o[2]) / d[2];
+		tzmax = (bmax[2] - o[2]) / d[2];
+	}
+	else
+	{
+		tzmin = (bmax[2] - o[2]) / d[2];
+		tzmax = (bmin[2] - o[2]) / d[2];
+	}
+	if ( (tmin > tzmax) || (tzmin > tmax) )
+		return false;
+	if (tzmin > tmin)
+		tmin = tzmin;
+	if (tzmax < tmax)
+		tmax = tzmax;
+	return ( (tmin < t_max) && (tmax > t_min) );
+} // end ray - aabb test
+
+template<typename T>
+bool geom::ray_triangle(
+		const Eigen::Matrix<T,3,1> &o,
+		const Eigen::Matrix<T,3,1> &d,
+		const Eigen::Matrix<T,3,1> &p0,
+		const Eigen::Matrix<T,3,1> &p1,
+		const Eigen::Matrix<T,3,1> &p2,
+		T t_min,
+		T &t_max,
+		Eigen::Matrix<T,3,1> *bary)
+{
+	typedef Eigen::Matrix<T,3,1> VecType;
+	VecType e0 = p1 - p0;
+	VecType e1 = p0 - p2;
+	VecType n = e1.cross( e0 );
+	VecType e2 = (p0-o) / (n.dot(d));
+	T t = n.dot(e2);
+	if (t > t_max)
+		return false;
+	if (t < t_min)
+		return false;
+	if (bary != nullptr)
+	{
+		VecType i  = d.cross(e2);
+		T beta = i.dot(e1);
+		T gamma = i.dot(e0);
+		*bary = VecType(1.0-beta-gamma, beta, gamma);
+		const T eps = 1e-8;
+		if (bary->sum()>1+eps)
+			return false;
+	}
+	t_max = t;
+	return true;
+} // end ray - triangle test
+
+template <typename T>
+void geom::merge_close_vertices(
+	std::vector<Matrix<T,3,1> > &verts,
+	std::vector<RowVector4i> &tets,
+	T eps)
+{
+	int nv = verts.size();
+	std::vector<Matrix<T,3,1> > new_v(nv); // new verts
+	std::vector<int> idx(nv,0); // index mapping
+	std::vector<int> visited(nv,0);
+	int curr_idx = 0;
+	for (int i=0; i<nv; ++i)
+	{
+		if(!visited[i])
+		{
+			new_v[curr_idx] = verts[i];
+			visited[i] = 1;
+			idx[i] = curr_idx;
+			for (int j = i+1; j<nv; ++j)
+			{
+				if((verts[j]-verts[i]).norm() < eps)
+				{
+					visited[j] = 1;
+					idx[j] = curr_idx;
+				}
+			}
+			curr_idx++;
+		}
+	}
+	new_v.resize(curr_idx);
+	verts = new_v;
+	int nt = tets.size();
+	for (int i=0; i<nt; ++i)
+	{
+		for (int j=0; j<4; ++j)
+		{
+			tets[i][j] = idx[tets[i][j]];
+		}
+	}
+}
+
+template<typename T>
+void geom::make_n3(
+		const Eigen::SparseMatrix<T> &A,
+		Eigen::SparseMatrix<T> &A3)
+{
+	int na = A.rows();
+	A3.resize(na*3, na*3);
+	A3.reserve(A.nonZeros()*3);
+	int cols = A.rows();
+	for(int c=0; c<cols; ++c)
+	{
+		for(int dim=0; dim<3; ++dim)
+		{
+			int col = c*3+dim;
+			A3.startVec(col);
+			for(typename SparseMatrix<T>::InnerIterator itA(A,c); itA; ++itA)
+				A3.insertBack((itA.row()*3+dim), col) = itA.value();
+		}
+	}
+	A3.finalize();
+	A3.makeCompressed();
+}
+
+//
+// Compile template types
+//
+template Eigen::Matrix<double,4,1>
+	admmpd::geom::point_tet_barys<double>(
+	const Eigen::Matrix<double,3,1>&,
+	const Eigen::Matrix<double,3,1>&, const Eigen::Matrix<double,3,1>&,
+	const Eigen::Matrix<double,3,1>&, const Eigen::Matrix<double,3,1>&);
+template Eigen::Matrix<float,4,1>
+	admmpd::geom::point_tet_barys<float>(
+	const Eigen::Vector3f&,
+	const Eigen::Vector3f&, const Eigen::Vector3f&,
+	const Eigen::Vector3f&, const Eigen::Vector3f&);
+template bool admmpd::geom::point_in_tet<double>(
+	const Matrix<double,3,1>&,
+	const Matrix<double,3,1>&,
+	const Matrix<double,3,1>&,
+	const Matrix<double,3,1>&,
+	const Matrix<double,3,1>&);
+template bool admmpd::geom::point_in_tet<float>(
+	const Matrix<float,3,1>&,
+	const Matrix<float,3,1>&,
+	const Matrix<float,3,1>&,
+	const Matrix<float,3,1>&,
+	const Matrix<float,3,1>&);
+template void geom::create_tets_from_box<double>(
+    const Eigen::Matrix<double,3,1>&,
+    const Eigen::Matrix<double,3,1>&,
+    std::vector<Eigen::Matrix<double,3,1> >&,
+    std::vector<Eigen::RowVector4i>&);
+template void geom::create_tets_from_box<float>(
+    const Eigen::Matrix<float,3,1>&,
+    const Eigen::Matrix<float,3,1>&,
+    std::vector<Eigen::Matrix<float,3,1> >&,
+    std::vector<Eigen::RowVector4i>&);
+template Eigen::Matrix<double,3,1>
+	admmpd::geom::point_triangle_barys<double>(
+	const Eigen::Matrix<double,3,1>&, const Eigen::Matrix<double,3,1>&,
+	const Eigen::Matrix<double,3,1>&, const Eigen::Matrix<double,3,1>&);
+template Eigen::Matrix<float,3,1>
+	admmpd::geom::point_triangle_barys<float>(
+	const Eigen::Vector3f&, const Eigen::Vector3f&,
+	const Eigen::Vector3f&, const Eigen::Vector3f&);
+template Eigen::Matrix<double,3,1>
+	admmpd::geom::point_on_triangle<double>(
+	const Eigen::Matrix<double,3,1>&, const Eigen::Matrix<double,3,1>&,
+	const Eigen::Matrix<double,3,1>&, const Eigen::Matrix<double,3,1>&);
+template Eigen::Matrix<float,3,1>
+	admmpd::geom::point_on_triangle<float>(
+	const Eigen::Vector3f&, const Eigen::Vector3f&,
+	const Eigen::Vector3f&, const Eigen::Vector3f&);
+template bool geom::aabb_plane_intersect<double>(
+	const Eigen::Matrix<double,3,1>&,
+	const Eigen::Matrix<double,3,1>&,
+	const Eigen::Matrix<double,3,1>&,
+	const Eigen::Matrix<double,3,1>&);
+template bool geom::aabb_plane_intersect<float>(
+	const Eigen::Matrix<float,3,1>&,
+	const Eigen::Matrix<float,3,1>&,
+	const Eigen::Matrix<float,3,1>&,
+	const Eigen::Matrix<float,3,1>&);
+template bool geom::aabb_triangle_intersect<double>(
+    const Eigen::Matrix<double,3,1>&,
+    const Eigen::Matrix<double,3,1>&,
+    const Eigen::Matrix<double,3,1>&,
+    const Eigen::Matrix<double,3,1>&,
+    const Eigen::Matrix<double,3,1>&);
+template bool geom::aabb_triangle_intersect<float>(
+    const Eigen::Matrix<float,3,1>&,
+    const Eigen::Matrix<float,3,1>&,
+    const Eigen::Matrix<float,3,1>&,
+    const Eigen::Matrix<float,3,1>&,
+    const Eigen::Matrix<float,3,1>&);
+template bool admmpd::geom::ray_aabb<double>(
+	const Eigen::Matrix<double,3,1>&, const Eigen::Matrix<double,3,1>&,
+	const Eigen::AlignedBox<double,3>&, double, double);
+template bool admmpd::geom::ray_aabb<float>(
+	const Eigen::Vector3f&, const Eigen::Vector3f&,
+	const Eigen::AlignedBox<float,3>&, float, float);
+template bool admmpd::geom::ray_triangle<double>(
+	const Eigen::Matrix<double,3,1>&, const Eigen::Matrix<double,3,1>&,
+	const Eigen::Matrix<double,3,1>&, const Eigen::Matrix<double,3,1>&,
+	const Eigen::Matrix<double,3,1>&, double, double&, Eigen::Matrix<double,3,1>*);
+template bool admmpd::geom::ray_triangle<float>(
+	const Eigen::Vector3f&, const Eigen::Vector3f&,
+	const Eigen::Vector3f&, const Eigen::Vector3f&,
+	const Eigen::Vector3f&, float, float&, Eigen::Vector3f*);
+template void geom::merge_close_vertices<double>(
+	std::vector<Matrix<double,3,1> > &,
+	std::vector<RowVector4i> &,
+	double eps);
+template void geom::merge_close_vertices<float>(
+	std::vector<Matrix<float,3,1> >&,
+	std::vector<RowVector4i> &,
+	float);
+template void admmpd::geom::make_n3<float>(
+	const Eigen::SparseMatrix<float>&,
+	Eigen::SparseMatrix<float>&);
+template void admmpd::geom::make_n3<double>(
+	const Eigen::SparseMatrix<double>&,
+	Eigen::SparseMatrix<double>&);
+
+} // namespace admmpd

extern/softbody/src/admmpd_geom.h

@@ -0,0 +1,110 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_GEOM_H_
+#define ADMMPD_GEOM_H_
+
+#include <Eigen/Geometry>
+#include <Eigen/Sparse>
+#include <vector>
+
+// Common geometry kernels
+namespace admmpd {
+class geom {
+public:
+
+template<typename T>
+static Eigen::Matrix<T,4,1> point_tet_barys(
+    const Eigen::Matrix<T,3,1> &p,
+    const Eigen::Matrix<T,3,1> &a,
+    const Eigen::Matrix<T,3,1> &b,
+    const Eigen::Matrix<T,3,1> &c,
+    const Eigen::Matrix<T,3,1> &d);
+
+template<typename T>
+static bool point_in_tet(
+    const Eigen::Matrix<T,3,1> &p,
+    const Eigen::Matrix<T,3,1> &a,
+    const Eigen::Matrix<T,3,1> &b,
+    const Eigen::Matrix<T,3,1> &c,
+    const Eigen::Matrix<T,3,1> &d);
+
+template<typename T>
+static void create_tets_from_box(
+    const Eigen::Matrix<T,3,1> &bmin,
+    const Eigen::Matrix<T,3,1> &bmax,
+    std::vector<Eigen::Matrix<T,3,1>> &verts,
+    std::vector<Eigen::RowVector4i> &tets);
+
+template<typename T>
+static Eigen::Matrix<T,3,1> point_triangle_barys(
+    const Eigen::Matrix<T,3,1> &p,
+    const Eigen::Matrix<T,3,1> &a,
+    const Eigen::Matrix<T,3,1> &b,
+    const Eigen::Matrix<T,3,1> &c);
+
+// From RTCD by C.E.
+template<typename T>
+static Eigen::Matrix<T,3,1> point_on_triangle_ce(
+    const Eigen::Matrix<T,3,1> &p,
+    const Eigen::Matrix<T,3,1> &a,
+    const Eigen::Matrix<T,3,1> &b,
+    const Eigen::Matrix<T,3,1> &c);
+
+template<typename T>
+static Eigen::Matrix<T,3,1> point_on_triangle(
+    const Eigen::Matrix<T,3,1> &p,
+    const Eigen::Matrix<T,3,1> &a,
+    const Eigen::Matrix<T,3,1> &b,
+    const Eigen::Matrix<T,3,1> &c);
+
+template<typename T>
+static bool aabb_plane_intersect(
+    const Eigen::Matrix<T,3,1> &bmin,
+    const Eigen::Matrix<T,3,1> &bmax,
+    const Eigen::Matrix<T,3,1> &p, // pt on plane
+    const Eigen::Matrix<T,3,1> &n); // normal
+
+template<typename T>
+static bool aabb_triangle_intersect(
+    const Eigen::Matrix<T,3,1> &bmin,
+    const Eigen::Matrix<T,3,1> &bmax,
+    const Eigen::Matrix<T,3,1> &a,
+    const Eigen::Matrix<T,3,1> &b,
+    const Eigen::Matrix<T,3,1> &c);
+
+template<typename T>
+static bool ray_aabb(
+    const Eigen::Matrix<T,3,1> &o,
+    const Eigen::Matrix<T,3,1> &d,
+    const Eigen::AlignedBox<T,3> &aabb,
+    T t_min, T t_max);
+
+template<typename T>
+static bool ray_triangle(
+	const Eigen::Matrix<T,3,1> &o,
+	const Eigen::Matrix<T,3,1> &d,
+	const Eigen::Matrix<T,3,1> &p0,
+	const Eigen::Matrix<T,3,1> &p1,
+	const Eigen::Matrix<T,3,1> &p2,
+	T t_min,
+	T &t_max,
+	Eigen::Matrix<T,3,1> *bary=nullptr);
+
+// Combines vertices if less than eps apart
+template<typename T>
+static void merge_close_vertices(
+	std::vector<Eigen::Matrix<T,3,1> > &verts,
+	std::vector<Eigen::RowVector4i> &tets,
+	T eps = 1e-12);
+
+// Replicates a matrix along the diagonal
+template<typename T>
+static void make_n3(
+		const Eigen::SparseMatrix<T> &A,
+		Eigen::SparseMatrix<T> &A3);
+
+}; // class geom
+} // namespace admmpd
+
+#endif // ADMMPD_GEOM_H_

extern/softbody/src/admmpd_linsolve.cpp

@@ -0,0 +1,724 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#include "admmpd_linsolve.h"
+#include "admmpd_geom.h"
+#include <numeric>
+#include <iostream>
+#include <set>
+#include <unordered_set>
+#include <random>
+#include <thread>
+#include "BLI_assert.h"
+#include "BLI_task.h"
+
+namespace admmpd {
+using namespace Eigen;
+
+// Throws an exception for a given function with message
+static inline void throw_err(const std::string &f, const std::string &m)
+{
+	throw std::runtime_error("LinearSolver::"+f+": "+m);
+}
+
+void LDLT::init_solve(
+	const Mesh *mesh,
+	const Options *options,
+	const Collision *collision,
+	SolverData *data)
+{
+	(void)(collision);
+	int nx = data->x.rows();
+	if (nx==0) { throw_err("init_solve","no vertices"); }
+	if (!mesh) { throw_err("init_solve","no mesh"); }
+	if (!options) { throw_err("init_solve","no options"); }
+
+	// Get the P matrix
+	std::set<int> pin_inds;
+	std::vector<Triplet<double> > trips;
+	std::vector<double> q_coeffs;
+	bool replicate = false;
+	bool new_P = mesh->linearize_pins(trips, q_coeffs, pin_inds, replicate);
+
+	// If we've changed the stiffness but not the pins,
+	// the P matrix is still changing
+	if (std::abs(options->pk-data->ls.last_pk) > 1e-8 && trips.size()>0)
+		new_P = true;
+
+	// Compute P
+	data->ls.last_pk = options->pk;
+	int np = q_coeffs.size()/3;
+	SparseMatrix<double> P;
+	MatrixXd q;
+	if (data->ls.Ptq.rows() != nx)
+		data->ls.Ptq.resize(nx,3);
+
+	if (np==0)
+	{ // no springs
+		P.resize(1,nx);
+		P.setZero();
+		q = MatrixXd::Zero(1,3);
+		data->ls.Ptq.setZero();
+	}
+	else
+	{
+		P.resize(np,nx);
+		P.setFromTriplets(trips.begin(), trips.end());
+		q.resize(np,3);
+		for (int i=0; i<np; ++i)
+		{
+			q(i,0) = q_coeffs[i*3+0];
+			q(i,1) = q_coeffs[i*3+1];
+			q(i,2) = q_coeffs[i*3+2];
+		}
+		data->ls.Ptq = options->pk * P.transpose() * q;
+	}
+
+	bool new_ptr = false;
+	if (!data->ls.ldlt_A_PtP) {
+		data->ls.ldlt_A_PtP = std::make_unique<Cholesky>();
+		new_ptr = true;
+	}
+
+	// Compute A + P'P and factorize:
+	// 1) A not computed
+	// 2) P has changed
+	// 3) factorization not set
+	if ( new_ptr ||
+		data->ls.A_PtP.nonZeros()==0 ||
+		new_P)
+	{
+		data->ls.A_PtP = SparseMatrix<double>(data->A) + options->pk * P.transpose()*P;
+		data->ls.ldlt_A_PtP->compute(data->ls.A_PtP);
+		if(data->ls.ldlt_A_PtP->info() != Eigen::Success)
+			throw_err("init_solve","facorization failed");
+
+		geom::make_n3<double>(data->ls.A_PtP,data->ls.A_PtP_3); // replicate
+	}
+
+} // end init solve
+
+void LDLT::solve(
+	const Mesh *mesh,
+	const Options *options,
+	const Collision *collision,
+	SolverData *data)
+{
+	int nx = data->x.rows();
+	if (nx==0)
+		throw_err("solve","no vertices");
+
+	if (!data->ls.ldlt_A_PtP)
+		init_solve(mesh,options,collision,data);
+
+	// Linearize collision constraints
+	RowSparseMatrix<double> C;
+	VectorXd d;
+	if (collision != nullptr)
+	{
+		std::vector<double> d_coeffs;
+		std::vector<Eigen::Triplet<double> > trips;
+		collision->linearize(mesh, options, data, &data->x, &trips, &d_coeffs);
+		int nc = d_coeffs.size();
+		if (nc>0)
+		{
+			d = Map<VectorXd>(d_coeffs.data(), d_coeffs.size());
+			C.resize(nc,nx*3);
+			C.setFromTriplets(trips.begin(),trips.end());		
+		}
+	}
+	else
+	{
+		C.resize(1,nx*3);
+		d = VectorXd::Zero(1);
+	}
+	
+	// Compute RHS
+	data->ls.rhs.noalias() =
+		data->M_xbar +
+		data->DtW2*(data->z-data->u) +
+		data->ls.Ptq;
+
+	// If there are no collision constraints,
+	// we can use our initial factorization.
+	if (C.nonZeros()==0)
+	{
+		data->x.noalias() = data->ls.ldlt_A_PtP->solve(data->ls.rhs);
+		return;
+	}
+
+	// Otherwise we have to solve the full system:
+	// (A + PtP + CtC) x = b + Ptq + Ctd
+	data->ls.A_PtP_CtC_3 = data->ls.A_PtP_3 + options->ck * C.transpose()*C;
+	VectorXd Ctd3 = options->ck * C.transpose() * d;
+	VectorXd rhs3(nx*3);
+	for (int i=0; i<nx; ++i)
+	{
+		rhs3.segment<3>(i*3) =
+			data->ls.rhs.row(i).transpose() +
+			Ctd3.segment<3>(i*3);
+	}
+
+	Cholesky ldlt_full(data->ls.A_PtP_CtC_3);
+	VectorXd x3 = ldlt_full.solve(rhs3);
+	for (int i=0; i<nx; ++i)
+		data->x.row(i) = x3.segment<3>(i*3);
+
+} // end solve
+
+void ConjugateGradients::init_solve(
+	const Mesh *mesh,
+	const Options *options,
+	const Collision *collision,
+	SolverData *data)
+{
+	int nx = data->x.rows();
+	if (nx==0)
+		throw_err("init_solve","no vertices");
+
+	// We'll just use our LDLT implementation
+	// to decide when to refactor the matrix
+	data->ls.Ptq.resize(nx,3);
+	LDLT().init_solve(mesh,options,collision,data);
+	data->ls.rhs.resize(nx,3);
+	data->ls.Ctd.resize(nx,3);
+	data->ls.r.resize(nx,3);
+	data->ls.z.resize(nx,3);
+	data->ls.p.resize(nx,3);
+	data->ls.p3.resize(nx*3);
+	data->ls.Ap.resize(nx,3);
+}
+
+void ConjugateGradients::solve(
+	const Mesh *mesh,
+	const Options *options,
+	const Collision *collision,
+	SolverData *data)
+{
+	int nx = data->x.rows();
+	if (nx==0)
+		throw_err("solve","no vertices");
+
+	if (!data->ls.ldlt_A_PtP)
+		init_solve(mesh,options,collision,data);
+
+	BLI_assert(data != NULL);
+	BLI_assert(options != NULL);
+	BLI_assert(nx > 0);
+	BLI_assert(data->ls.Ptq.rows()==nx);
+
+	auto map_vector_to_matrix = [](const VectorXd &x3, MatrixXd &x)
+	{
+		int nx_ = x3.rows()/3;
+		if (x.rows() != nx_) { x.resize(nx_,3); }
+		for (int i=0; i<nx_; ++i)
+			x.row(i) = x3.segment<3>(i*3);
+	};
+
+	auto map_matrix_to_vector = [](const MatrixXd &x, VectorXd &x3)
+	{
+		int nx_ = x.rows();
+		if (x3.rows() != nx_*3) { x3.resize(nx_*3); }
+		for (int i=0; i<nx_; ++i)
+			x3.segment<3>(i*3) = x.row(i);
+	};
+
+	auto mat_inner = [](const MatrixXd &A, const MatrixXd &B)
+	{
+		double sum = 0.0;
+		int rows = std::min(A.rows(), B.rows());
+		int dim = std::min(A.cols(), B.cols());
+		for (int i=0; i<rows; ++i)
+		{
+			for (int j=0; j<dim; ++j)
+			{
+				sum += A(i,j)*B(i,j);
+			}
+		}
+		return sum;
+	};
+
+	// Linearize collision constraints
+	RowSparseMatrix<double> C(1,nx*3);
+	VectorXd d = VectorXd::Zero(1);
+	if (collision != nullptr)
+	{
+		std::vector<double> d_coeffs;
+		std::vector<Eigen::Triplet<double> > trips;
+		collision->linearize(mesh,options,data,&data->x,&trips,&d_coeffs);
+		int nc = d_coeffs.size();
+		if (nc>0)
+		{
+			d = Map<VectorXd>(d_coeffs.data(), d_coeffs.size());
+			C.resize(nc,nx*3);
+			C.setFromTriplets(trips.begin(),trips.end());		
+		}
+	}
+
+	// Compute RHS
+	data->ls.rhs.noalias() =
+		data->M_xbar +
+		data->DtW2*(data->z-data->u) +
+		data->ls.Ptq;
+
+	// If there are no collision constraints,
+	// we can use our initial factorization.
+	if (C.nonZeros()==0)
+	{
+		data->x.noalias() = data->ls.ldlt_A_PtP->solve(data->ls.rhs);
+		return;
+	}
+
+	// Otherwise we have to replicate the system
+	// (A + PtP + CtC) x = Mxbar + DtW2(z-u) + Ptq
+	data->ls.A_PtP_CtC_3 = data->ls.A_PtP_3 + options->ck * C.transpose()*C;
+	map_vector_to_matrix(options->ck*C.transpose()*d, data->ls.Ctd);
+	data->ls.rhs.noalias() += data->ls.Ctd;
+
+	// Grab refs for convenience
+	Eigen::MatrixXd &r = data->ls.r;
+	Eigen::MatrixXd &z = data->ls.z;
+	Eigen::MatrixXd &p = data->ls.p;
+	Eigen::VectorXd &p3 = data->ls.p3;
+	Eigen::MatrixXd &Ap = data->ls.Ap;
+
+	VectorXd x3;
+	map_matrix_to_vector(data->x, x3);
+	MatrixXd tmp_mtx;
+	map_vector_to_matrix(data->ls.A_PtP_CtC_3*x3, tmp_mtx);
+	r.noalias() = data->ls.rhs - tmp_mtx; // b-Ax
+	apply_preconditioner(data,z,r);
+	p = z;
+	map_matrix_to_vector(p,p3);
+
+	for (int iter=0; iter<options->max_cg_iters; ++iter)
+	{
+		map_vector_to_matrix(data->ls.A_PtP_CtC_3*p3, Ap);
+		double p_dot_Ap = mat_inner(p,Ap);
+		if (p_dot_Ap==0.0)
+			break;
+
+		double zk_dot_rk = mat_inner(z,r);
+		if (zk_dot_rk==0.0)
+			break;
+
+		double alpha = zk_dot_rk / p_dot_Ap;
+		data->x.noalias() += alpha * p;
+		r.noalias() -= alpha * Ap;
+		double r_norm = r.lpNorm<Infinity>();
+		if (r_norm < 1e-4)
+			break;
+
+		apply_preconditioner(data,z,r);
+		double zk1_dot_rk1 = mat_inner(z,r);
+		double beta = zk1_dot_rk1 / zk_dot_rk;
+		p = z + beta*p;
+		map_matrix_to_vector(p,p3);
+	}
+
+} // end ConjugateGradients solve
+
+void ConjugateGradients::apply_preconditioner(
+	SolverData *data,
+	Eigen::MatrixXd &x,
+	const Eigen::MatrixXd &b)
+{
+	// Only bother with parallel solve if we have large dof
+	if (x.rows()<10000)
+	{
+		x = data->ls.ldlt_A_PtP->solve(b);
+		return;
+	}
+
+	BLI_assert(b.cols()==3);
+	if (x.rows() != b.rows())
+		x.resize(b.rows(),3);
+
+	Cholesky *chol = data->ls.ldlt_A_PtP.get();
+	const auto & linsolve = [&x,&b,&chol](int col)
+	{
+		x.col(col) = chol->solve(b.col(col));
+	};
+
+	std::vector<std::thread> pool;
+	for (int i=0; i<3; ++i)
+		pool.emplace_back(linsolve,i);
+
+	for (int i=0; i<3; ++i)
+	{
+		if (pool[i].joinable())
+			pool[i].join();
+	}
+
+} // end apply preconditioner
+
+#if 0
+void GaussSeidel::solve(
+		const Options *options,
+		SolverData *data,
+		Collision *collision)
+{
+	MatrixXd dx(data->x.rows(),3);
+	dx.setZero();
+
+	struct GaussSeidelThreadData {
+		int iter;
+		int color;
+		std::vector<std::vector<int> > *colors;
+		const Options *options;
+		SolverData *data;
+		Collision *collision;
+		MatrixXd *dx;
+	};
+
+	GaussSeidelThreadData thread_data = {
+		.iter = 0,
+		.color = 0,
+		.colors = &data->ls.A_colors,
+		.options = options,
+		.data = data,
+		.collision = collision,
+		.dx = &dx };
+
+	// Inner iteration of Gauss-Seidel
+	auto parallel_gs_sweep = [](void *__restrict userdata, const int i_,
+		const TaskParallelTLS *__restrict tls) -> void
+	{
+		(void)(tls);
+		GaussSeidelThreadData *td = (GaussSeidelThreadData*)userdata;
+		int idx = td->colors->at(td->color)[i_];
+		double omega = td->options->gs_omega;
+		typedef RowSparseMatrix<double>::InnerIterator InnerIter;
+
+		// Loop over expanded A matrix, i.e. segment update
+		Vector3d LUx(0,0,0);
+		Vector3d inv_aii(0,0,0);
+		for (int j=0; j<3; ++j)
+		{
+			InnerIter rit(td->data->ls.A3_CtC_PtP, idx*3+j);
+			for (; rit; ++rit)
+			{
+				double v = rit.value();
+				if (v==0.0)
+					continue;
+
+				int r = rit.row();
+				int c = rit.col();
+				if (r==c) // Diagonal
+				{
+					inv_aii[j] = 1.0/v;
+					continue;
+				}
+
+				double xj = td->data->x(c/3,j);
+				LUx[j] += v*xj;
+			}
+
+		} // end loop segment
+
+		// Update x
+		Vector3d bi = ...;
+
+		//Vector3d bi = td->data->b.row(idx);
+		Vector3d xi = td->data->x.row(idx);
+		Vector3d xi_new = (bi-LUx);
+		for (int j=0; j<3; ++j)
+			xi_new[j] *= inv_aii[j];
+
+		td->data->x.row(idx) = xi*(1.0-omega) + xi_new*omega;
+
+		// Check fast-query constraints, e.g.
+		// floor, obstacle (if SDF). Of course this doesn't
+		// work so well with embedded solver
+//		double floor_z = td->collision->get_floor();
+//		if (td->data->x(idx,2) < floor_z)
+//			td->data->x(idx,2) = floor_z;
+
+		// Update deltas
+		td->dx->row(idx) = td->data->x.row(idx)-xi.transpose();
+	};
+
+	TaskParallelSettings thrd_settings;
+	BLI_parallel_range_settings_defaults(&thrd_settings);
+
+	// Outer iteration loop
+	int n_colors = data->gsdata.A3_plus_CtC_colors.size();
+	int iter = 0;
+	for (; iter < options->max_gs_iters; ++iter)
+	{
+		for (int color=0; color<n_colors; ++color)
+		{
+			thread_data.color = color;
+			int n_inds = data->gsdata.A3_plus_CtC_colors[color].size();
+			thrd_settings.use_threading = false;
+			BLI_task_parallel_range(0, n_inds, &thread_data, parallel_gs_sweep, &thrd_settings);
+		} // end loop colors
+
+		double dxn = dx.rowwise().lpNorm<Infinity>().maxCoeff();
+		if (dxn < options->min_res)
+			break;
+	} // end loop GS iters
+
+} // end solve with constraints
+
+void GaussSeidel::init_solve(
+		const Options *options,
+		SolverData *data,
+		Collision *collision)
+{
+	BLI_assert(options != nullptr);
+	BLI_assert(data != nullptr);
+	BLI_assert(collision != nullptr);
+	int nx = data->x.rows();
+	BLI_assert(nx>0);
+	BLI_assert(data->x.cols()==3);
+
+	// Do we need to color the default colorings?
+	if (data->ls.A_colors.size() == 0)
+	{
+		std::vector<std::set<int> > c_graph;
+		compute_colors(data->energies_graph, c_graph, data->ls.A_colors);
+	}
+
+} // end init solve
+
+// Rehash of graph coloring from
+// https://github.com/mattoverby/mclscene/blob/master/include/MCL/GraphColor.hpp
+void GaussSeidel::compute_colors(
+		const std::vector<std::set<int> > &vertex_energies_graph,
+		const std::vector<std::set<int> > &vertex_constraints_graph,
+		std::vector<std::vector<int> > &colors)
+{
+	int n_nodes = vertex_energies_graph.size();
+	BLI_assert(n_nodes>0);
+	BLI_assert(
+		vertex_constraints_graph.size()==0 ||
+		(int)vertex_constraints_graph.size()==n_nodes);
+
+	{
+		colors.clear();
+		colors.resize(n_nodes, std::vector<int>());
+		for (int i=0; i<n_nodes; ++i)
+			colors[i].emplace_back(i);
+		return;
+	}
+	
+	// Graph color settings
+	int init_palette_size = 6;
+
+	// Graph coloring tmp data
+	std::vector<std::set<int> > palette(n_nodes, std::set<int>());
+	std::vector<int> conflict(n_nodes,1);
+	std::vector<int> node_colors(n_nodes,-1);
+	std::vector<int> node_queue(n_nodes);
+	std::iota(node_queue.begin(), node_queue.end(), 0);
+    std::random_device rd;
+    std::mt19937 mt(rd());
+	std::uniform_int_distribution<int> dist(0, n_nodes);
+
+	struct GraphColorThreadData {
+		int iter;
+		int init_palette_size;
+		const std::vector<std::set<int> > *e_graph; // energies
+		const std::vector<std::set<int> > *c_graph; // constraints
+		std::vector<std::set<int> > *palette;
+		std::vector<int> *conflict;
+		std::vector<int> *node_colors;
+		std::vector<int> *node_queue;
+    	std::mt19937 *mt;
+		std::uniform_int_distribution<int> *dist;
+	};
+	GraphColorThreadData thread_data = {
+		.iter = 0,
+		.init_palette_size = init_palette_size,
+		.e_graph = &vertex_energies_graph,
+		.c_graph = &vertex_constraints_graph,
+		.palette = &palette,
+		.conflict = &conflict,
+		.node_colors = &node_colors,
+		.node_queue = &node_queue,
+		.mt = &mt,
+		.dist = &dist };
+	TaskParallelSettings thrd_settings;
+	BLI_parallel_range_settings_defaults(&thrd_settings);
+
+	//
+	// Step 1)
+	// Graph color initialization
+	//
+	auto init_graph = [](void *__restrict userdata, const int i,
+		const TaskParallelTLS *__restrict tls) -> void
+	{
+		(void)(tls);
+		GraphColorThreadData *td = (GraphColorThreadData*)userdata;
+		for( int j=0; j<td->init_palette_size; ++j ) // init colors
+			td->palette->at(i).insert(j); 
+	};
+	BLI_task_parallel_range(0, n_nodes, &thread_data, init_graph, &thrd_settings);
+
+	//
+	// Step 2)
+	// Stochastic Graph coloring
+	//
+	int max_iters = n_nodes;
+	for (int rand_iter=0; n_nodes>0 && rand_iter<max_iters; ++rand_iter)
+	{
+		thread_data.iter = rand_iter;
+
+		// Generate a random color
+		auto generate_color = [](void *__restrict userdata, const int i,
+			const TaskParallelTLS *__restrict tls) -> void
+		{
+			(void)(tls);
+			GraphColorThreadData *td = (GraphColorThreadData*)userdata;
+			int idx = td->node_queue->at(i);
+			if (td->palette->at(idx).size()<2) // Feed the hungry
+					td->palette->at(idx).insert(td->init_palette_size+td->iter);
+			int c_idx = td->dist->operator()(*td->mt) % td->palette->at(idx).size();
+			td->node_colors->at(idx) = *std::next(td->palette->at(idx).begin(), c_idx);
+		};
+		BLI_task_parallel_range(0, n_nodes, &thread_data, generate_color, &thrd_settings);
+
+		// Detect conflicts
+		auto detect_conflicts = [](void *__restrict userdata, const int i,
+			const TaskParallelTLS *__restrict tls) -> void
+		{
+			(void)(tls);
+			GraphColorThreadData *td = (GraphColorThreadData*)userdata;
+			int idx = td->node_queue->at(i);
+			int curr_c = td->node_colors->at(idx);
+			bool curr_conflict = false;
+			for (std::set<int>::iterator e_it = td->e_graph->at(idx).begin();
+				e_it != td->e_graph->at(idx).end() && !curr_conflict; ++e_it)
+			{
+				int adj_idx = *e_it;
+				if (adj_idx<=idx)
+					continue; // Hungarian heuristic
+				int adj_c = td->node_colors->at(adj_idx);
+				if (curr_c==adj_c)
+					curr_conflict = true;
+			}
+			if ((int)td->c_graph->size() > idx)
+			{
+				for (std::set<int>::iterator c_it = td->c_graph->at(idx).begin();
+					c_it != td->c_graph->at(idx).end() && !curr_conflict; ++c_it)
+				{
+					int adj_idx = *c_it;
+					if (adj_idx<=idx)
+						continue; // Hungarian heuristic
+					int adj_c = td->node_colors->at(adj_idx);
+					if (curr_c==adj_c)
+						curr_conflict = true;
+				}
+			}
+			td->conflict->at(idx) = curr_conflict;
+		};
+		BLI_task_parallel_range(0, n_nodes, &thread_data, detect_conflicts, &thrd_settings);
+
+		// Resolve conflicts and update queue
+		std::vector<int> new_queue;
+		for (int i=0; i<n_nodes; ++i)
+		{
+			int idx = node_queue[i];
+			if (conflict[idx])
+				new_queue.emplace_back(idx);
+			else
+			{
+				int curr_color = node_colors[idx];
+				// Remove color from neighbor palletes
+				for (std::set<int>::iterator e_it = vertex_energies_graph[idx].begin();
+					e_it != vertex_energies_graph[idx].end(); ++e_it)
+				{
+					int adj_idx = *e_it;
+					if (conflict[adj_idx]) // still in the set?
+						palette[adj_idx].erase(curr_color);
+				}
+				if ((int)vertex_constraints_graph.size() > idx)
+				{
+					for (std::set<int>::iterator c_it = vertex_constraints_graph[idx].begin();
+						c_it != vertex_constraints_graph[idx].end(); ++c_it)
+					{
+						int adj_idx = *c_it;
+						if (conflict[adj_idx]) // still in the set?
+							palette[adj_idx].erase(curr_color);
+					}
+				}
+			}
+		}
+
+		node_queue = new_queue;
+		n_nodes = node_queue.size();
+
+	} // end color loop
+
+	//
+	// Step 3)
+	// Map per-vertex colors
+	//
+	colors.clear();
+	colors.resize(14,std::vector<int>());
+	n_nodes = node_colors.size();
+	for( int i=0; i<n_nodes; ++i ){
+		int color = node_colors[i];
+		if (color<0)
+			throw std::runtime_error("GaussSeidel: Error with coloring");
+		while( color >= (int)colors.size() )
+			colors.emplace_back(std::vector<int>());
+		colors[color].emplace_back(i);
+	}
+
+	// Remove empty color groups
+	for (std::vector<std::vector<int> >::iterator it = colors.begin(); it != colors.end();)
+		it->size() == 0 ? it = colors.erase(it) : it++;
+
+} // end compute colors
+
+void GaussSeidel::verify_colors(SolverData *data)
+{
+	// TODO check constraints too
+	// Verify color groups are correct
+	std::cout << "TESTING " << data->tets.rows() << " tets" << std::endl;
+	std::cout << "num colors: " << data->gsdata.A_colors.size() << std::endl;
+	int nt = data->tets.rows();
+	int nc = data->gsdata.A_colors.size();
+	for (int i=0; i<nc; ++i)
+	{
+		// Each vertex in the color should not
+		// be a part of a tet with a vertex in the same color
+		const std::vector<int> &grp = data->gsdata.A_colors[i];
+		int n_grp = grp.size();
+		for (int j=0; j<n_grp; ++j)
+		{
+			int p_idx = grp[j];
+			auto in_tet = [](int idx, const RowVector4i &t)
+			{
+				return (t[0]==idx||t[1]==idx||t[2]==idx||t[3]==idx);
+			};
+
+			for (int k=0; k<nt; ++k)
+			{
+				RowVector4i t = data->tets.row(k);
+				if (!in_tet(p_idx,t))
+					continue;
+
+				for (int l=0; l<n_grp; ++l)
+				{
+					int q_idx = grp[l];
+					if (p_idx==q_idx)
+						continue;
+
+					if (in_tet(q_idx,t))
+					{
+						std::cerr << "p: " << p_idx << ", q: " << q_idx <<
+							", tet (" << k << "): " << t <<
+							", color: " << i <<
+							std::endl;
+					}
+				}
+			}
+		}
+	}
+} // end verify colors
+#endif
+
+} // namespace admmpd

extern/softbody/src/admmpd_linsolve.h

@@ -0,0 +1,106 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_LINSOLVE_H_
+#define ADMMPD_LINSOLVE_H_
+
+#include "admmpd_types.h"
+#include "admmpd_collision.h"
+
+namespace admmpd {
+
+class LinearSolver
+{
+public:
+	virtual void init_solve(
+        const Mesh *mesh,
+        const Options *options,
+		const Collision *collision,
+        SolverData *data) = 0;
+
+	virtual void solve(
+        const Mesh *mesh,
+        const Options *options,
+		const Collision *collision,
+        SolverData *data) = 0;
+};
+
+class LDLT : public LinearSolver
+{
+public:
+	// Factors the matrix on any change to P
+	void init_solve(
+        const Mesh *mesh,
+        const Options *options,
+		const Collision *collision,
+        SolverData *data);
+
+	// Factors the matrix on any change to C
+	void solve(
+        const Mesh *mesh,
+        const Options *options,
+		const Collision *collision, // may be null
+        SolverData *data);
+};
+
+// Preconditioned Conjugate Gradients
+class ConjugateGradients : public LinearSolver
+{
+public:
+	// Calls LDLT::init_solve, since the same
+	// factorization and quantities are used.
+	void init_solve(
+        const Mesh *mesh,
+        const Options *options,
+		const Collision *collision,
+        SolverData *data);
+
+	// Does not factor a new matrix if changes to C.
+	// Instead, uses CG with the factorization computed
+	// in init_solve as the preconditioner.
+	void solve(
+        const Mesh *mesh,
+        const Options *options,
+		const Collision *collision,
+        SolverData *data);
+
+protected:
+	void apply_preconditioner(
+		SolverData *data,
+		Eigen::MatrixXd &x,
+		const Eigen::MatrixXd &b);
+};
+
+#if 0
+// Multi-Colored Gauss-Seidel
+class GaussSeidel : public LinearSolver
+{
+public:
+	void init(
+		const Options *options,
+		SolverData *data,
+		Collision *collision);
+
+	void solve(
+		const Options *options,
+		SolverData *data,
+		Collision *collision);
+
+protected:
+    std::vector<std::vector<int> > A_colors; // colors of (original) A matrix
+    std::vector<std::vector<int> > A3_plus_CtC_colors; // colors of A3+CtC
+
+	void compute_colors(
+		const std::vector<std::set<int> > &vertex_energies_graph,
+		const std::vector<std::set<int> > &vertex_constraints_graph,
+		std::vector<std::vector<int> > &colors);
+
+	// For debugging:
+	void verify_colors(SolverData *data);
+
+};
+#endif
+
+} // namespace admmpd
+
+#endif // ADMMPD_LINSOLVE_H_

extern/softbody/src/admmpd_log.cpp

@@ -0,0 +1,87 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#include "admmpd_types.h"
+#include "admmpd_log.h"
+
+namespace admmpd {
+
+void Logger::reset()
+{
+	curr_timer.clear();
+	elapsed_ms.clear();
+}
+
+void Logger::start_state(int state)
+{
+	if (m_log_level < LOGLEVEL_HIGH)
+		return;
+
+	if (m_log_level >= LOGLEVEL_DEBUG)
+		printf("Starting state %s\n",state_string(state).c_str());
+
+	if (curr_timer.count(state)==0)
+	{
+		elapsed_ms[state] = 0;
+		curr_timer[state] = MicroTimer();
+		return;
+	}
+	curr_timer[state].reset();
+}
+
+// Returns time elapsed
+double Logger::stop_state(int state)
+{
+	if (m_log_level < LOGLEVEL_HIGH)
+		return 0;
+
+	if (m_log_level >= LOGLEVEL_DEBUG)
+		printf("Stopping state %s\n",state_string(state).c_str());
+
+	if (curr_timer.count(state)==0)
+	{
+		elapsed_ms[state] = 0;
+		curr_timer[state] = MicroTimer();
+		return 0;
+	}
+	double dt = curr_timer[state].elapsed_ms();
+	elapsed_ms[state] += dt;
+	return dt;
+}
+
+std::string Logger::state_string(int state)
+{
+	std::string str = "unknown";
+	switch (state)
+	{
+		default: break;
+		case SOLVERSTATE_INIT: str="init"; break;
+		case SOLVERSTATE_MESHCREATE: str="mesh_init"; break;
+		case SOLVERSTATE_SOLVE: str="solve"; break;
+		case SOLVERSTATE_INIT_SOLVE: str="init_solve"; break;
+		case SOLVERSTATE_LOCAL_STEP: str="local_step"; break;
+		case SOLVERSTATE_GLOBAL_STEP: str="global_step"; break;
+		case SOLVERSTATE_COLLISION_UPDATE: str="collision_update"; break;
+		case SOLVERSTATE_TEST_CONVERGED: str="test_converged"; break;
+	}
+	return str;
+}
+
+std::string Logger::to_string()
+{
+	// Sort by largest time
+	auto sort_ms = [](const std::pair<int, double> &a, const std::pair<int, double> &b)
+		{ return (a.second > b.second); };
+	std::vector<std::pair<double, int> > ms(elapsed_ms.begin(), elapsed_ms.end());
+	std::sort(ms.begin(), ms.end(), sort_ms);
+
+	// Concat string
+	std::stringstream ss;
+	int n_timers = ms.size();
+	for (int i=0; i<n_timers; ++i)
+		ss << state_string(ms[i].first) << ": " << ms[i].second << "ms" << std::endl;
+
+	return ss.str();
+}
+
+} // namespace admmpd

extern/softbody/src/admmpd_log.h

@@ -0,0 +1,28 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_LOG_H_
+#define ADMMPD_LOG_H_
+
+#include "admmpd_timer.h"
+#include <unordered_map>
+
+namespace admmpd {
+
+class Logger {
+protected:
+	std::unordered_map<int,double> elapsed_ms;
+	std::unordered_map<int,MicroTimer> curr_timer;
+	int m_log_level;
+public:
+    Logger(int level) : m_log_level(level) {}
+	void reset();
+	void start_state(int state);
+	double stop_state(int state); // ret time elapsed
+	std::string state_string(int state);
+	std::string to_string();
+};
+
+} // namespace admmpd
+
+#endif // ADMMPD_LOG_H_

extern/softbody/src/admmpd_mesh.cpp

@@ -0,0 +1,843 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#include "admmpd_mesh.h"
+#include "admmpd_geom.h"
+#include "admmpd_timer.h"
+#include "admmpd_log.h"
+#include <unordered_map>
+#include <set>
+#include <iostream>
+#include <algorithm>
+#include "BLI_assert.h"
+#include "BLI_task.h"
+
+namespace admmpd {
+using namespace Eigen;
+
+static inline void throw_err(const std::string f, const std::string &msg)
+{
+	throw std::runtime_error("Mesh::"+f+": "+msg);
+}
+
+static void gather_octree_tets(
+	Octree<double,3>::Node *node,
+	const MatrixXd *emb_V,
+	const MatrixXi *emb_F,
+	const Discregrid::CubicLagrangeDiscreteGrid *sdf,
+	std::vector<Vector3d> &verts,
+	std::vector<RowVector4i> &tets
+	)
+{
+	if (node == nullptr)
+		return;
+
+	bool is_leaf = node->is_leaf();
+	bool has_prims = (int)node->prims.size()>0;
+	if (is_leaf)
+	{
+		Vector3d bmin = node->center-Vector3d::Ones()*node->halfwidth;
+		Vector3d bmax = node->center+Vector3d::Ones()*node->halfwidth;
+
+		// If we have primitives in the cell,
+		// create tets and compute embedding
+		if (has_prims)
+		{
+//			int prev_tets = tets.size();
+			geom::create_tets_from_box(bmin,bmax,verts,tets);
+//			int num_box_tets = (int)tets.size()-prev_tets;
+		}
+		else
+		{
+			double dist = sdf->interpolate(0, node->center);
+			if (dist <= 0)
+				geom::create_tets_from_box(bmin,bmax,verts,tets);
+		}
+		return;
+	}
+	for (int i=0; i<8; ++i)
+	{
+		gather_octree_tets(node->children[i],emb_V,emb_F,sdf,verts,tets);
+	}
+
+} // end gather octree tets
+
+bool EmbeddedMesh::create(
+	const Options *options,
+	const float *verts, // size nv*3
+	int nv,
+	const unsigned int *faces, // size nf*3
+	int nf,
+	const unsigned int *tets, // ignored
+	int nt)
+{
+	admmpd::Logger log(options->log_level);
+	log.start_state(SOLVERSTATE_MESHCREATE);
+
+	P_updated = true;
+	mesh_is_closed = false;
+
+	if (nv<=0 || verts == nullptr) {
+		log.stop_state(SOLVERSTATE_MESHCREATE);
+		return false;
+	}
+	if (nf<=0 || faces == nullptr) {
+		log.stop_state(SOLVERSTATE_MESHCREATE);
+		return false;
+	}
+	(void)(tets);
+	(void)(nt);
+
+//	std::vector<std::set<int> > mesh_faces;
+//	std::vector<std::set<int> > mesh_vertices;
+	std::vector<AlignedBox<double,3> > emb_leaves(nf);
+	emb_V0.resize(nv,3);
+	emb_F.resize(nf,3);
+
+	for (int i=0; i<nv; ++i)
+	{
+		emb_V0(i,0) = verts[i*3+0];
+		emb_V0(i,1) = verts[i*3+1];
+		emb_V0(i,2) = verts[i*3+2];
+	}
+
+	for (int i=0; i<nf; ++i)
+	{
+		emb_F(i,0) = faces[i*3+0];
+		emb_F(i,1) = faces[i*3+1];
+		emb_F(i,2) = faces[i*3+2];
+		AlignedBox<double,3> &box = emb_leaves[i];
+		box.extend(emb_V0.row(emb_F(i,0)).transpose());
+		box.extend(emb_V0.row(emb_F(i,1)).transpose());
+		box.extend(emb_V0.row(emb_F(i,2)).transpose());
+		box.extend(box.min()-Vector3d::Ones()*1e-4);
+		box.extend(box.max()+Vector3d::Ones()*1e-4);
+
+		// This code works just fine and does what I want:
+		// splits a triangle mesh into multiple meshes
+		// based on connectivity. However, it isn't as helpful
+		// in practice as I hoped.
+//		std::vector<int> in_mesh;
+//		for (int j=0; j<(int)mesh_vertices.size(); ++j)
+//		{
+//			if (mesh_vertices[j].count(faces[i*3+0]) ||
+//				mesh_vertices[j].count(faces[i*3+1]) ||
+//				mesh_vertices[j].count(faces[i*3+2]))
+//					in_mesh.emplace_back(j);
+//		}
+//
+//		// If it's in no mesh, create a new one
+//		if (in_mesh.size()==0)
+//		{
+//			mesh_vertices.emplace_back(std::set<int>());
+//			mesh_faces.emplace_back(std::set<int>());
+//			mesh_vertices.back().emplace(faces[i*3+0]);
+//			mesh_vertices.back().emplace(faces[i*3+1]);
+//			mesh_vertices.back().emplace(faces[i*3+2]);
+//			mesh_faces.back().emplace(i);
+//			continue;
+//		}
+//
+//		// Otherwise, combine meshes if in muliple
+//		for (int j=0; j<(int)in_mesh.size(); ++j)
+//		{
+//			int mesh0 = in_mesh[0];
+//			int meshj = in_mesh[j];
+//			if (j==0) // insert into lowest index mesh
+//			{
+//				mesh_vertices[mesh0].emplace(faces[i*3+0]);
+//				mesh_vertices[mesh0].emplace(faces[i*3+1]);
+//				mesh_vertices[mesh0].emplace(faces[i*3+2]);
+//				mesh_faces[mesh0].emplace(i);
+//				continue;
+//			}
+//			// Merge meshes if shared stencil
+//			mesh_faces[mesh0].insert(mesh_faces[meshj].begin(), mesh_faces[meshj].end());
+//			mesh_vertices[mesh0].insert(mesh_vertices[meshj].begin(), mesh_vertices[meshj].end());
+//			mesh_faces.erase(mesh_faces.begin()+meshj);
+//			mesh_vertices.erase(mesh_vertices.begin()+meshj);
+//		}
+
+	} // end loop faces
+
+	// Compute SDF and tree on the full mesh.
+	compute_sdf(&emb_V0, &emb_F, &emb_sdf);
+	emb_rest_facet_tree.init(emb_leaves);
+
+	if (!compute_lattice(options)) {
+		throw_err("create","Failed lattice generation");
+	}
+	if (!compute_embedding(options)) {
+		throw_err("create","Failed embedding calculation");
+	}
+
+	// Verify embedding is correct
+	for (int i=0; i<nv; ++i)
+	{
+		if (emb_v_to_tet[i]<0)
+			throw_err("create","Failed embedding");
+		if (std::abs(emb_barys.row(i).sum()-1.0)>1e-6)
+		{
+			std::stringstream ss; ss << emb_barys.row(i);
+			throw_err("create","Bad embedding barys: "+ss.str());
+		}
+	}
+
+	if (!emb_rest_facet_tree.root())
+		throw_err("create","Failed to create tree");
+	if (lat_V0.rows()==0)
+		throw_err("create","Failed to create verts");
+	if (lat_T.rows()==0)
+		throw_err("create","Failed to create tets");
+	if (emb_F.rows()==0)
+		throw_err("create","Did not set faces");
+	if (emb_V0.rows()==0)
+		throw_err("create","Did not set verts");
+
+	log.stop_state(SOLVERSTATE_MESHCREATE);
+	if (options->log_level >= LOGLEVEL_DEBUG) {
+		printf("%s\n",log.to_string().c_str());
+	}
+	
+	return true;
+}
+
+void EmbeddedMesh::compute_sdf(
+	const Eigen::MatrixXd *emb_v,
+	const Eigen::MatrixXi *emb_f,
+	SDFType *sdf)
+{
+	if (emb_f->rows()==0) {
+		return;
+	}
+
+	Matrix<double,Dynamic,Dynamic,RowMajor> v_rm = *emb_v;
+	Matrix<unsigned int,Dynamic,Dynamic,RowMajor> f_rm = emb_f->cast<unsigned int>();
+
+	Vector3d min_x(v_rm.col(0).minCoeff(), v_rm.col(1).minCoeff(), v_rm.col(2).minCoeff());
+	Vector3d max_x(v_rm.col(0).maxCoeff(), v_rm.col(1).maxCoeff(), v_rm.col(2).maxCoeff());
+	AlignedBox<double,3> domain;
+	domain.extend(min_x);
+	domain.extend(max_x);
+	domain.max() += 1e-3 * domain.diagonal().norm() * Eigen::Vector3d::Ones();
+	domain.min() -= 1e-3 * domain.diagonal().norm() * Eigen::Vector3d::Ones();
+
+	// Decide an SDF resolution. We want it to scale: high resolution
+	// for lower resolution meshes. This is because it becomes WAY too
+	// expensive to compute SDFs for high res meshes.
+	int nf = emb_f->rows();
+	unsigned int res = std::max(10, 20-(int)std::log(nf));
+	std::array<unsigned int, 3> resolution;
+	resolution[0] = res; resolution[1] = res; resolution[2] = res;
+	Discregrid::TriangleMesh tm(v_rm.data(), f_rm.data(), v_rm.rows(), f_rm.rows());
+	mesh_is_closed = tm.is_closed();
+	Discregrid::MeshDistance md(tm);
+	*sdf = Discregrid::CubicLagrangeDiscreteGrid(domain, resolution);
+	auto func = Discregrid::DiscreteGrid::ContinuousFunction{};
+	std::vector<std::thread::id> thread_map;
+	md.set_thread_map(&thread_map);
+	func = [&md](Eigen::Vector3d const& xi) {
+		return md.signedDistanceCached(xi);
+	};
+	sdf->addFunction(func, &thread_map, false);
+}
+
+bool EmbeddedMesh::compute_lattice(const admmpd::Options *options)
+{
+	// Create subset of faces
+//	if (emb_faces.size()==0) { return false; }
+//	MatrixXi F(emb_faces.size(),3);
+//	int f_count = 0;
+//	for (std::set<int>::const_iterator it = emb_faces.begin();
+//		it != emb_faces.end(); ++it)
+//	{
+//		F.row(f_count) = emb_F.row(*it);
+//		f_count++;
+//	}
+	MatrixXi &F = emb_F;
+
+	// Create an octree to generate the tets from
+	Octree<double,3> octree;
+	octree.init(&emb_V0,&F,options->lattice_subdiv);
+
+	std::vector<Vector3d> lat_verts;
+	std::vector<RowVector4i> lat_tets;
+	Octree<double,3>::Node *root = octree.root().get();
+	gather_octree_tets(
+		root,
+		&emb_V0,
+		&F,
+		&emb_sdf,
+		lat_verts,
+		lat_tets);
+
+	geom::merge_close_vertices(lat_verts,lat_tets);
+	int nlv = lat_verts.size();
+	int nlt = lat_tets.size();
+	int prev_lv = lat_V0.rows();
+	int prev_lt = lat_T.rows();
+	lat_V0.conservativeResize(prev_lv+nlv,3);
+	lat_T.conservativeResize(prev_lt+nlt,4);
+
+	for (int i=0; i<nlv; ++i)
+		lat_V0.row(prev_lv+i) = lat_verts[i];
+
+	for (int i=0; i<nlt; ++i)
+		lat_T.row(prev_lt+i) = lat_tets[i];
+
+	return nlt>0;
+}
+
+bool EmbeddedMesh::compute_embedding(const admmpd::Options *options)
+{
+	struct FindTetThreadData {
+		const Options *opt;
+		AABBTree<double,3> *tree;
+		EmbeddedMesh *emb_mesh; // thread sets vtx_to_tet and barys
+		MatrixXd *lat_V0;
+		MatrixXi *lat_T;
+		MatrixXd *emb_barys;
+		VectorXi *emb_v_to_tet;
+	};
+
+	if (options->log_level >= LOGLEVEL_DEBUG) {
+		printf("Computing embedding for %d verts\n", (int)emb_V0.rows());
+	}
+
+	auto parallel_point_in_tet = [](
+		void *__restrict userdata,
+		const int i,
+		const TaskParallelTLS *__restrict tls)->void
+	{
+		(void)(tls);
+		FindTetThreadData *td = (FindTetThreadData*)userdata;
+		const MatrixXd *emb_x0 = td->emb_mesh->rest_facet_verts();
+		Vector3d pt = emb_x0->row(i);
+		PointInTetMeshTraverse<double> traverser(
+				pt,
+				td->lat_V0,
+				td->lat_T);
+		bool success = td->tree->traverse(traverser);
+		int tet_idx = traverser.output.prim;
+		if (success && tet_idx >= 0)
+		{
+			const MatrixXd *tet_V0 = td->emb_mesh->rest_prim_verts();
+			RowVector4i tet = td->emb_mesh->prims()->row(tet_idx);
+			Vector3d t[4] = {
+				tet_V0->row(tet[0]),
+				tet_V0->row(tet[1]),
+				tet_V0->row(tet[2]),
+				tet_V0->row(tet[3])
+			};
+			td->emb_v_to_tet->operator[](i) = tet_idx;
+			Vector4d b = geom::point_tet_barys<double>(pt,t[0],t[1],t[2],t[3]);
+			td->emb_barys->row(i) = b;
+
+			if (td->opt->log_level >= LOGLEVEL_DEBUG) {
+				printf("\tFound embedding for %d: %f %f %f %f\n", i, b[0],b[1],b[2],b[3]);
+			}
+		}
+		else {
+			if (td->opt->log_level >= LOGLEVEL_DEBUG) {
+				printf("\tDid NOT find embedding for %d!\n", i);
+			}
+		}
+	}; // end parallel find tet
+
+	int nv = emb_V0.rows();
+	if (nv==0)
+	{
+		throw_err("compute_embedding", "No embedded verts");
+	}
+
+	emb_barys.resize(nv,4);
+	emb_barys.setZero();
+	emb_v_to_tet.resize(nv);
+	emb_v_to_tet.setOnes();
+	emb_v_to_tet *= -1;
+	int nt = lat_T.rows();
+
+	// BVH tree for finding point-in-tet and computing
+	// barycoords for each embedded vertex
+	std::vector<AlignedBox<double,3> > tet_aabbs;
+	tet_aabbs.resize(nt);
+	Vector3d veta = Vector3d::Ones()*1e-4;
+	for (int i=0; i<nt; ++i)
+	{
+		tet_aabbs[i].setEmpty();
+		RowVector4i tet = lat_T.row(i);
+		for (int j=0; j<4; ++j)
+			tet_aabbs[i].extend(lat_V0.row(tet[j]).transpose());
+
+		tet_aabbs[i].extend(tet_aabbs[i].min()-veta);
+		tet_aabbs[i].extend(tet_aabbs[i].max()+veta);
+	}
+
+	AABBTree<double,3> tree;
+	tree.init(tet_aabbs);
+
+	FindTetThreadData thread_data = {
+		options,
+		&tree,
+		this,
+		&lat_V0,
+		&lat_T,
+		&emb_barys,
+		&emb_v_to_tet
+	};
+	TaskParallelSettings settings;
+	BLI_parallel_range_settings_defaults(&settings);
+	if (options->log_level >= LOGLEVEL_DEBUG) {
+		settings.use_threading = false;
+	}
+	BLI_task_parallel_range(0, nv, &thread_data, parallel_point_in_tet, &settings);
+
+	// Double check we set (valid) barycoords for every embedded vertex
+	const double eps = 1e-8;
+	for (int i=0; i<nv; ++i)
+	{
+		RowVector4d b = emb_barys.row(i);
+		if (b.minCoeff() < -eps)
+		{
+			throw_err("compute_embedding", "negative barycoords");
+		}
+		if (b.maxCoeff() > 1 + eps)
+		{
+			throw_err("compute_embedding", "max barycoord > 1");
+		}
+		if (b.sum() > 1 + eps)
+		{
+			throw_err("compute_embedding", "barycoord sum > 1");
+		}
+	}
+
+	return true;
+
+} // end compute vtx to tet mapping
+
+Eigen::Vector3d EmbeddedMesh::get_mapped_facet_vertex(
+	const Eigen::MatrixXd *prim_verts,
+	int facet_vertex_idx) const
+{
+    int t_idx = emb_v_to_tet[facet_vertex_idx];
+    RowVector4i tet = lat_T.row(t_idx);
+    RowVector4d b = emb_barys.row(facet_vertex_idx);
+    return Vector3d(
+		prim_verts->row(tet[0]) * b[0] +
+		prim_verts->row(tet[1]) * b[1] +
+		prim_verts->row(tet[2]) * b[2] +
+		prim_verts->row(tet[3]) * b[3]);
+}
+
+void EmbeddedMesh::compute_masses(
+	const Eigen::MatrixXd *x,
+	double density_kgm3,
+	Eigen::VectorXd &m) const
+{
+	density_kgm3 = std::abs(density_kgm3);
+
+	int nx = x->rows();
+	m.resize(nx);
+	m.setZero();
+	int n_tets = lat_T.rows();
+	for (int t=0; t<n_tets; ++t)
+	{
+		RowVector4i tet = lat_T.row(t);
+		RowVector3d tet_v0 = x->row(tet[0]);
+		Matrix3d edges;
+		edges.col(0) = x->row(tet[1]) - tet_v0;
+		edges.col(1) = x->row(tet[2]) - tet_v0;
+		edges.col(2) = x->row(tet[3]) - tet_v0;
+		double vol = std::abs((edges).determinant()/6.f);
+		double tet_mass = density_kgm3 * vol;
+		m[tet[0]] += tet_mass / 4.f;
+		m[tet[1]] += tet_mass / 4.f;
+		m[tet[2]] += tet_mass / 4.f;
+		m[tet[3]] += tet_mass / 4.f;
+	}
+
+	// Verify masses
+	for (int i=0; i<nx; ++i)
+	{
+		if (m[i] <= 0.0)
+		{
+			throw_err("compute_masses","unref'd vertex");
+			m[i]=1;
+		}
+	}
+}
+
+void EmbeddedMesh::set_pin(
+	int idx,
+	const Eigen::Vector3d &p,
+    double k)
+{
+	std::unordered_map<int,double>::const_iterator it = emb_pin_k.find(idx);
+	if (it == emb_pin_k.end()) { P_updated = true; }
+	else if (k != it->second) { P_updated = true; }
+
+	// Remove pin
+	if (k <= 1e-5)
+	{
+		emb_pin_k.erase(idx);
+		emb_pin_pos.erase(idx);
+		return;
+	}
+
+	emb_pin_k[idx] = k;
+	emb_pin_pos[idx] = p;
+}
+
+bool EmbeddedMesh::linearize_pins(
+	std::vector<Eigen::Triplet<double> > &trips,
+	std::vector<double> &q,
+	std::set<int> &pin_inds,
+    bool replicate) const
+{
+	int np = emb_pin_k.size();
+	trips.reserve((int)trips.size() + np*3*4);
+	q.reserve((int)q.size() + np*3);
+
+	std::unordered_map<int,double>::const_iterator it_k = emb_pin_k.begin();
+	for (; it_k != emb_pin_k.end(); ++it_k)
+	{
+		int emb_idx = it_k->first;
+		pin_inds.emplace(emb_idx);
+		const Vector3d &qi = emb_pin_pos.at(emb_idx);
+		const double &ki = it_k->second;
+
+		if (ki <= 0.0)
+			continue;
+
+		int tet_idx = emb_v_to_tet[emb_idx];
+		RowVector4d bary = emb_barys.row(emb_idx);
+		RowVector4i tet = lat_T.row(tet_idx);
+
+		int p_idx = q.size()/3;
+		for (int i=0; i<3; ++i)
+		{
+			q.emplace_back(qi[i]*ki);
+		}
+
+		if (replicate)
+		{
+			for (int i=0; i<3; ++i)
+			{
+				for (int j=0; j<4; ++j)
+				{
+					double wi = bary[j];
+					trips.emplace_back(p_idx*3+i, tet[j]*3+i, wi*ki);
+				}
+			}
+		}
+		else
+		{
+			for (int j=0; j<4; ++j)
+			{
+				double wi = bary[j];
+				trips.emplace_back(p_idx, tet[j], wi*ki);
+			}
+		}
+	}
+
+	bool has_P_updated = P_updated;
+	P_updated = false;
+	return has_P_updated;
+}
+
+bool TetMesh::create(
+	const Options *options,
+	const float *verts, // size nv*3
+	int nv,
+	const unsigned int *faces, // size nf*3 (surface faces)
+	int nf,
+	const unsigned int *tets, // size nt*4
+	int nt) // must be > 0
+{
+	P_updated = true;
+
+	if (nv<=0 || verts == nullptr)
+		return false;
+	if (nf<=0 || faces == nullptr)
+		return false;
+	if (nt<=0 || tets == nullptr)
+		return false;
+
+	V0.resize(nv,3);
+	for (int i=0; i<nv; ++i)
+	{
+		V0(i,0) = verts[i*3+0];
+		V0(i,1) = verts[i*3+1];
+		V0(i,2) = verts[i*3+2];
+	}
+
+	F.resize(nf,3);
+	std::vector<AlignedBox<double,3> > leaves(nf);
+	for (int i=0; i<nf; ++i)
+	{
+		F(i,0) = faces[i*3+0];
+		F(i,1) = faces[i*3+1];
+		F(i,2) = faces[i*3+2];
+		leaves.emplace_back();
+		AlignedBox<double,3> &box = leaves[i];
+		box.extend(V0.row(F(i,0)).transpose());
+		box.extend(V0.row(F(i,1)).transpose());
+		box.extend(V0.row(F(i,2)).transpose());
+		box.extend(box.min()-Vector3d::Ones()*1e-8);
+		box.extend(box.max()+Vector3d::Ones()*1e-8);
+	}
+
+	T.resize(nt,4);
+	for (int i=0; i<nt; ++i)
+	{
+		T(i,0) = tets[i*4+0];
+		T(i,1) = tets[i*4+1];
+		T(i,2) = tets[i*4+2];
+		T(i,3) = tets[i*4+3];
+	}
+
+	rest_facet_tree.init(leaves);
+	return true;
+
+} // end TetMesh create
+
+void TetMesh::compute_masses(
+	const Eigen::MatrixXd *x,
+	double density_kgm3,
+	Eigen::VectorXd &m) const
+{
+	density_kgm3 = std::abs(density_kgm3);
+
+	// Source: https://github.com/mattoverby/mclscene/blob/master/include/MCL/TetMesh.hpp
+	// Computes volume-weighted masses for each vertex
+	// density_kgm3 is the unit-volume density
+	int nx = x->rows();
+	m.resize(nx);
+	m.setZero();
+	int n_tets = T.rows();
+	for (int t=0; t<n_tets; ++t)
+	{
+		RowVector4i tet = T.row(t);
+		for (int i=0; i<4; ++i)
+		{
+			if (tet[i] < 0 || tet[i] >= nx)
+				throw_err("compute_masses","Bad vertex index");
+		}
+		RowVector3d tet_v0 = x->row(tet[0]);
+		Matrix3d edges;
+		edges.col(0) = x->row(tet[1]) - tet_v0;
+		edges.col(1) = x->row(tet[2]) - tet_v0;
+		edges.col(2) = x->row(tet[3]) - tet_v0;
+		double vol = edges.determinant()/6.0;
+		if (vol <= 0) {
+			throw_err("compute_masses","Inverted or flattened tet");
+		}
+
+		double tet_mass = density_kgm3 * vol;
+		m[tet[0]] += tet_mass / 4.0;
+		m[tet[1]] += tet_mass / 4.0;
+		m[tet[2]] += tet_mass / 4.0;
+		m[tet[3]] += tet_mass / 4.0;
+	}
+
+	// Verify masses
+	for (int i=0; i<nx; ++i)
+	{
+		if (m[i] <= 0.0)
+		{
+			throw_err("compute_masses","unref'd vertex");
+			m[i]=1;
+		}
+	}
+
+} // end compute masses
+
+void TetMesh::set_pin(
+	int idx,
+	const Eigen::Vector3d &p,
+    double k)
+{
+	std::unordered_map<int,double>::const_iterator it = pin_k.find(idx);
+	if (it == pin_k.end()) { P_updated = true; }
+	else if (k != it->second) { P_updated = true; }
+
+	// Remove pin
+	if (k <= 1e-5)
+	{
+		pin_k.erase(idx);
+		pin_pos.erase(idx);
+		return;
+	}
+
+	pin_k[idx] = k;
+	pin_pos[idx] = p;
+}
+
+bool TetMesh::linearize_pins(
+	std::vector<Eigen::Triplet<double> > &trips,
+	std::vector<double> &q,
+	std::set<int> &pin_inds,
+    bool replicate) const
+{
+	int np = pin_k.size();
+	trips.reserve((int)trips.size() + np*3);
+	q.reserve((int)q.size() + np*3);
+
+	std::unordered_map<int,double>::const_iterator it_k = pin_k.begin();
+	for (; it_k != pin_k.end(); ++it_k)
+	{
+		int idx = it_k->first;
+		pin_inds.emplace(idx);
+		const Vector3d &qi = pin_pos.at(idx);
+		const double &ki = it_k->second;
+
+		if (ki <= 0.0)
+			continue;
+
+		for (int i=0; i<3; ++i)
+		{
+			int p_idx = q.size();
+			q.emplace_back(qi[i]*ki);
+			if (replicate)
+			{
+				trips.emplace_back(p_idx, idx*3+i, ki);
+			}
+			else if (i==0)
+			{
+				trips.emplace_back(p_idx/3, idx, ki);	
+			}
+		}
+	}
+
+	bool has_P_updated = P_updated;
+	P_updated = false;
+	return has_P_updated;
+}
+
+bool TriangleMesh::create(
+	const Options *options,
+	const float *verts,
+	int nv,
+	const unsigned int *faces,
+	int nf,
+	const unsigned int *tets,
+	int nt)
+{
+	(void)(options);
+	P_updated = true;
+
+	(void)(tets); (void)(nt);
+	if (nv<=0 || verts == nullptr)
+		return false;
+	if (nf<=0 || faces == nullptr)
+		return false;
+
+	V0.resize(nv,3);
+	for (int i=0; i<nv; ++i)
+	{
+		V0(i,0) = verts[i*3+0];
+		V0(i,1) = verts[i*3+1];
+		V0(i,2) = verts[i*3+2];
+	}
+
+	std::vector<AlignedBox<double,3> > leaves(nf);
+	F.resize(nf,3);
+	for (int i=0; i<nf; ++i)
+	{
+		F(i,0) = faces[i*3+0];
+		F(i,1) = faces[i*3+1];
+		F(i,2) = faces[i*3+2];
+		AlignedBox<double,3> &box = leaves[i];
+		box.setEmpty();
+		box.extend(V0.row(F(i,0)).transpose());
+		box.extend(V0.row(F(i,1)).transpose());
+		box.extend(V0.row(F(i,2)).transpose());
+		box.extend(box.min()-Vector3d::Ones()*1e-4);
+		box.extend(box.max()+Vector3d::Ones()*1e-4);
+	}
+
+	rest_facet_tree.init(leaves);
+	return true;
+}
+
+void TriangleMesh::compute_masses(
+	const Eigen::MatrixXd *x,
+	double density_kgm2,
+	Eigen::VectorXd &m) const
+{
+	int nv = x->rows();
+	m.resize(nv);
+	m.setZero();
+	int nf = F.rows();
+	for (int i=0; i<nf; ++i)
+	{
+		RowVector3i f = F.row(i);
+		Vector3d edge1 = x->row(f[1]) - x->row(f[0]);
+		Vector3d edge2 = x->row(f[2]) - x->row(f[0]);
+		double area = 0.5 * (edge1.cross(edge2)).norm();
+		double tri_mass = density_kgm2 * area;
+		m[f[0]] += tri_mass / 3.0;
+		m[f[1]] += tri_mass / 3.0;
+		m[f[2]] += tri_mass / 3.0;
+	}
+}
+
+void TriangleMesh::set_pin(
+	int idx,
+	const Eigen::Vector3d &p,
+    double k)
+{
+	std::unordered_map<int,double>::const_iterator it = pin_k.find(idx);
+	if (it == pin_k.end()) { P_updated = true; }
+	else if (k != it->second) { P_updated = true; }
+
+	// Remove pin if effectively zero
+	if (k <= 1e-5)
+	{
+		pin_k.erase(idx);
+		pin_pos.erase(idx);
+		return;
+	}
+
+	pin_pos[idx] = p;
+	pin_k[idx] = k;
+}
+
+bool TriangleMesh::linearize_pins(
+	std::vector<Eigen::Triplet<double> > &trips,
+	std::vector<double> &q,
+	std::set<int> &pin_inds,
+    bool replicate) const
+{
+	int np = pin_k.size();
+	trips.reserve((int)trips.size() + np*3);
+	q.reserve((int)q.size() + np*3);
+
+	std::unordered_map<int,double>::const_iterator it_k = pin_k.begin();
+	for (; it_k != pin_k.end(); ++it_k)
+	{
+		int idx = it_k->first;
+		pin_inds.emplace(idx);
+		const Vector3d &qi = pin_pos.at(idx);
+		const double &ki = it_k->second;
+		int p_idx = q.size()/3;
+		for (int i=0; i<3; ++i)
+			q.emplace_back(qi[i]*ki);
+
+		if (replicate)
+		{
+			for (int i=0; i<3; ++i)
+				trips.emplace_back(p_idx*3+i, idx*3+i, ki);
+		}
+		else
+		{
+			trips.emplace_back(p_idx, idx, ki);	
+		}
+	}
+
+	bool has_P_updated = P_updated;
+	P_updated = false;
+	return has_P_updated;
+}
+
+} // namespace admmpd

extern/softbody/src/admmpd_mesh.h

@@ -0,0 +1,297 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_TETMESH_H_
+#define ADMMPD_TETMESH_H_
+
+#include "admmpd_types.h"
+#include "admmpd_bvh.h"
+#include <unordered_map>
+
+namespace admmpd {
+
+class Mesh {
+public:
+    // Returns meshtype (see admmpd_types)
+    virtual int type() const = 0;
+
+    // Copy buffers to internal data, 
+    // calculates BVH/SDF, etc...
+    virtual bool create(
+        const Options *options,
+        const float *verts, // size nv*3
+        int nv,
+        const unsigned int *faces, // size nf*3
+        int nf,
+        const unsigned int *tets, // null or size nt*4
+        int nt) = 0; // If embedded mesh, set to zero
+
+    // ====================
+    //  Accessors
+    // ====================
+
+    virtual const Eigen::MatrixXi *prims() const = 0;
+    virtual const Eigen::MatrixXd *rest_prim_verts() const = 0;
+    virtual const Eigen::MatrixXi *facets() const = 0;
+    virtual const Eigen::MatrixXd *rest_facet_verts() const = 0;
+    virtual const SDFType *rest_facet_sdf() const = 0;
+
+    virtual bool self_collision_allowed() const = 0;
+
+    // Maps primitive vertex to facet vertex. For standard tet meshes
+    // it's just one-to-one, but embedded meshes use bary weighting.
+    virtual Eigen::Vector3d get_mapped_facet_vertex(
+        const Eigen::MatrixXd *prim_verts,
+        int facet_vertex_idx) const = 0;
+
+    // ====================
+    //  Utility
+    // ====================
+
+    virtual void compute_masses(
+        const Eigen::MatrixXd *x,
+        double density_kgm3,
+        Eigen::VectorXd &m) const = 0;
+
+    // ====================
+    //  Pins
+    // ====================
+
+    virtual int num_pins() const = 0;
+
+    // Pins a vertex at a location with stiffness k
+    virtual void set_pin(
+        int idx,
+        const Eigen::Vector3d &p,
+        double k) = 0;
+
+    virtual void clear_pins() = 0;
+
+    // Px=q with stiffnesses baked in
+    // Returns true if P (but not q) has changed from last
+    // call to linearize_pins.
+    virtual bool linearize_pins(
+        std::vector<Eigen::Triplet<double> > &trips,
+        std::vector<double> &q,
+        std::set<int> &pin_inds,
+        bool replicate) const = 0;
+
+}; // class Mesh
+
+
+class EmbeddedMesh : public Mesh {
+protected:
+    Eigen::MatrixXd lat_V0, emb_V0;
+    Eigen::MatrixXi lat_T, emb_F;
+    Eigen::VectorXi emb_v_to_tet; // maps embedded vert to tet
+    Eigen::MatrixXd emb_barys; // barycoords of the embedding
+    std::unordered_map<int,double> emb_pin_k;
+    std::unordered_map<int,Eigen::Vector3d> emb_pin_pos;
+    admmpd::AABBTree<double,3> emb_rest_facet_tree;
+    bool mesh_is_closed;
+    SDFType emb_sdf;
+    mutable bool P_updated; // set to false on linearize_pins
+
+    bool compute_embedding(const admmpd::Options *options);
+
+    // Computes the tet mesh on a subset of faces
+    bool compute_lattice(const admmpd::Options *options);
+
+    // Sets mesh_is_closed
+    void compute_sdf(
+        const Eigen::MatrixXd *emb_v,
+        const Eigen::MatrixXi *emb_f,
+        SDFType *sdf);
+
+public:
+
+    int type() const { return MESHTYPE_EMBEDDED; }
+
+    bool create(
+        const Options *options,
+        const float *verts, // size nv*3
+        int nv,
+        const unsigned int *faces, // size nf*3
+        int nf,
+        const unsigned int *tets, // ignored
+        int nt); // ignored
+
+    const Eigen::MatrixXi *prims() const { return &lat_T; }
+    const Eigen::MatrixXd *rest_prim_verts() const { return &lat_V0; }
+    const Eigen::MatrixXi *facets() const { return &emb_F; }
+    const Eigen::MatrixXd *rest_facet_verts() const { return &emb_V0; }
+    const Eigen::VectorXi *emb_vtx_to_tet() const { return &emb_v_to_tet; }
+    const Eigen::MatrixXd *emb_barycoords() const { return &emb_barys; }
+    const SDFType *rest_facet_sdf() const { return &emb_sdf; }
+    const admmpd::AABBTree<double,3> *emb_rest_tree() const { return &emb_rest_facet_tree; }
+
+    bool self_collision_allowed() const { return mesh_is_closed; }
+
+    Eigen::Vector3d get_mapped_facet_vertex(
+        const Eigen::MatrixXd *prim_verts,
+        int facet_vertex_idx) const;
+
+    void compute_masses(
+        const Eigen::MatrixXd *x,
+        double density_kgm3,
+        Eigen::VectorXd &m) const;
+
+    int num_pins() const { return emb_pin_k.size(); }
+
+    // Set the position of an embedded pin
+    void set_pin(
+        int idx,
+        const Eigen::Vector3d &p,
+        double k);
+
+    void clear_pins()
+    {
+        if (emb_pin_pos.size()) { P_updated=true; }
+        emb_pin_k.clear();
+        emb_pin_pos.clear();
+    }
+
+    // Px=q with stiffnesses baked in
+    bool linearize_pins(
+        std::vector<Eigen::Triplet<double> > &trips,
+        std::vector<double> &q,
+        std::set<int> &pin_inds,
+        bool replicate) const;
+
+}; // class EmbeddedMesh
+
+
+class TetMesh : public Mesh {
+protected:
+    Eigen::MatrixXd V0; // rest verts
+    Eigen::MatrixXi F; // surface faces
+    Eigen::MatrixXi T; // tets
+    std::unordered_map<int,double> pin_k;
+    std::unordered_map<int,Eigen::Vector3d> pin_pos;
+    admmpd::AABBTree<double,3> rest_facet_tree;
+    SDFType rest_sdf;
+    mutable bool P_updated; // set to false on linearize_pins
+
+public:
+
+    int type() const { return MESHTYPE_TET; }
+
+    bool create(
+        const Options *options,
+        const float *verts, // size nv*3
+        int nv,
+        const unsigned int *faces, // size nf*3 (surface faces)
+        int nf,
+        const unsigned int *tets, // size nt*4
+        int nt); // must be > 0
+
+    const Eigen::MatrixXi *facets() const { return &F; }
+    const Eigen::MatrixXd *rest_facet_verts() const { return &V0; }
+    const Eigen::MatrixXi *prims() const { return &T; }
+    const Eigen::MatrixXd *rest_prim_verts() const { return &V0; }
+    const SDFType *rest_facet_sdf() const { return &rest_sdf; }
+
+    // Not yet implemented
+    bool self_collision_allowed() const { return false; }
+
+    Eigen::Vector3d get_mapped_facet_vertex(
+        const Eigen::MatrixXd *prim_verts,
+        int facet_vertex_idx) const
+        { return prim_verts->row(facet_vertex_idx); }
+
+    void compute_masses(
+        const Eigen::MatrixXd *x,
+        double density_kgm3,
+        Eigen::VectorXd &m) const;
+
+    int num_pins() const { return pin_k.size(); }
+
+    void set_pin(
+        int idx,
+        const Eigen::Vector3d &p,
+        double k);
+
+    void clear_pins()
+    {
+        if (pin_pos.size()) { P_updated=true; }
+        pin_k.clear();
+        pin_pos.clear();
+    }
+
+    // pin_inds refers to the index of the embedded vertex
+    bool linearize_pins(
+        std::vector<Eigen::Triplet<double> > &trips,
+        std::vector<double> &q,
+        std::set<int> &pin_inds,
+        bool replicate) const;
+
+}; // class TetMesh
+
+class TriangleMesh : public Mesh {
+protected:
+    Eigen::MatrixXi F;
+    Eigen::MatrixXd V0;
+    std::unordered_map<int,Eigen::Vector3d> pin_pos;
+    std::unordered_map<int,double> pin_k;
+    admmpd::AABBTree<double,3> rest_facet_tree;
+    mutable bool P_updated; // set to false on linearize_pins
+
+public:
+
+    int type() const { return MESHTYPE_TRIANGLE; }
+
+    bool create(
+        const Options *options,
+        const float *verts, // size nv*3
+        int nv,
+        const unsigned int *faces, // size nf*3
+        int nf,
+        const unsigned int *tets, // ignored
+        int nt); // ignored
+
+    const Eigen::MatrixXi *prims() const { return nullptr; }
+    const Eigen::MatrixXd *rest_prim_verts() const { return nullptr; }
+    const Eigen::MatrixXi *facets() const { return &F; }
+    const Eigen::MatrixXd *rest_facet_verts() const { return &V0; }
+    const SDFType *rest_facet_sdf() const { return nullptr; }
+
+    // Not yet implemented
+    bool self_collision_allowed() const { return false; }
+
+    Eigen::Vector3d get_mapped_facet_vertex(
+        const Eigen::MatrixXd *prim_verts,
+        int facet_vertex_idx) const {
+            return prim_verts->row(facet_vertex_idx);
+    }
+
+    void compute_masses(
+        const Eigen::MatrixXd *x,
+        double density_kgm2,
+        Eigen::VectorXd &m) const;
+
+    int num_pins() const { return pin_pos.size(); }
+
+    // Pins a vertex at a location with stiffness k
+    void set_pin(
+        int idx,
+        const Eigen::Vector3d &p,
+        double k);
+
+    void clear_pins() {
+        if (pin_pos.size()) { P_updated=true; }
+        pin_pos.clear();
+        pin_k.clear();
+    }
+
+    // Px=q with stiffnesses baked in
+    bool linearize_pins(
+        std::vector<Eigen::Triplet<double> > &trips,
+        std::vector<double> &q,
+        std::set<int> &pin_inds,
+        bool replicate) const;
+
+}; // class TriangleMesh
+
+} // namespace admmpd
+
+#endif // ADMMPD_LATTICE_H_

extern/softbody/src/admmpd_solver.cpp

@@ -0,0 +1,497 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#include "admmpd_solver.h"
+#include "admmpd_energy.h"
+#include "admmpd_collision.h"
+#include "admmpd_linsolve.h"
+#include "admmpd_geom.h"
+#include "admmpd_log.h"
+
+#include <Eigen/Geometry>
+#include <Eigen/Sparse>
+
+#include <stdio.h>
+#include <iostream>
+#include <unordered_map>
+#include <numeric>
+
+#include "BLI_task.h" // threading
+#include "BLI_assert.h"
+
+namespace admmpd {
+using namespace Eigen;
+
+// Throws an exception for a given function with message
+static inline void throw_err(const std::string &f, const std::string &m)
+{
+	throw std::runtime_error("Solver::"+f+": "+m);
+}
+
+bool Solver::init(
+	const Mesh *mesh,
+    const Options *options,
+    SolverData *data)
+{
+	Logger log(options->log_level);
+	log.start_state(SOLVERSTATE_INIT);
+
+	BLI_assert(data != NULL);
+	BLI_assert(options != NULL);
+	BLI_assert(mesh != NULL);
+
+    data->energies_graph.clear();
+	data->indices.clear();
+	data->rest_volumes.clear();
+	data->weights.clear();
+
+	switch (mesh->type())
+	{
+		default: {
+			throw_err("init","unknown mesh type");
+		} break;
+		case MESHTYPE_EMBEDDED:
+		case MESHTYPE_TET: {
+			data->x = *mesh->rest_prim_verts();
+		} break;
+		case MESHTYPE_TRIANGLE: {
+			data->x = *mesh->rest_facet_verts();
+		} break;
+	}
+
+	BLI_assert(data->x.rows()>0);
+	BLI_assert(data->x.cols()==3);
+	data->v.resize(data->x.rows(), 3);
+	data->v.setZero();
+	mesh->compute_masses(&data->x, options->density_kgm3, data->m);
+	init_matrices(mesh,options,data);
+
+	int ne = data->indices.size();
+	int nx = data->x.rows();
+	if (options->log_level >= LOGLEVEL_LOW) {
+		printf("Solver::init:\n\tNum energy terms: %d\n\tNum verts: %d\n",ne,nx);
+	}
+
+	log.stop_state(SOLVERSTATE_INIT);
+	if (options->log_level >= LOGLEVEL_HIGH) {
+		printf("Timings:\n%s", log.to_string().c_str());
+	}
+
+	return true;
+} // end init
+
+void Solver::solve(
+	const Mesh *mesh,
+	const Options *options,
+	SolverData *data,
+	Collision *collision)
+{
+	Logger log(options->log_level);
+	log.start_state(SOLVERSTATE_SOLVE);
+
+	BLI_assert(mesh != NULL);
+	BLI_assert(options != NULL);
+	BLI_assert(data != NULL);
+	BLI_assert(data->x.cols() == 3);
+	BLI_assert(data->x.rows() > 0);
+	BLI_assert(options->max_admm_iters > 0);
+	double dt = options->timestep_s;
+
+	// If doing CCD, we can do broad phase collision here
+	// and shrink the time step.
+
+	// Init the solve which computes
+	// quantaties like M_xbar and makes sure
+	// the variables are sized correctly.
+	log.start_state(SOLVERSTATE_INIT_SOLVE);
+	init_solve(mesh,options,data,collision);
+	log.stop_state(SOLVERSTATE_INIT_SOLVE);
+
+	if (options->collision_mode == COLLISIONMODE_CONTINUOUS)
+	{
+		// TODO: Perform continuous collision here
+		throw_err("solve","CCD not yet supported");
+	}
+
+	// Begin solver loop
+	int iters = 0;
+	for (; iters < options->max_admm_iters; ++iters)
+	{
+		// Update ADMM z/u
+		log.start_state(SOLVERSTATE_LOCAL_STEP);
+		solve_local_step(options,data);
+		log.stop_state(SOLVERSTATE_LOCAL_STEP);
+
+		// Collision detection
+		log.start_state(SOLVERSTATE_COLLISION_UPDATE);
+		if (collision && options->collision_mode==COLLISIONMODE_DISCRETE)
+		{
+			bool sort_bvh = false;
+			collision->update_bvh(mesh,options,data,&data->x_start,&data->x,sort_bvh);
+			collision->detect(mesh,options,data,&data->x_start,&data->x);
+		}
+		log.stop_state(SOLVERSTATE_COLLISION_UPDATE);
+
+		// Solve Ax=b s.t. Px=q and Cx=d
+		log.start_state(SOLVERSTATE_GLOBAL_STEP);
+		solve_global_step(mesh,options,data,collision);
+		log.stop_state(SOLVERSTATE_GLOBAL_STEP);
+
+		// Check convergence
+		if (options->min_res>0)
+		{
+			log.start_state(SOLVERSTATE_TEST_CONVERGED);
+			double ra = ((data->D*data->x) - data->z).norm();
+			double rx = (data->D*(data->x-data->x_prev)).norm();
+			bool converged = (ra+rx) <= options->min_res;
+			log.stop_state(SOLVERSTATE_TEST_CONVERGED);
+			if (converged)
+				break;
+		}
+
+	} // end solver iters
+
+	// Update velocity (if not static solve)
+	if (dt > 0.0)
+		data->v.noalias() = (data->x-data->x_start)*(1.0/dt);
+
+	log.stop_state(SOLVERSTATE_SOLVE);
+
+	if (options->log_level >= LOGLEVEL_HIGH)
+	{
+		printf("\nTimings:\n%s", log.to_string().c_str());
+		printf("Iters: %d\n", iters);
+	}
+
+	return;
+} // end solve
+
+void Solver::init_solve(
+	const Mesh *mesh,
+	const Options *options,
+	SolverData *data,
+	Collision *collision)
+{
+	BLI_assert(data != NULL);
+	BLI_assert(options != NULL);
+	int nx = data->x.rows();
+	BLI_assert(nx > 0);
+
+	if (data->M_xbar.rows() != nx)
+		data->M_xbar.resize(nx,3);
+
+	// Initialize:
+	// - update velocity with explicit forces
+	// - update pin constraint matrix (goal positions)
+	// - set x init guess
+	double dt = std::max(0.0, options->timestep_s);
+	data->x_start = data->x;
+	data->x_prev = data->x;
+	for (int i=0; i<nx; ++i)
+	{
+		data->v.row(i) += dt*options->grav;
+		RowVector3d xbar_i = data->x.row(i) + dt*data->v.row(i);
+		data->M_xbar.row(i) = data->m[i]*xbar_i / (dt*dt);
+		data->x.row(i) = xbar_i; // initial guess
+	}
+
+	if (collision)
+	{
+		// Sorts BVH tree
+		collision->update_bvh(mesh, options, data, &data->x_start, &data->x, true);
+	}
+
+	// ADMM variables
+	data->Dx.noalias() = data->D * data->x;
+	data->z = data->Dx; // init guess
+	data->u.setZero();
+
+	switch (options->linsolver)
+	{
+		default: {
+			throw_err("init_solve","unknown linsolver");
+		} break;
+		case LINSOLVER_LDLT: {
+			LDLT().init_solve(mesh,options,collision,data);
+		} break;
+		case LINSOLVER_PCG: {
+			ConjugateGradients().init_solve(mesh,options,collision,data);
+		} break;
+		case LINSOLVER_MCGS: {
+			throw_err("init_solve","MCGS solver not yet supported");
+		} break;
+	}
+
+} // end init solve
+
+void Solver::solve_local_step(
+	const Options *options,
+	SolverData *data)
+{
+	BLI_assert(data != NULL);
+	BLI_assert(options != NULL);
+
+	struct LocalStepThreadData {
+		const Options *options;
+		SolverData *data;
+	};
+
+	auto parallel_zu_update = [](
+		void *__restrict userdata,
+		const int i,
+		const TaskParallelTLS *__restrict tls)->void
+	{
+		(void)(tls);
+		LocalStepThreadData *td = (LocalStepThreadData*)userdata;
+		EnergyTerm().update(
+			td->options,
+			td->data->indices[i][0], // index
+			td->data->indices[i][2], // type
+			td->data->rest_volumes[i],
+			td->data->weights[i],
+			&td->data->x,
+			&td->data->Dx,
+			&td->data->z,
+			&td->data->u );
+	}; // end parallel zu update
+
+	data->Dx.noalias() = data->D * data->x;
+	int ne = data->indices.size();
+	BLI_assert(ne > 0);
+  	LocalStepThreadData thread_data = {options, data};
+	TaskParallelSettings settings;
+	BLI_parallel_range_settings_defaults(&settings);
+	BLI_task_parallel_range(0, ne, &thread_data, parallel_zu_update, &settings);
+
+} // end local step
+
+void Solver::solve_global_step(
+	const Mesh *mesh,
+	const Options *options,
+	SolverData *data,
+	Collision *collision)
+{
+	data->x_prev = data->x;
+	switch (options->linsolver)
+	{
+		default: {
+			throw_err("solve_global_step","unknown linsolver");
+		} break;
+		case LINSOLVER_LDLT: {
+			LDLT().solve(mesh,options,collision,data);
+		} break;
+		case LINSOLVER_PCG: {
+			ConjugateGradients().solve(mesh,options,collision,data);
+		} break;
+		case LINSOLVER_MCGS: {
+			throw_err("solve_global_step","MCGS not yet supported");
+		} break;
+	}
+}
+
+void Solver::init_matrices(
+	const Mesh *mesh,
+	const Options *options,
+	SolverData *data)
+{
+	BLI_assert(data != NULL);
+	BLI_assert(options != NULL);
+	int nx = data->x.rows();
+	BLI_assert(nx > 0);
+	BLI_assert(data->x.cols() == 3);
+
+	data->ls.last_pk = -1;
+
+	double dt = options->timestep_s;
+	double dt2 = dt*dt;
+	if (dt2 < 0) // static solve
+		dt2 = 1.0;
+
+	// Allocate per-vertex data
+	data->x_start = data->x;
+	data->M_xbar.resize(nx,3);
+	data->M_xbar.setZero();
+	data->Dx.resize(nx,3);
+	data->Dx.setZero();
+	if (data->v.rows() != nx)
+	{
+		data->v.resize(nx,3);
+		data->v.setZero();
+	}
+
+	// Add per-element energies to data
+	std::vector<Triplet<double> > trips;
+	append_energies(mesh,options,data,trips);
+	if (trips.size()==0)
+	{
+		throw_err("compute_matrices","No reduction coeffs");
+	}
+	int n_row_D = trips.back().row()+1;
+
+	update_weight_matrix(options,data,n_row_D);
+	RowSparseMatrix<double> W2 = data->W*data->W;
+
+	// Mass weighted Laplacian
+	data->D.resize(n_row_D,nx);
+	data->D.setFromTriplets(trips.begin(), trips.end());
+	data->DtW2 = data->D.transpose() * W2;
+	data->A = data->DtW2 * data->D;
+	for (int i=0; i<nx; ++i)
+	{
+		data->A.coeffRef(i,i) += data->m[i]/dt2;
+	}
+
+	// ADMM dual/lagrange
+	data->z.resize(n_row_D,3);
+	data->z.setZero();
+	data->u.resize(n_row_D,3);
+	data->u.setZero();
+
+} // end compute matrices
+
+void Solver::update_weight_matrix(
+	const Options *options,
+	SolverData *data,
+	int rows)
+{
+	(void)(options);
+	data->W.resize(rows,rows);
+	VectorXi W_nnz = VectorXi::Ones(rows);
+	data->W.reserve(W_nnz);
+	int ne = data->indices.size();
+	if (ne != (int)data->weights.size())
+		throw_err("update_weight_matrix","bad num indices/weights");
+
+	for (int i=0; i<ne; ++i)
+	{
+		const Vector3i &idx = data->indices[i];
+		if (idx[0]+idx[1] > rows)
+			throw_err("update_weight_matrix","bad matrix dim");
+
+		for (int j=0; j<idx[1]; ++j)
+			data->W.coeffRef(idx[0]+j,idx[0]+j) = data->weights[i];
+	}
+	data->W.finalize();
+}
+
+void Solver::append_energies(
+	const Mesh *mesh,
+	const Options *options,
+	SolverData *data,
+	std::vector<Triplet<double> > &D_triplets)
+{
+	BLI_assert(mesh != NULL);
+	BLI_assert(options != NULL);
+	BLI_assert(data != NULL);
+
+	const MatrixXi *elems = nullptr;
+	int mesh_type = mesh->type();
+	switch (mesh_type)
+	{
+		default: {
+			throw_err("append_energies","unknown mesh type");
+		} break;
+		case MESHTYPE_EMBEDDED:
+		case MESHTYPE_TET: {
+			elems = mesh->prims(); // tets
+			BLI_assert(elems->cols()==4);
+		} break;
+		case MESHTYPE_TRIANGLE: {
+			elems = mesh->facets(); // triangles
+			BLI_assert(elems->cols()==3);
+		} break;
+	}
+
+	int n_elems = elems->rows();
+	BLI_assert(n_elems > 0);
+
+	int nx = data->x.rows();
+	if ((int)data->energies_graph.size() != nx)
+		data->energies_graph.resize(nx,std::set<int>());
+
+	data->indices.reserve((int)data->indices.size()+n_elems);
+	data->rest_volumes.reserve((int)data->rest_volumes.size()+n_elems);
+	data->weights.reserve((int)data->weights.size()+n_elems);
+
+	// The possibility of having an error in energy initialization
+	// while still wanting to continue the simulation is very low.
+	// We can parallelize this step in the future if needed.
+
+	int energy_index = 0;
+	for (int i=0; i<n_elems; ++i)
+	{
+
+		data->rest_volumes.emplace_back();
+		data->weights.emplace_back();
+		int energy_dim = -1;
+		int energy_type = -1;
+
+		switch (mesh_type)
+		{
+
+			case MESHTYPE_EMBEDDED:
+			case MESHTYPE_TET: {
+				energy_type = ENERGYTERM_TET;
+				RowVector4i ele(
+					elems->operator()(i,0),
+					elems->operator()(i,1),
+					elems->operator()(i,2),
+					elems->operator()(i,3)
+				);
+				energy_dim = EnergyTerm().init_tet(
+					options,
+					energy_index,
+					ele,
+					&data->x,
+					data->rest_volumes.back(),
+					data->weights.back(),
+					D_triplets );
+			} break;
+
+			case MESHTYPE_TRIANGLE: {
+				energy_type = ENERGYTERM_TRIANGLE;
+				RowVector3i ele(
+					elems->operator()(i,0),
+					elems->operator()(i,1),
+					elems->operator()(i,2)
+				);
+				energy_dim = EnergyTerm().init_triangle(
+					options,
+					energy_index,
+					ele,
+					&data->x,
+					data->rest_volumes.back(),
+					data->weights.back(),
+					D_triplets );
+			} break;
+
+		} // end switch mesh type
+
+		// Error in initialization
+		if( energy_dim <= 0 ){
+			data->rest_volumes.pop_back();
+			data->weights.pop_back();
+			continue;
+		}
+
+		// Add stencil to graph
+		int ele_dim = elems->cols();
+		for (int j=0; j<ele_dim; ++j)
+		{
+			int ej = elems->operator()(i,j);
+			for (int k=0; k<ele_dim; ++k)
+			{
+				int ek = elems->operator()(i,k);
+				if (ej==ek)
+					continue;
+				data->energies_graph[ej].emplace(ek);
+			}
+		}
+
+		data->indices.emplace_back(energy_index, energy_dim, energy_type);
+		energy_index += energy_dim;
+	}
+
+} // end append energies
+
+} // namespace admmpd

extern/softbody/src/admmpd_solver.h

@@ -0,0 +1,78 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_SOLVER_H_
+#define ADMMPD_SOLVER_H_
+
+#include "admmpd_types.h"
+#include "admmpd_collision.h"
+#include "admmpd_mesh.h"
+#include "admmpd_linsolve.h"
+
+namespace admmpd {
+
+class Solver {
+public:
+    // Initialies solver data.
+    // Returns true on success
+    bool init(
+        const Mesh *mesh,
+        const Options *options,
+        SolverData *data);
+
+    // Solve a single time step.
+    // Collision ptr can be null.
+    void solve(
+        const Mesh *mesh,
+        const Options *options,
+        SolverData *data,
+        Collision *collision);
+
+protected:
+
+    // Computes start-of-solve quantites.
+    // Collision ptr can be null.
+    void init_solve(
+        const Mesh *mesh,
+        const Options *options,
+        SolverData *data,
+        Collision *collision);
+
+    // Update z and u in parallel, g(Dx)
+	void solve_local_step(
+        const Options *options,
+        SolverData *data);
+
+    // Solves the linear system, f(x)
+    void solve_global_step(
+        const Mesh *mesh,
+        const Options *options,
+        SolverData *data,
+        Collision *collision);
+
+    // Called once at start of simulation.
+    // Computes constant quantities
+    void init_matrices(
+        const Mesh *mesh,
+        const Options *options,
+        SolverData *data);
+
+    // Computes the W matrix
+    // from the current weights
+    void update_weight_matrix(
+        const Options *options,
+        SolverData *data,
+        int rows);
+
+    // Generates energies from a mesh
+	void append_energies(
+        const Mesh *mesh,
+		const Options *options,
+		SolverData *data,
+		std::vector<Eigen::Triplet<double> > &D_triplets);
+
+}; // class ADMMPD_solver
+
+} // namespace admmpd
+
+#endif // ADMMPD_SOLVER_H_

extern/softbody/src/admmpd_timer.h

@@ -0,0 +1,49 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_TIMER_H_
+#define ADMMPD_TIMER_H_
+
+#include <chrono>
+
+namespace admmpd {
+
+// I call it MicroTimer to avoid name clashes. Originally from
+// https://github.com/mattoverby/mclscene/blob/master/include/MCL/MicroTimer.hpp
+class MicroTimer {
+	//typedef std::chrono::high_resolution_clock C;
+	typedef std::chrono::steady_clock C;
+	typedef double T;
+public:
+
+	MicroTimer() : start_time( C::now() ){}
+
+	void reset() { start_time = C::now(); }
+
+	T elapsed_s() const { // seconds
+		curr_time = C::now();
+		std::chrono::duration<T> durr = curr_time-start_time;
+		return durr.count();
+	}
+
+	T elapsed_ms() const { // milliseconds
+		curr_time = C::now();
+		std::chrono::duration<T, std::milli> durr = curr_time-start_time;
+		return durr.count();
+	}
+
+	T elapsed_us() const { // microseconds
+		curr_time = C::now();
+		std::chrono::duration<T, std::micro> durr = curr_time-start_time;
+		return durr.count();
+	}
+
+private:
+	std::chrono::time_point<C> start_time;
+	mutable std::chrono::time_point<C> curr_time;
+
+}; // end class MicroTimer
+
+} // namespace admmpd
+
+#endif // ADMMPD_TIMER_H_

extern/softbody/src/admmpd_types.h

@@ -0,0 +1,185 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_TYPES_H_
+#define ADMMPD_TYPES_H_
+
+#include <Eigen/Geometry>
+#include <Eigen/SparseCholesky>
+#include <thread>
+#include <vector>
+#include <set>
+#include <Discregrid/All> // SDF
+#include "admmpd_bvh.h"
+
+namespace admmpd {
+
+template <typename T> using RowSparseMatrix = Eigen::SparseMatrix<T,Eigen::RowMajor>;
+typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > Cholesky;
+typedef Discregrid::CubicLagrangeDiscreteGrid SDFType;
+
+#define MESHTYPE_EMBEDDED 0
+#define MESHTYPE_TET 1
+#define MESHTYPE_TRIANGLE 2 // i.e. cloth
+#define MESHTYPE_NUM 3
+
+#define ENERGYTERM_TET 0
+#define ENERGYTERM_TRIANGLE 1
+#define ENERGYTERM_NUM 2
+
+#define ELASTIC_ARAP 0 // As-rigid-as-possible
+#define ELASTIC_NH 1 // NeoHookean
+#define ELASTIC_NUM 2
+
+#define COLLISIONMODE_DISCRETE 0
+#define COLLISIONMODE_CONTINUOUS 1
+#define COLLISIONMODE_NUM 2
+
+#define SOLVERSTATE_INIT 0
+#define SOLVERSTATE_MESHCREATE 1
+#define SOLVERSTATE_SOLVE 2
+#define SOLVERSTATE_INIT_SOLVE 3
+#define SOLVERSTATE_LOCAL_STEP 4
+#define SOLVERSTATE_GLOBAL_STEP 5
+#define SOLVERSTATE_COLLISION_UPDATE 6
+#define SOLVERSTATE_TEST_CONVERGED 7
+#define SOLVERSTATE_NUM 8
+
+#define LOGLEVEL_NONE 0
+#define LOGLEVEL_LOW 1
+#define LOGLEVEL_HIGH 2
+#define LOGLEVEL_DEBUG 3
+#define LOGLEVEL_NUM 4
+
+#define LINSOLVER_LDLT 0 // Eigen's LDL^T
+#define LINSOLVER_PCG 1 // Precon. Conj. Grad.
+#define LINSOLVER_MCGS 2 // Multi-Color Gauss-Siedel (not yet supported)
+#define LINSOLVER_NUM 3
+
+struct Options {
+    double timestep_s;
+    int lattice_subdiv; // max subdiv levels for lattice gen
+    int log_level;
+    int linsolver;
+    int max_admm_iters;
+    int max_cg_iters;
+    int max_gs_iters;
+    int max_threads; // -1 = auto (num cpu threads - 1)
+    int elastic_material; // ENUM, see admmpd_energy.h
+    int collision_mode;
+    int substeps; // used externally, ignore in solve()
+    double gs_omega; // Gauss-Seidel relaxation
+    double ck; // collision stiffness
+    double pk; // pin stiffness
+    double min_res; // exit tolerance for global step
+    double youngs; // Young's modulus // TODO variable per-tet
+    double poisson; // Poisson ratio // TODO variable per-tet
+    double density_kgm3; // density of mesh
+    double floor; // floor location
+    //double collision_thickness;
+    bool self_collision; // process self collisions
+    Eigen::Vector2d strain_limit; // min=[-inf,1], max=[1,inf]
+    Eigen::Vector3d grav;
+    Options() :
+        timestep_s(1.0/24.0),
+        lattice_subdiv(3),
+        log_level(LOGLEVEL_NONE),
+        linsolver(LINSOLVER_PCG),
+        max_admm_iters(20),
+        max_cg_iters(10),
+        max_gs_iters(100),
+        max_threads(-1),
+        elastic_material(ELASTIC_ARAP),
+        collision_mode(COLLISIONMODE_DISCRETE),
+        substeps(1),
+        gs_omega(1),
+        ck(10000),
+        pk(10000),
+        min_res(1e-6),
+        youngs(1000000),
+        poisson(0.399),
+        density_kgm3(1522),
+        floor(-std::numeric_limits<double>::max()),
+        //collision_thickness(1e-6),
+        self_collision(false),
+        strain_limit(0,100),
+        grav(0,0,-9.8)
+        {}
+};
+
+class SolverData {
+public:
+    Eigen::MatrixXd x; // vertices, n x 3
+    Eigen::MatrixXd v; // velocity, n x 3
+    Eigen::MatrixXd x_start; // x at t=0 (and goal if k>0), n x 3
+    Eigen::MatrixXd x_prev; // x at k-1
+    Eigen::VectorXd m; // masses, n x 1
+    Eigen::MatrixXd z; // ADMM z variable
+    Eigen::MatrixXd u; // ADMM u aug lag with W inv
+    Eigen::MatrixXd M_xbar; // M*(x + dt v)
+    Eigen::MatrixXd Dx; // D * x
+    RowSparseMatrix<double> D; // reduction matrix
+    RowSparseMatrix<double> DtW2; // D'W'W
+    RowSparseMatrix<double> A; // M + DtW'WD
+    RowSparseMatrix<double> W; // weight matrix
+    // Set in append_energies:
+    std::vector<std::set<int> > energies_graph; // per-vertex adjacency list (graph)
+	std::vector<Eigen::Vector3i> indices; // per-energy index into D (row, num rows, type)
+	std::vector<double> rest_volumes; // per-energy rest volume
+	std::vector<double> weights; // per-energy weights
+    struct LinSolveData {
+        LinSolveData() : last_pk(0) {}
+        LinSolveData(const LinSolveData &src); // see comments below
+        mutable std::unique_ptr<Cholesky> ldlt_A_PtP; // see copy constructor
+        double last_pk; // buffered to flag P update
+        Eigen::MatrixXd rhs; // Mxbar + DtW2(z-u) + Ptq + Ctd
+        Eigen::MatrixXd Ptq;
+        Eigen::MatrixXd Ctd;
+        Eigen::SparseMatrix<double> A_PtP;
+        Eigen::SparseMatrix<double> A_PtP_3; // replicated
+        RowSparseMatrix<double> A_PtP_CtC_3;
+        Eigen::MatrixXd r;
+        Eigen::MatrixXd z;
+        Eigen::MatrixXd p;
+        Eigen::VectorXd p3;
+        Eigen::MatrixXd Ap;
+    } ls;
+    struct CollisionData {
+        std::set<int> selfcollision_verts; // inds to test self collision
+        std::vector<Eigen::AlignedBox<double,3> > prim_boxes;
+        AABBTree<double,3> prim_tree;
+    } col;
+};
+
+static inline int get_max_threads(const Options *options)
+{
+    if (options->max_threads > 0)
+        return options->max_threads;
+    return std::max(1,(int)std::thread::hardware_concurrency()-1);
+}
+
+// Copying the LinSolveData is a special operation.
+// Basically everything can be copied easily except the
+// Cholesky decomp (the one thing we want to avoid recomputing).
+// Because of this, we "move" the copied SolverData's ptr
+// to the new copy, thus setting src's ptr to null. 
+inline SolverData::LinSolveData::LinSolveData(const LinSolveData &src)
+{
+    this->last_pk = src.last_pk;
+    this->rhs = src.rhs;
+    this->Ptq = src.Ptq;
+    this->Ctd = src.Ctd;
+    this->A_PtP = src.A_PtP;
+    this->A_PtP_3 = src.A_PtP_3;
+    this->A_PtP_CtC_3 = src.A_PtP_CtC_3;
+    this->r = src.r;
+    this->z = src.z;
+    this->p = src.p;
+    this->p3 = src.p3;
+    this->Ap = src.Ap;
+    this->ldlt_A_PtP = std::move(src.ldlt_A_PtP);
+}
+
+} // namespace admmpd
+
+#endif // ADMMPD_TYPES_H_

extern/softbody/src/svd/ImplicitQRSVD.h

@@ -0,0 +1,878 @@
+/**
+Copyright (c) 2016 Theodore Gast, Chuyuan Fu, Chenfanfu Jiang, Joseph Teran
+
+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.
+
+If the code is used in an article, the following paper shall be cited:
+@techreport{qrsvd:2016,
+  title={Implicit-shifted Symmetric QR Singular Value Decomposition of 3x3 Matrices},
+  author={Gast, Theodore and Fu, Chuyuan and Jiang, Chenfanfu and Teran, Joseph},
+  year={2016},
+  institution={University of California Los Angeles}
+}
+
+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.
+
+################################################################################
+This file implements 2D and 3D polar decompositions and SVDs.
+
+T may be float or double.
+
+2D Polar:
+    Eigen::Matrix<T, 2, 2> A,R,S;
+    A<<1,2,3,4;
+    JIXIE::polarDecomposition(A, R, S);
+    // R will be the closest rotation to A
+    // S will be symmetric
+
+2D SVD:
+    Eigen::Matrix<T, 2, 2> A;
+    A<<1,2,3,4;
+    Eigen::Matrix<T, 2, 1> S;
+    Eigen::Matrix<T, 2, 2> U;
+    Eigen::Matrix<T, 2, 2> V;
+    JIXIE::singularValueDecomposition(A,U,S,V);
+    // A = U S V'
+    // U and V will be rotations
+    // S will be singular values sorted by decreasing magnitude. Only the last one may be negative.
+
+3D Polar:
+    Eigen::Matrix<T, 3, 3> A,R,S;
+    A<<1,2,3,4,5,6;
+    JIXIE::polarDecomposition(A, R, S);
+    // R will be the closest rotation to A
+    // S will be symmetric
+
+3D SVD:
+    Eigen::Matrix<T, 3, 3> A;
+    A<<1,2,3,4,5,6;
+    Eigen::Matrix<T, 3, 1> S;
+    Eigen::Matrix<T, 3, 3> U;
+    Eigen::Matrix<T, 3, 3> V;
+    JIXIE::singularValueDecomposition(A,U,S,V);
+    // A = U S V'
+    // U and V will be rotations
+    // S will be singular values sorted by decreasing magnitude. Only the last one may be negative.
+
+################################################################################
+*/
+
+/**
+SVD based on implicit QR with Wilkinson Shift
+*/
+#ifndef JIXIE_IMPLICIT_QR_SVD_H
+#define JIXIE_IMPLICIT_QR_SVD_H
+
+#include "Tools.h"
+
+namespace JIXIE {
+
+/**
+    Class for givens rotation.
+    Row rotation G*A corresponds to something like
+    c -s  0
+    ( s  c  0 ) A
+    0  0  1
+    Column rotation A G' corresponds to something like
+    c -s  0
+    A ( s  c  0 )
+    0  0  1
+
+    c and s are always computed so that
+    ( c -s ) ( a )  =  ( * )
+    s  c     b       ( 0 )
+
+    Assume rowi<rowk.
+    */
+template <class T>
+class GivensRotation {
+public:
+    int rowi;
+    int rowk;
+    T c;
+    T s;
+
+    inline GivensRotation(int rowi_in, int rowk_in)
+        : rowi(rowi_in)
+        , rowk(rowk_in)
+        , c(1)
+        , s(0)
+    {
+    }
+
+    inline GivensRotation(T a, T b, int rowi_in, int rowk_in)
+        : rowi(rowi_in)
+        , rowk(rowk_in)
+    {
+        compute(a, b);
+    }
+
+    ~GivensRotation() {}
+
+    inline void transposeInPlace()
+    {
+        s = -s;
+    }
+
+    /**
+        Compute c and s from a and b so that
+        ( c -s ) ( a )  =  ( * )
+        s  c     b       ( 0 )
+        */
+    inline void compute(const T a, const T b)
+    {
+        using std::sqrt;
+
+        T d = a * a + b * b;
+        c = 1;
+        s = 0;
+        if (d != 0) {
+            // T t = 1 / sqrt(d);
+            T t = JIXIE::MATH_TOOLS::rsqrt(d);
+            c = a * t;
+            s = -b * t;
+        }
+    }
+
+    /**
+        This function computes c and s so that
+        ( c -s ) ( a )  =  ( 0 )
+        s  c     b       ( * )
+        */
+    inline void computeUnconventional(const T a, const T b)
+    {
+        using std::sqrt;
+
+        T d = a * a + b * b;
+        c = 0;
+        s = 1;
+        if (d != 0) {
+            // T t = 1 / sqrt(d);
+            T t = JIXIE::MATH_TOOLS::rsqrt(d);
+            s = a * t;
+            c = b * t;
+        }
+    }
+    /**
+      Fill the R with the entries of this rotation
+        */
+    template <class MatrixType>
+    inline void fill(const MatrixType& R) const
+    {
+        MatrixType& A = const_cast<MatrixType&>(R);
+        A = MatrixType::Identity();
+        A(rowi, rowi) = c;
+        A(rowk, rowi) = -s;
+        A(rowi, rowk) = s;
+        A(rowk, rowk) = c;
+    }
+
+    /**
+        This function does something like
+        c -s  0
+        ( s  c  0 ) A -> A
+        0  0  1
+        It only affects row i and row k of A.
+        */
+    template <class MatrixType>
+    inline void rowRotation(MatrixType& A) const
+    {
+        for (int j = 0; j < MatrixType::ColsAtCompileTime; j++) {
+            T tau1 = A(rowi, j);
+            T tau2 = A(rowk, j);
+            A(rowi, j) = c * tau1 - s * tau2;
+            A(rowk, j) = s * tau1 + c * tau2;
+        }
+    }
+
+    /**
+        This function does something like
+        c  s  0
+        A ( -s  c  0 )  -> A
+        0  0  1
+        It only affects column i and column k of A.
+        */
+    template <class MatrixType>
+    inline void columnRotation(MatrixType& A) const
+    {
+        for (int j = 0; j < MatrixType::RowsAtCompileTime; j++) {
+            T tau1 = A(j, rowi);
+            T tau2 = A(j, rowk);
+            A(j, rowi) = c * tau1 - s * tau2;
+            A(j, rowk) = s * tau1 + c * tau2;
+        }
+    }
+
+    /**
+      Multiply givens must be for same row and column
+      **/
+    inline void operator*=(const GivensRotation<T>& A)
+    {
+        T new_c = c * A.c - s * A.s;
+        T new_s = s * A.c + c * A.s;
+        c = new_c;
+        s = new_s;
+    }
+
+    /**
+      Multiply givens must be for same row and column
+      **/
+    inline GivensRotation<T> operator*(const GivensRotation<T>& A) const
+    {
+        GivensRotation<T> r(*this);
+        r *= A;
+        return r;
+    }
+};
+
+/**
+    \brief zero chasing the 3X3 matrix to bidiagonal form
+    original form of H:   x x 0
+    x x x
+    0 0 x
+    after zero chase:
+    x x 0
+    0 x x
+    0 0 x
+    */
+template <class T>
+inline void zeroChase(Eigen::Matrix<T, 3, 3>& H, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 3>& V)
+{
+
+    /**
+        Reduce H to of form
+        x x +
+        0 x x
+        0 0 x
+        */
+    GivensRotation<T> r1(H(0, 0), H(1, 0), 0, 1);
+    /**
+        Reduce H to of form
+        x x 0
+        0 x x
+        0 + x
+        Can calculate r2 without multiplying by r1 since both entries are in first two
+        rows thus no need to divide by sqrt(a^2+b^2)
+        */
+    GivensRotation<T> r2(1, 2);
+    if (H(1, 0) != 0)
+        r2.compute(H(0, 0) * H(0, 1) + H(1, 0) * H(1, 1), H(0, 0) * H(0, 2) + H(1, 0) * H(1, 2));
+    else
+        r2.compute(H(0, 1), H(0, 2));
+
+    r1.rowRotation(H);
+
+    /* GivensRotation<T> r2(H(0, 1), H(0, 2), 1, 2); */
+    r2.columnRotation(H);
+    r2.columnRotation(V);
+
+    /**
+        Reduce H to of form
+        x x 0
+        0 x x
+        0 0 x
+        */
+    GivensRotation<T> r3(H(1, 1), H(2, 1), 1, 2);
+    r3.rowRotation(H);
+
+    // Save this till end for better cache coherency
+    // r1.rowRotation(u_transpose);
+    // r3.rowRotation(u_transpose);
+    r1.columnRotation(U);
+    r3.columnRotation(U);
+}
+
+/**
+     \brief make a 3X3 matrix to upper bidiagonal form
+     original form of H:   x x x
+                           x x x
+                           x x x
+     after zero chase:
+                           x x 0
+                           0 x x
+                           0 0 x
+  */
+template <class T>
+inline void makeUpperBidiag(Eigen::Matrix<T, 3, 3>& H, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 3>& V)
+{
+    U = Eigen::Matrix<T, 3, 3>::Identity();
+    V = Eigen::Matrix<T, 3, 3>::Identity();
+
+    /**
+      Reduce H to of form
+                          x x x
+                          x x x
+                          0 x x
+    */
+
+    GivensRotation<T> r(H(1, 0), H(2, 0), 1, 2);
+    r.rowRotation(H);
+    // r.rowRotation(u_transpose);
+    r.columnRotation(U);
+    // zeroChase(H, u_transpose, V);
+    zeroChase(H, U, V);
+}
+
+/**
+     \brief make a 3X3 matrix to lambda shape
+     original form of H:   x x x
+     *                     x x x
+     *                     x x x
+     after :
+     *                     x 0 0
+     *                     x x 0
+     *                     x 0 x
+  */
+template <class T>
+inline void makeLambdaShape(Eigen::Matrix<T, 3, 3>& H, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 3>& V)
+{
+    U = Eigen::Matrix<T, 3, 3>::Identity();
+    V = Eigen::Matrix<T, 3, 3>::Identity();
+
+    /**
+      Reduce H to of form
+      *                    x x 0
+      *                    x x x
+      *                    x x x
+      */
+
+    GivensRotation<T> r1(H(0, 1), H(0, 2), 1, 2);
+    r1.columnRotation(H);
+    r1.columnRotation(V);
+
+    /**
+      Reduce H to of form
+      *                    x x 0
+      *                    x x 0
+      *                    x x x
+      */
+
+    r1.computeUnconventional(H(1, 2), H(2, 2));
+    r1.rowRotation(H);
+    r1.columnRotation(U);
+
+    /**
+      Reduce H to of form
+      *                    x x 0
+      *                    x x 0
+      *                    x 0 x
+      */
+
+    GivensRotation<T> r2(H(2, 0), H(2, 1), 0, 1);
+    r2.columnRotation(H);
+    r2.columnRotation(V);
+
+    /**
+      Reduce H to of form
+      *                    x 0 0
+      *                    x x 0
+      *                    x 0 x
+      */
+    r2.computeUnconventional(H(0, 1), H(1, 1));
+    r2.rowRotation(H);
+    r2.columnRotation(U);
+}
+
+/**
+   \brief 2x2 polar decomposition.
+   \param[in] A matrix.
+   \param[out] R Robustly a rotation matrix in givens form
+   \param[out] S_Sym Symmetric. Whole matrix is stored
+
+   Whole matrix S is stored since its faster to calculate due to simd vectorization
+   Polar guarantees negative sign is on the small magnitude singular value.
+   S is guaranteed to be the closest one to identity.
+   R is guaranteed to be the closest rotation to A.
+*/
+template <class TA, class T, class TS>
+inline enable_if_t<isSize<TA>(2, 2) && isSize<TS>(2, 2)>
+polarDecomposition(const Eigen::MatrixBase<TA>& A,
+    GivensRotation<T>& R,
+    const Eigen::MatrixBase<TS>& S_Sym)
+{
+    Eigen::Matrix<T, 2, 1> x(A(0, 0) + A(1, 1), A(1, 0) - A(0, 1));
+    T denominator = x.norm();
+    R.c = (T)1;
+    R.s = (T)0;
+    if (denominator != 0) {
+        /*
+          No need to use a tolerance here because x(0) and x(1) always have
+          smaller magnitude then denominator, therefore overflow never happens.
+        */
+        R.c = x(0) / denominator;
+        R.s = -x(1) / denominator;
+    }
+    Eigen::MatrixBase<TS>& S = const_cast<Eigen::MatrixBase<TS>&>(S_Sym);
+    S = A;
+    R.rowRotation(S);
+}
+
+/**
+   \brief 2x2 polar decomposition.
+   \param[in] A matrix.
+   \param[out] R Robustly a rotation matrix.
+   \param[out] S_Sym Symmetric. Whole matrix is stored
+
+   Whole matrix S is stored since its faster to calculate due to simd vectorization
+   Polar guarantees negative sign is on the small magnitude singular value.
+   S is guaranteed to be the closest one to identity.
+   R is guaranteed to be the closest rotation to A.
+*/
+template <class TA, class TR, class TS>
+inline enable_if_t<isSize<TA>(2, 2) && isSize<TR>(2, 2) && isSize<TS>(2, 2)>
+polarDecomposition(const Eigen::MatrixBase<TA>& A,
+    const Eigen::MatrixBase<TR>& R,
+    const Eigen::MatrixBase<TS>& S_Sym)
+{
+    using T = ScalarType<TA>;
+    GivensRotation<T> r(0, 1);
+    polarDecomposition(A, r, S_Sym);
+    r.fill(R);
+}
+
+/**
+   \brief 2x2 SVD (singular value decomposition) A=USV'
+   \param[in] A Input matrix.
+   \param[out] U Robustly a rotation matrix in Givens form
+   \param[out] Sigma Vector of singular values sorted with decreasing magnitude. The second one can be negative.
+   \param[out] V Robustly a rotation matrix in Givens form
+*/
+template <class TA, class T, class Ts>
+inline enable_if_t<isSize<TA>(2, 2) && isSize<Ts>(2, 1)>
+singularValueDecomposition(
+    const Eigen::MatrixBase<TA>& A,
+    GivensRotation<T>& U,
+    const Eigen::MatrixBase<Ts>& Sigma,
+    GivensRotation<T>& V,
+    const ScalarType<TA> tol = 64 * std::numeric_limits<ScalarType<TA> >::epsilon())
+{
+	(void)(tol);
+    using std::sqrt;
+    Eigen::MatrixBase<Ts>& sigma = const_cast<Eigen::MatrixBase<Ts>&>(Sigma);
+
+    Eigen::Matrix<T, 2, 2> S_Sym;
+    polarDecomposition(A, U, S_Sym);
+    T cosine, sine;
+    T x = S_Sym(0, 0);
+    T y = S_Sym(0, 1);
+    T z = S_Sym(1, 1);
+    if (y == 0) {
+        // S is already diagonal
+        cosine = 1;
+        sine = 0;
+        sigma(0) = x;
+        sigma(1) = z;
+    }
+    else {
+        T tau = 0.5 * (x - z);
+        T w = sqrt(tau * tau + y * y);
+        // w > y > 0
+        T t;
+        if (tau > 0) {
+            // tau + w > w > y > 0 ==> division is safe
+            t = y / (tau + w);
+        }
+        else {
+            // tau - w < -w < -y < 0 ==> division is safe
+            t = y / (tau - w);
+        }
+        cosine = T(1) / sqrt(t * t + T(1));
+        sine = -t * cosine;
+        /*
+          V = [cosine -sine; sine cosine]
+          Sigma = V'SV. Only compute the diagonals for efficiency.
+          Also utilize symmetry of S and don't form V yet.
+        */
+        T c2 = cosine * cosine;
+        T csy = 2 * cosine * sine * y;
+        T s2 = sine * sine;
+        sigma(0) = c2 * x - csy + s2 * z;
+        sigma(1) = s2 * x + csy + c2 * z;
+    }
+
+    // Sorting
+    // Polar already guarantees negative sign is on the small magnitude singular value.
+    if (sigma(0) < sigma(1)) {
+        std::swap(sigma(0), sigma(1));
+        V.c = -sine;
+        V.s = cosine;
+    }
+    else {
+        V.c = cosine;
+        V.s = sine;
+    }
+    U *= V;
+}
+/**
+   \brief 2x2 SVD (singular value decomposition) A=USV'
+   \param[in] A Input matrix.
+   \param[out] U Robustly a rotation matrix.
+   \param[out] Sigma Vector of singular values sorted with decreasing magnitude. The second one can be negative.
+   \param[out] V Robustly a rotation matrix.
+*/
+template <class TA, class TU, class Ts, class TV>
+inline enable_if_t<isSize<TA>(2, 2) && isSize<TU>(2, 2) && isSize<TV>(2, 2) && isSize<Ts>(2, 1)>
+singularValueDecomposition(
+    const Eigen::MatrixBase<TA>& A,
+    const Eigen::MatrixBase<TU>& U,
+    const Eigen::MatrixBase<Ts>& Sigma,
+    const Eigen::MatrixBase<TV>& V,
+    const ScalarType<TA> tol = 64 * std::numeric_limits<ScalarType<TA> >::epsilon())
+{
+    using T = ScalarType<TA>;
+    GivensRotation<T> gv(0, 1);
+    GivensRotation<T> gu(0, 1);
+    singularValueDecomposition(A, gu, Sigma, gv);
+
+    gu.fill(U);
+    gv.fill(V);
+}
+
+/**
+  \brief compute wilkinsonShift of the block
+  a1     b1
+  b1     a2
+  based on the wilkinsonShift formula
+  mu = c + d - sign (d) \ sqrt (d*d + b*b), where d = (a-c)/2
+
+  */
+template <class T>
+T wilkinsonShift(const T a1, const T b1, const T a2)
+{
+    using std::sqrt;
+    using std::fabs;
+    using std::copysign;
+
+    T d = (T)0.5 * (a1 - a2);
+    T bs = b1 * b1;
+
+    T mu = a2 - copysign(bs / (fabs(d) + sqrt(d * d + bs)), d);
+    // T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
+    return mu;
+}
+
+/**
+  \brief Helper function of 3X3 SVD for processing 2X2 SVD
+  */
+template <int t, class T>
+inline void process(Eigen::Matrix<T, 3, 3>& B, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma, Eigen::Matrix<T, 3, 3>& V)
+{
+    int other = (t == 1) ? 0 : 2;
+    GivensRotation<T> u(0, 1);
+    GivensRotation<T> v(0, 1);
+    sigma(other) = B(other, other);
+    singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
+    u.rowi += t;
+    u.rowk += t;
+    v.rowi += t;
+    v.rowk += t;
+    u.columnRotation(U);
+    v.columnRotation(V);
+}
+
+/**
+  \brief Helper function of 3X3 SVD for flipping signs due to flipping signs of sigma
+  */
+template <class T>
+inline void flipSign(int i, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma)
+{
+    sigma(i) = -sigma(i);
+    U.col(i) = -U.col(i);
+}
+
+/**
+  \brief Helper function of 3X3 SVD for sorting singular values
+  */
+template <int t, class T>
+enable_if_t<t == 0> sort(Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma, Eigen::Matrix<T, 3, 3>& V)
+{
+    using std::fabs;
+
+    // Case: sigma(0) > |sigma(1)| >= |sigma(2)|
+    if (fabs(sigma(1)) >= fabs(sigma(2))) {
+        if (sigma(1) < 0) {
+            flipSign(1, U, sigma);
+            flipSign(2, U, sigma);
+        }
+        return;
+    }
+
+    //fix sign of sigma for both cases
+    if (sigma(2) < 0) {
+        flipSign(1, U, sigma);
+        flipSign(2, U, sigma);
+    }
+
+    //swap sigma(1) and sigma(2) for both cases
+    std::swap(sigma(1), sigma(2));
+    U.col(1).swap(U.col(2));
+    V.col(1).swap(V.col(2));
+
+    // Case: |sigma(2)| >= sigma(0) > |simga(1)|
+    if (sigma(1) > sigma(0)) {
+        std::swap(sigma(0), sigma(1));
+        U.col(0).swap(U.col(1));
+        V.col(0).swap(V.col(1));
+    }
+
+    // Case: sigma(0) >= |sigma(2)| > |simga(1)|
+    else {
+        U.col(2) = -U.col(2);
+        V.col(2) = -V.col(2);
+    }
+}
+
+/**
+  \brief Helper function of 3X3 SVD for sorting singular values
+  */
+template <int t, class T>
+enable_if_t<t == 1> sort(Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma, Eigen::Matrix<T, 3, 3>& V)
+{
+    using std::fabs;
+
+    // Case: |sigma(0)| >= sigma(1) > |sigma(2)|
+    if (fabs(sigma(0)) >= sigma(1)) {
+        if (sigma(0) < 0) {
+            flipSign(0, U, sigma);
+            flipSign(2, U, sigma);
+        }
+        return;
+    }
+
+    //swap sigma(0) and sigma(1) for both cases
+    std::swap(sigma(0), sigma(1));
+    U.col(0).swap(U.col(1));
+    V.col(0).swap(V.col(1));
+
+    // Case: sigma(1) > |sigma(2)| >= |sigma(0)|
+    if (fabs(sigma(1)) < fabs(sigma(2))) {
+        std::swap(sigma(1), sigma(2));
+        U.col(1).swap(U.col(2));
+        V.col(1).swap(V.col(2));
+    }
+
+    // Case: sigma(1) >= |sigma(0)| > |sigma(2)|
+    else {
+        U.col(1) = -U.col(1);
+        V.col(1) = -V.col(1);
+    }
+
+    // fix sign for both cases
+    if (sigma(1) < 0) {
+        flipSign(1, U, sigma);
+        flipSign(2, U, sigma);
+    }
+}
+
+/**
+  \brief 3X3 SVD (singular value decomposition) A=USV'
+  \param[in] A Input matrix.
+  \param[out] U is a rotation matrix.
+  \param[out] sigma Diagonal matrix, sorted with decreasing magnitude. The third one can be negative.
+  \param[out] V is a rotation matrix.
+  */
+template <class T>
+inline int singularValueDecomposition(const Eigen::Matrix<T, 3, 3>& A,
+    Eigen::Matrix<T, 3, 3>& U,
+    Eigen::Matrix<T, 3, 1>& sigma,
+    Eigen::Matrix<T, 3, 3>& V,
+    T tol = 1024 * std::numeric_limits<T>::epsilon())
+{
+    using std::fabs;
+    using std::sqrt;
+    using std::max;
+    Eigen::Matrix<T, 3, 3> B = A;
+    U = Eigen::Matrix<T, 3, 3>::Identity();
+    V = Eigen::Matrix<T, 3, 3>::Identity();
+
+    makeUpperBidiag(B, U, V);
+
+    int count = 0;
+    T mu = (T)0;
+    GivensRotation<T> r(0, 1);
+
+    T alpha_1 = B(0, 0);
+    T beta_1 = B(0, 1);
+    T alpha_2 = B(1, 1);
+    T alpha_3 = B(2, 2);
+    T beta_2 = B(1, 2);
+    T gamma_1 = alpha_1 * beta_1;
+    T gamma_2 = alpha_2 * beta_2;
+    tol *= max((T)0.5 * sqrt(alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2), (T)1);
+
+    /**
+      Do implicit shift QR until A^T A is block diagonal
+      */
+
+    while (fabs(beta_2) > tol && fabs(beta_1) > tol
+        && fabs(alpha_1) > tol && fabs(alpha_2) > tol
+        && fabs(alpha_3) > tol) {
+        mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2);
+
+        r.compute(alpha_1 * alpha_1 - mu, gamma_1);
+        r.columnRotation(B);
+
+        r.columnRotation(V);
+        zeroChase(B, U, V);
+
+        alpha_1 = B(0, 0);
+        beta_1 = B(0, 1);
+        alpha_2 = B(1, 1);
+        alpha_3 = B(2, 2);
+        beta_2 = B(1, 2);
+        gamma_1 = alpha_1 * beta_1;
+        gamma_2 = alpha_2 * beta_2;
+        count++;
+    }
+    /**
+      Handle the cases of one of the alphas and betas being 0
+      Sorted by ease of handling and then frequency
+      of occurrence
+
+      If B is of form
+      x x 0
+      0 x 0
+      0 0 x
+      */
+    if (fabs(beta_2) <= tol) {
+        process<0>(B, U, sigma, V);
+        sort<0>(U, sigma, V);
+    }
+    /**
+      If B is of form
+      x 0 0
+      0 x x
+      0 0 x
+      */
+    else if (fabs(beta_1) <= tol) {
+        process<1>(B, U, sigma, V);
+        sort<1>(U, sigma, V);
+    }
+    /**
+      If B is of form
+      x x 0
+      0 0 x
+      0 0 x
+      */
+    else if (fabs(alpha_2) <= tol) {
+        /**
+        Reduce B to
+        x x 0
+        0 0 0
+        0 0 x
+        */
+        GivensRotation<T> r1(1, 2);
+        r1.computeUnconventional(B(1, 2), B(2, 2));
+        r1.rowRotation(B);
+        r1.columnRotation(U);
+
+        process<0>(B, U, sigma, V);
+        sort<0>(U, sigma, V);
+    }
+    /**
+      If B is of form
+      x x 0
+      0 x x
+      0 0 0
+      */
+    else if (fabs(alpha_3) <= tol) {
+        /**
+        Reduce B to
+        x x +
+        0 x 0
+        0 0 0
+        */
+        GivensRotation<T> r1(1, 2);
+        r1.compute(B(1, 1), B(1, 2));
+        r1.columnRotation(B);
+        r1.columnRotation(V);
+        /**
+        Reduce B to
+        x x 0
+        + x 0
+        0 0 0
+        */
+        GivensRotation<T> r2(0, 2);
+        r2.compute(B(0, 0), B(0, 2));
+        r2.columnRotation(B);
+        r2.columnRotation(V);
+
+        process<0>(B, U, sigma, V);
+        sort<0>(U, sigma, V);
+    }
+    /**
+      If B is of form
+      0 x 0
+      0 x x
+      0 0 x
+      */
+    else if (fabs(alpha_1) <= tol) {
+        /**
+        Reduce B to
+        0 0 +
+        0 x x
+        0 0 x
+        */
+        GivensRotation<T> r1(0, 1);
+        r1.computeUnconventional(B(0, 1), B(1, 1));
+        r1.rowRotation(B);
+        r1.columnRotation(U);
+
+        /**
+        Reduce B to
+        0 0 0
+        0 x x
+        0 + x
+        */
+        GivensRotation<T> r2(0, 2);
+        r2.computeUnconventional(B(0, 2), B(2, 2));
+        r2.rowRotation(B);
+        r2.columnRotation(U);
+
+        process<1>(B, U, sigma, V);
+        sort<1>(U, sigma, V);
+    }
+
+    return count;
+}
+
+/**
+       \brief 3X3 polar decomposition.
+       \param[in] A matrix.
+       \param[out] R Robustly a rotation matrix.
+       \param[out] S_Sym Symmetric. Whole matrix is stored
+
+       Whole matrix S is stored
+       Polar guarantees negative sign is on the small magnitude singular value.
+       S is guaranteed to be the closest one to identity.
+       R is guaranteed to be the closest rotation to A.
+    */
+template <class T>
+inline void polarDecomposition(const Eigen::Matrix<T, 3, 3>& A,
+    Eigen::Matrix<T, 3, 3>& R,
+    Eigen::Matrix<T, 3, 3>& S_Sym)
+{
+    Eigen::Matrix<T, 3, 3> U;
+    Eigen::Matrix<T, 3, 1> sigma;
+    Eigen::Matrix<T, 3, 3> V;
+
+    singularValueDecomposition(A, U, sigma, V);
+    R.noalias() = U * V.transpose();
+    S_Sym.noalias() = V * Eigen::DiagonalMatrix<T, 3, 3>(sigma) * V.transpose();
+}
+}
+#endif

extern/softbody/src/svd/Tools.h

@@ -0,0 +1,199 @@
+/**
+Copyright (c) 2016 Theodore Gast, Chuyuan Fu, Chenfanfu Jiang, Joseph Teran
+
+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.
+
+If the code is used in an article, the following paper shall be cited:
+@techreport{qrsvd:2016,
+  title={Implicit-shifted Symmetric QR Singular Value Decomposition of 3x3 Matrices},
+  author={Gast, Theodore and Fu, Chuyuan and Jiang, Chenfanfu and Teran, Joseph},
+  year={2016},
+  institution={University of California Los Angeles}
+}
+
+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.
+
+################################################################################
+This file provides a random number generator and a timer.
+Sample usage:
+    RandomNumber<float> rand;
+    float x = randReal(-0.5, 0.8);
+
+    Timer timer;
+    timer.start();
+    SOME CODE A
+    std::cout<<"CODE A took "<<timer.click()<<" seconds"<<std::endl;
+    SOME CODE B
+    std::cout<<"CODE B took "<<timer.click()<<" seconds"<<std::endl;
+################################################################################
+*/
+
+#ifndef JIXIE_SVD_TOOLS_H
+#define JIXIE_SVD_TOOLS_H
+
+//#pragma GCC diagnostic push
+//#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#include <Eigen/Dense>
+#include <Eigen/Core>
+#include <Eigen/SVD>
+//#pragma GCC diagnostic pop
+
+#include <mmintrin.h>
+#include <xmmintrin.h>
+#include <cmath>
+#include <random>
+#include <chrono>
+#include <iostream>
+#include <iomanip>
+
+namespace JIXIE {
+    
+template< bool B, class T = void >
+using enable_if_t = typename std::enable_if<B,T>::type;
+
+/**
+    Random number generator.
+*/
+template <class T>
+class RandomNumber {
+public:
+    std::mt19937 generator;
+
+    RandomNumber(unsigned s = 123)
+        : generator(s)
+    {
+    }
+
+    ~RandomNumber()
+    {
+    }
+
+    /**
+       Random real number from an interval
+    */
+    T randReal(T a, T b)
+    {
+        std::uniform_real_distribution<T> distribution(a, b);
+        return distribution(generator);
+    }
+
+    /**
+      Fill with uniform random numbers
+    */
+    template <class Derived>
+    void fill(Eigen::DenseBase<Derived>& x, T a, T b)
+    {
+        for (typename Derived::Index i = 0; i < x.size(); i++)
+            x(i) = randReal(a, b);
+    }
+};
+
+namespace MATH_TOOLS {
+
+/**
+\brief Approximate inverse square root
+
+A fast approximation to the inverse sqrt
+The relative error is less than  1.5*2^-12
+*/
+inline float approx_rsqrt(float a)
+{
+return _mm_cvtss_f32(_mm_rsqrt_ss(_mm_set_ss(a)));
+}
+
+/**
+\brief Inverse square root
+computed from approx_rsqrt and one newton step
+*/
+inline float rsqrt(float a)
+{
+    return (float)1.0f / std::sqrt(a);
+    
+// float b = approx_rsqrt(a);
+// // Newton step with f(x) = a - 1/x^2
+// b = 0.5f * b * (3.0f - a * (b * b));
+// return b;
+}
+
+/**
+\brief Inverse square root
+computed from 1/std::sqrt
+*/
+inline double rsqrt(double a)
+{
+using std::sqrt;
+return 1 / sqrt(a);
+}
+}
+
+/**
+    Timer. We can use either system timer or stready timer
+*/
+class Timer {
+public:
+    Timer() {}
+
+    ~Timer() {}
+
+    /**
+ 	  \brief Start timing
+	*/
+    void start()
+    {
+        start_time = std::chrono::steady_clock::now();
+    }
+
+    /**
+  	 \return time elapsed since last click in seconds
+*/
+    double click()
+    {
+        to_time = std::chrono::steady_clock::now();
+        elapsed_seconds = to_time - start_time;
+        start_time = to_time;
+        return elapsed_seconds.count();
+    }
+
+private:
+    std::chrono::time_point<std::chrono::steady_clock> start_time;
+    std::chrono::time_point<std::chrono::steady_clock> to_time;
+    std::chrono::duration<double> elapsed_seconds;
+};
+
+namespace INTERNAL {
+using namespace std;
+template <class T, class Enable = void>
+struct ScalarTypeHelper {
+    using type = typename T::Scalar;
+};
+template <class T>
+struct ScalarTypeHelper<T, enable_if_t<is_arithmetic<T>::value> > {
+    using type = T;
+};
+}
+
+template <class T>
+using ScalarType = typename INTERNAL::ScalarTypeHelper<T>::type;
+
+template <class MatrixType>
+constexpr bool isSize(int m, int n)
+{
+    return MatrixType::RowsAtCompileTime == m && MatrixType::ColsAtCompileTime == n;
+}
+
+}
+#endif

extern/tetgen/CMakeLists.txt

@@ -0,0 +1,36 @@
+# ***** BEGIN GPL LICENSE BLOCK *****
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software Foundation,
+# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+#
+# The Original Code is Copyright (C) 2006, Blender Foundation
+# All rights reserved.
+# ***** END GPL LICENSE BLOCK *****
+
+set(INC
+  .
+)
+
+set(INC_SYS
+)
+
+set(SRC
+    tetgen.cxx
+    predicates.cxx
+)
+
+set(LIB
+)
+
+blender_add_lib(extern_tetgen "${SRC}" "${INC}" "${INC_SYS}" "${LIB}")

extern/tetgen/LICENSE

@@ -0,0 +1,666 @@
+TetGen License
+--------------
+
+TetGen is distributed under a dual licensing scheme. You can
+redistribute it and/or modify it under the terms of the GNU Affero
+General Public License as published by the Free Software Foundation,
+either version 3 of the License, or (at your option) any later
+version. A copy of the GNU Affero General Public License is reproduced
+below.
+
+If the terms and conditions of the AGPL v.3. would prevent you from
+using TetGen, please consider the option to obtain a commercial
+license for a fee. These licenses are offered by the Weierstrass
+Institute for Applied Analysis and Stochastics (WIAS). As a rule,
+licenses are provided "as-is", unlimited in time for a one time
+fee. Please send corresponding requests to:
+tetgen@wias-berlin.de. Please do not forget to include some
+description of your company and the realm of its activities.
+
+=====================================================================
+GNU AFFERO GENERAL PUBLIC LICENSE
+
+Version 3, 19 November 2007
+
+Copyright © 2007 Free Software Foundation, Inc. <http://fsf.org/>
+Everyone is permitted to copy and distribute verbatim copies of this
+license document, but changing it is not allowed.
+
+Preamble
+
+The GNU Affero General Public License is a free, copyleft license for
+software and other kinds of works, specifically designed to ensure
+cooperation with the community in the case of network server software.
+
+The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works. By contrast,
+our General Public Licenses are intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains
+free software for all its users.
+
+When we speak of free software, we are referring to freedom, not
+price. Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+Developers that use our General Public Licenses protect your rights
+with two steps: (1) assert copyright on the software, and (2) offer
+you this License which gives you legal permission to copy, distribute
+and/or modify the software.
+
+A secondary benefit of defending all users' freedom is that
+improvements made in alternate versions of the program, if they
+receive widespread use, become available for other developers to
+incorporate. Many developers of free software are heartened and
+encouraged by the resulting cooperation. However, in the case of
+software used on network servers, this result may fail to come
+about. The GNU General Public License permits making a modified
+version and letting the public access it on a server without ever
+releasing its source code to the public.
+
+The GNU Affero General Public License is designed specifically to
+ensure that, in such cases, the modified source code becomes available
+to the community. It requires the operator of a network server to
+provide the source code of the modified version running there to the
+users of that server. Therefore, public use of a modified version, on
+a publicly accessible server, gives the public access to the source
+code of the modified version.
+
+An older license, called the Affero General Public License and
+published by Affero, was designed to accomplish similar goals. This is
+a different license, not a version of the Affero GPL, but Affero has
+released a new version of the Affero GPL which permits relicensing
+under this license.
+
+The precise terms and conditions for copying, distribution and
+modification follow.
+
+TERMS AND CONDITIONS
+
+0. Definitions.
+
+"This License" refers to version 3 of the GNU Affero General Public
+License.
+
+"Copyright" also means copyright-like laws that apply to other kinds
+of works, such as semiconductor masks.
+
+"The Program" refers to any copyrightable work licensed under this
+License. Each licensee is addressed as "you". "Licensees" and
+"recipients" may be individuals or organizations.
+
+To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of
+an exact copy. The resulting work is called a "modified version" of
+the earlier work or a work "based on" the earlier work.
+
+A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy. Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies. Mere interaction with a user
+through a computer network, with no transfer of a copy, is not
+conveying.
+
+An interactive user interface displays "Appropriate Legal Notices" to
+the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License. If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+1. Source Code.
+
+The "source code" for a work means the preferred form of the work for
+making modifications to it. "Object code" means any non-source form of
+a work.
+
+A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form. A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities. However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work. For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+The Corresponding Source need not include anything that users can
+regenerate automatically from other parts of the Corresponding Source.
+
+The Corresponding Source for a work in source code form is that same
+work.
+
+2. Basic Permissions.
+
+All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met. This License explicitly affirms your unlimited
+permission to run the unmodified Program. The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work. This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+You may make, run and propagate covered works that you do not convey,
+without conditions so long as your license otherwise remains in
+force. You may convey covered works to others for the sole purpose of
+having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright. Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+Conveying under any other circumstances is permitted solely under the
+conditions stated below. Sublicensing is not allowed; section 10 makes
+it unnecessary.
+
+3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such
+circumvention is effected by exercising rights under this License with
+respect to the covered work, and you disclaim any intention to limit
+operation or modification of the work as a means of enforcing, against
+the work's users, your or third parties' legal rights to forbid
+circumvention of technological measures.
+
+4. Conveying Verbatim Copies.
+
+You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+5. Conveying Modified Source Versions.
+
+You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these
+conditions:
+
+a) The work must carry prominent notices stating that you modified it,
+and giving a relevant date.  b) The work must carry prominent notices
+stating that it is released under this License and any conditions
+added under section 7. This requirement modifies the requirement in
+section 4 to "keep intact all notices".  c) You must license the
+entire work, as a whole, under this License to anyone who comes into
+possession of a copy. This License will therefore apply, along with
+any applicable section 7 additional terms, to the whole of the work,
+and all its parts, regardless of how they are packaged. This License
+gives no permission to license the work in any other way, but it does
+not invalidate such permission if you have separately received it.  d)
+If the work has interactive user interfaces, each must display
+Appropriate Legal Notices; however, if the Program has interactive
+interfaces that do not display Appropriate Legal Notices, your work
+need not make them do so.  A compilation of a covered work with other
+separate and independent works, which are not by their nature
+extensions of the covered work, and which are not combined with it
+such as to form a larger program, in or on a volume of a storage or
+distribution medium, is called an "aggregate" if the compilation and
+its resulting copyright are not used to limit the access or legal
+rights of the compilation's users beyond what the individual works
+permit. Inclusion of a covered work in an aggregate does not cause
+this License to apply to the other parts of the aggregate.
+
+6. Conveying Non-Source Forms.
+
+You may convey a covered work in object code form under the terms of
+sections 4 and 5, provided that you also convey the machine-readable
+Corresponding Source under the terms of this License, in one of these
+ways:
+
+a) Convey the object code in, or embodied in, a physical product
+(including a physical distribution medium), accompanied by the
+Corresponding Source fixed on a durable physical medium customarily
+used for software interchange.  b) Convey the object code in, or
+embodied in, a physical product (including a physical distribution
+medium), accompanied by a written offer, valid for at least three
+years and valid for as long as you offer spare parts or customer
+support for that product model, to give anyone who possesses the
+object code either (1) a copy of the Corresponding Source for all the
+software in the product that is covered by this License, on a durable
+physical medium customarily used for software interchange, for a price
+no more than your reasonable cost of physically performing this
+conveying of source, or (2) access to copy the Corresponding Source
+from a network server at no charge.  c) Convey individual copies of
+the object code with a copy of the written offer to provide the
+Corresponding Source. This alternative is allowed only occasionally
+and noncommercially, and only if you received the object code with
+such an offer, in accord with subsection 6b.  d) Convey the object
+code by offering access from a designated place (gratis or for a
+charge), and offer equivalent access to the Corresponding Source in
+the same way through the same place at no further charge. You need not
+require recipients to copy the Corresponding Source along with the
+object code. If the place to copy the object code is a network server,
+the Corresponding Source may be on a different server (operated by you
+or a third party) that supports equivalent copying facilities,
+provided you maintain clear directions next to the object code saying
+where to find the Corresponding Source. Regardless of what server
+hosts the Corresponding Source, you remain obligated to ensure that it
+is available for as long as needed to satisfy these requirements.  e)
+Convey the object code using peer-to-peer transmission, provided you
+inform other peers where the object code and Corresponding Source of
+the work are being offered to the general public at no charge under
+subsection 6d.  A separable portion of the object code, whose source
+code is excluded from the Corresponding Source as a System Library,
+need not be included in conveying the object code work.
+
+A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal,
+family, or household purposes, or (2) anything designed or sold for
+incorporation into a dwelling. In determining whether a product is a
+consumer product, doubtful cases shall be resolved in favor of
+coverage. For a particular product received by a particular user,
+"normally used" refers to a typical or common use of that class of
+product, regardless of the status of the particular user or of the way
+in which the particular user actually uses, or expects or is expected
+to use, the product. A product is a consumer product regardless of
+whether the product has substantial commercial, industrial or
+non-consumer uses, unless such uses represent the only significant
+mode of use of the product.
+
+"Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to
+install and execute modified versions of a covered work in that User
+Product from a modified version of its Corresponding Source. The
+information must suffice to ensure that the continued functioning of
+the modified object code is in no case prevented or interfered with
+solely because modification has been made.
+
+If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information. But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or
+updates for a work that has been modified or installed by the
+recipient, or for the User Product in which it has been modified or
+installed. Access to a network may be denied when the modification
+itself materially and adversely affects the operation of the network
+or violates the rules and protocols for communication across the
+network.
+
+Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+7. Additional Terms.
+
+"Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its
+conditions. Additional permissions that are applicable to the entire
+Program shall be treated as though they were included in this License,
+to the extent that they are valid under applicable law. If additional
+permissions apply only to part of the Program, that part may be used
+separately under those permissions, but the entire Program remains
+governed by this License without regard to the additional permissions.
+
+When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it. (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.) You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders
+of that material) supplement the terms of this License with terms:
+
+a) Disclaiming warranty or limiting liability differently from the
+terms of sections 15 and 16 of this License; or b) Requiring
+preservation of specified reasonable legal notices or author
+attributions in that material or in the Appropriate Legal Notices
+displayed by works containing it; or c) Prohibiting misrepresentation
+of the origin of that material, or requiring that modified versions of
+such material be marked in reasonable ways as different from the
+original version; or d) Limiting the use for publicity purposes of
+names of licensors or authors of the material; or e) Declining to
+grant rights under trademark law for use of some trade names,
+trademarks, or service marks; or f) Requiring indemnification of
+licensors and authors of that material by anyone who conveys the
+material (or modified versions of it) with contractual assumptions of
+liability to the recipient, for any liability that these contractual
+assumptions directly impose on those licensors and authors.  All other
+non-permissive additional terms are considered "further restrictions"
+within the meaning of section 10. If the Program as you received it,
+or any part of it, contains a notice stating that it is governed by
+this License along with a term that is a further restriction, you may
+remove that term. If a license document contains a further restriction
+but permits relicensing or conveying under this License, you may add
+to a covered work material governed by the terms of that license
+document, provided that the further restriction does not survive such
+relicensing or conveying.
+
+If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions; the
+above requirements apply either way.
+
+8. Termination.
+
+You may not propagate or modify a covered work except as expressly
+provided under this License. Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+However, if you cease all violation of this License, then your license
+from a particular copyright holder is reinstated (a) provisionally,
+unless and until the copyright holder explicitly and finally
+terminates your license, and (b) permanently, if the copyright holder
+fails to notify you of the violation by some reasonable means prior to
+60 days after the cessation.
+
+Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License. If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+9. Acceptance Not Required for Having Copies.
+
+You are not required to accept this License in order to receive or run
+a copy of the Program. Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance. However,
+nothing other than this License grants you permission to propagate or
+modify any covered work. These actions infringe copyright if you do
+not accept this License. Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+10. Automatic Licensing of Downstream Recipients.
+
+Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License. You are not responsible
+for enforcing compliance by third parties with this License.
+
+An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations. If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License. For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+11. Patents.
+
+A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based. The
+work thus licensed is called the contributor's "contributor version".
+
+A contributor's "essential patent claims" are all patent claims owned
+or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version. For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement). To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients. "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+A patent license is "discriminatory" if it does not include within the
+scope of its coverage, prohibits the exercise of, or is conditioned on
+the non-exercise of one or more of the rights that are specifically
+granted under this License. You may not convey a covered work if you
+are a party to an arrangement with a third party that is in the
+business of distributing software, under which you make payment to the
+third party based on the extent of your activity of conveying the
+work, and under which the third party grants, to any of the parties
+who would receive the covered work from you, a discriminatory patent
+license (a) in connection with copies of the covered work conveyed by
+you (or copies made from those copies), or (b) primarily for and in
+connection with specific products or compilations that contain the
+covered work, unless you entered into that arrangement, or that patent
+license was granted, prior to 28 March 2007.
+
+Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+12. No Surrender of Others' Freedom.
+
+If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under
+this License and any other pertinent obligations, then as a
+consequence you may not convey it at all. For example, if you agree to
+terms that obligate you to collect a royalty for further conveying
+from those to whom you convey the Program, the only way you could
+satisfy both those terms and this License would be to refrain entirely
+from conveying the Program.
+
+13. Remote Network Interaction; Use with the GNU General Public
+License.
+
+Notwithstanding any other provision of this License, if you modify the
+Program, your modified version must prominently offer all users
+interacting with it remotely through a computer network (if your
+version supports such interaction) an opportunity to receive the
+Corresponding Source of your version by providing access to the
+Corresponding Source from a network server at no charge, through some
+standard or customary means of facilitating copying of software. This
+Corresponding Source shall include the Corresponding Source for any
+work covered by version 3 of the GNU General Public License that is
+incorporated pursuant to the following paragraph.
+
+Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU General Public License into a single
+combined work, and to convey the resulting work. The terms of this
+License will continue to apply to the part which is the covered work,
+but the work with which it is combined will remain governed by version
+3 of the GNU General Public License.
+
+14. Revised Versions of this License.
+
+The Free Software Foundation may publish revised and/or new versions
+of the GNU Affero General Public License from time to time. Such new
+versions will be similar in spirit to the present version, but may
+differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number. If the Program
+specifies that a certain numbered version of the GNU Affero General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation. If the Program does not specify a version number of the
+GNU Affero General Public License, you may choose any version ever
+published by the Free Software Foundation.
+
+If the Program specifies that a proxy can decide which future versions
+of the GNU Affero General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+Later license versions may give you additional or different
+permissions. However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+15. Disclaimer of Warranty.
+
+THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT
+WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND
+PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE
+DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR
+CORRECTION.
+
+16. Limitation of Liability.
+
+IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR
+CONVEYS THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
+INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES
+ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT
+NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR
+LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM
+TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER
+PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.
+
+17. Interpretation of Sections 15 and 16.
+
+If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
+
+END OF TERMS AND CONDITIONS
+
+How to Apply These Terms to Your New Programs
+
+If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these
+terms.
+
+To do so, attach the following notices to the program. It is safest to
+attach them to the start of each source file to most effectively state
+the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the program's name and a brief idea of what it
+    does.> Copyright (C) <year> <name of author>
+
+    This program is free software: you can redistribute it and/or
+    modify it under the terms of the GNU Affero General Public License
+    as published by the Free Software Foundation, either version 3 of
+    the License, or (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+    Affero General Public License for more details.
+
+    You should have received a copy of the GNU Affero General Public
+    License along with this program.  If not, see
+    <http://www.gnu.org/licenses/>.  Also add information on how to
+    contact you by electronic and paper mail.
+
+If your software can interact with users remotely through a computer
+network, you should also make sure that it provides a way for users to
+get its source. For example, if your program is a web application, its
+interface could display a "Source" link that leads users to an archive
+of the code. There are many ways you could offer source, and different
+solutions will be better for different programs; see section 13 for
+the specific requirements.
+
+You should also get your employer (if you work as a programmer) or
+school, if any, to sign a "copyright disclaimer" for the program, if
+necessary. For more information on this, and how to apply and follow
+the GNU AGPL, see <http://www.gnu.org/licenses/>.

extern/tetgen/README

@@ -0,0 +1,25 @@
+This is TetGen version 1.5 (released on November 4, 2013)
+
+Please see the documentation of TetGen for compiling and using TetGen.
+It is available at the following link:
+
+            http://www.tetgen.org
+
+For more information on this product, contact :
+
+  Hang Si
+  Research Group of Numerical Mathematics and Scientific Computing
+  Weierstrass Institute for Applied Analysis and Stochastics
+  Mohrenstr. 39
+  10117 Berlin, Germany
+
+ Phone: +49 (0) 30-20372-446   Fax: +49 (0) 30-2044975
+ EMail: <si@wias-berlin.de>
+ Web Site: http://www.wias-berlin.de/~si
+
+------------------- IMPORTANCE NOTICE -----------------------------
+
+BEFORE INTALLING OR USING TetGen(R) READ the 
+GENERAL LICENSE TERMS AND CONDITIONS
+
+-------------------------------------------------------------------

extern/tetgen/makefile

@@ -0,0 +1,62 @@
+###############################################################################
+#                                                                             #
+# makefile for TetGen                                                         #
+#                                                                             #
+# Type "make" to compile TetGen into an executable program (tetgen).          #
+# Type "make tetlib" to compile TetGen into a library (libtet.a).             #
+# Type "make distclean" to delete all object (*.o) files.                     #
+#                                                                             #
+###############################################################################
+
+# CXX should be set to the name of your favorite C++ compiler.
+# ===========================================================
+
+CXX = g++
+#CXX = icpc
+#CXX = CC
+
+# CXXFLAGS is the level of optimiztion, default is -O. One should try
+# -O2, -O3 ... to find the best optimization level.
+# ===================================================================
+
+CXXFLAGS = -O3
+
+# PREDCXXFLAGS is for compiling J. Shewchuk's predicates. 
+
+PREDCXXFLAGS = -O0
+
+# SWITCHES is a list of switches to compile TetGen.
+# =================================================
+#
+# By default, TetGen uses double precision floating point numbers.  If you
+#   prefer single precision, use the -DSINGLE switch. 
+#
+# The source code of TetGen includes a lot of assertions, which are mainly
+#   used for catching bugs at that places.  These assertions somewhat slow
+#   down the speed of TetGen.  They can be skipped by define the -DNDEBUG
+#   switch.
+
+SWITCHES = 
+
+# RM should be set to the name of your favorite rm (file deletion program).
+
+RM = /bin/rm
+
+# The action starts here.
+
+tetgen:	tetgen.cxx predicates.o
+	$(CXX) $(CXXFLAGS) $(SWITCHES) -o tetgen tetgen.cxx predicates.o -lm
+
+tetlib: tetgen.cxx predicates.o
+	$(CXX) $(CXXFLAGS) $(SWITCHES) -DTETLIBRARY -c tetgen.cxx
+	ar r libtet.a tetgen.o predicates.o
+
+predicates.o: predicates.cxx
+	$(CXX) $(PREDCXXFLAGS) -c predicates.cxx
+
+clean:
+	$(RM) *.o *.a tetgen *~
+
+
+
+

intern/CMakeLists.txt

@@ -32,6 +32,8 @@ add_subdirectory(glew-mx)
 add_subdirectory(eigen)
 add_subdirectory(sky)
 
+add_subdirectory(softbody)
+
 if(WITH_AUDASPACE)
   add_subdirectory(audaspace)
 endif()
@@ -77,6 +79,10 @@ if(WITH_QUADRIFLOW)
   add_subdirectory(quadriflow)
 endif()
 
+if(WITH_TETGEN)
+  add_subdirectory(tetgen)
+endif()
+
 if(WITH_CODEC_FFMPEG)
   add_subdirectory(ffmpeg)
 endif()

intern/softbody/CMakeLists.txt

@@ -0,0 +1,53 @@
+# ***** BEGIN GPL LICENSE BLOCK *****
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software Foundation,
+# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+#
+# The Original Code is Copyright (C) 2006, Blender Foundation
+# All rights reserved.
+# ***** END GPL LICENSE BLOCK *****
+
+set(INC
+  .
+)
+
+set(INC_SYS
+  ../../extern/softbody/src
+  ../../extern/discregrid/discregrid/include
+  ../../extern/tetgen
+  ../../source/blender/makesdna
+  ../../source/blender/blenkernel
+  ../../source/blender/blenlib
+  ../../intern/guardedalloc
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+set(SRC
+  admmpd_api.cpp
+  admmpd_api.h
+)
+
+set(LIB
+  extern_admmpd
+)
+
+if(WITH_TETGEN)
+  set(LIB
+    ${LIB}
+    extern_tetgen
+  )
+  add_definitions(-DWITH_TETGEN)
+endif()
+
+blender_add_lib(bf_intern_admmpd "${SRC}" "${INC}" "${INC_SYS}" "${LIB}")

intern/softbody/admmpd_api.cpp

@@ -0,0 +1,949 @@
+/*
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+ *
+ * The Original Code is Copyright (C) 2013 Blender Foundation
+ * All rights reserved.
+ */
+
+/** \file
+ * \ingroup admmpd
+ */
+
+#include "admmpd_api.h"
+#include "admmpd_types.h"
+#include "admmpd_solver.h"
+#include "admmpd_mesh.h"
+#include "admmpd_collision.h"
+#ifdef WITH_TETGEN
+  #include "tetgen.h"
+#endif
+#include "DNA_mesh_types.h" // Mesh
+#include "DNA_meshdata_types.h" // MVert
+#include "DNA_modifier_types.h" // CollisionModifierData
+#include "DNA_object_force_types.h" // Enums
+#include "BKE_mesh.h" // BKE_mesh_free
+#include "BKE_softbody.h" // BodyPoint
+#include "BKE_deform.h" // BKE_defvert_find_index
+#include "BKE_modifier.h" // BKE_modifiers_findby_type
+#include "MEM_guardedalloc.h"
+
+#include <iostream>
+#include <memory>
+#include <algorithm>
+
+struct ADMMPDInternalData
+{
+  // Created in admmpd_update_mesh
+  std::shared_ptr<admmpd::Mesh> mesh;
+  std::shared_ptr<admmpd::Collision> collision;
+  // Created in admmpd_update_solver
+  std::shared_ptr<admmpd::Options> options;
+  std::shared_ptr<admmpd::SolverData> data;
+  // Created in set_obstacles
+  std::vector<Eigen::MatrixXd> obs_x0, obs_x1;
+  std::vector<Eigen::MatrixXi> obs_F;
+};
+
+
+static inline void strcpy_error(ADMMPDInterfaceData *iface, const std::string &str)
+{
+  int len = std::min(256, (int)str.size()+1);
+  memset(iface->last_error, 0, sizeof(iface->last_error));
+  str.copy(iface->last_error, len);
+}
+
+static inline void options_from_object(
+  ADMMPDInterfaceData *iface,
+  Scene *scene,
+  Object *ob,
+  admmpd::Options *op,
+  bool skip_require_reset)
+{
+  (void)(iface);
+
+  SoftBody *sb = ob->soft;
+  if (sb==NULL) {
+    return;
+  }
+
+  // Set options that don't require a re-initialization
+  op->max_admm_iters = std::max(1,sb->admmpd_max_admm_iters);
+  op->min_res = std::max(0.f,sb->admmpd_converge_eps);
+  op->pk = std::pow(10.f, sb->admmpd_pk_exp);
+  op->ck = std::pow(10.f, sb->admmpd_ck_exp);
+  op->floor = sb->admmpd_floor_z;
+  op->self_collision = sb->admmpd_self_collision;
+  op->log_level = std::max(0, std::min(LOGLEVEL_NUM-1, sb->admmpd_loglevel));
+  op->grav = Eigen::Vector3d(0,0,sb->admmpd_gravity);
+  op->max_threads = sb->admmpd_maxthreads;
+  op->linsolver = std::max(0, std::min(LINSOLVER_NUM-1, sb->admmpd_linsolver));
+  op->strain_limit[0] = std::min(1.f, sb->admmpd_strainlimit_min);
+  op->strain_limit[1] = std::max(1.f, sb->admmpd_strainlimit_max);
+  op->lattice_subdiv = std::max(1,sb->admmpd_embed_res);
+
+  if (!skip_require_reset)
+  {
+    if (scene)
+    {
+      float framerate = scene->r.frs_sec / scene->r.frs_sec_base;
+      float fps = std::min(1000.f,std::max(1.f,framerate));
+      op->timestep_s = (1.0/fps) / float(std::max(1,sb->admmpd_substeps));
+    }
+    op->density_kgm3 = std::max(1.f,sb->admmpd_density_kgm3);
+    op->youngs = std::pow(10.f, sb->admmpd_youngs_exp);
+    op->poisson = std::max(0.f,std::min(0.499f,sb->admmpd_poisson));
+    op->elastic_material = std::max(0, std::min(ELASTIC_NUM-1, sb->admmpd_material));
+    op->substeps = std::max(1,sb->admmpd_substeps);
+  }
+}
+
+static inline void vecs_from_object(
+  Object *ob,
+  float (*vertexCos)[3],
+  std::vector<float> &v,
+  std::vector<unsigned int> &f)
+{
+  if(ob->type != OB_MESH) {
+    return;
+  }
+
+  Mesh *me = (Mesh*)ob->data;
+
+  // Initialize input vertices
+  v.resize(me->totvert*3, 0);
+  for (int i=0; i<me->totvert; ++i)
+  {
+    // Local to global coordinates
+    float vi[3];
+    vi[0] = vertexCos[i][0];
+    vi[1] = vertexCos[i][1];
+    vi[2] = vertexCos[i][2];
+    mul_m4_v3(ob->obmat, vi);
+    for (int j=0; j<3; ++j) {
+      v[i*3+j] = vi[j];
+    }
+  } // end loop input surface verts
+
+  // Initialize input faces
+  int totfaces = poly_to_tri_count(me->totpoly, me->totloop);
+  f.resize(totfaces*3, 0);
+  MLoopTri *looptri, *lt;
+  looptri = lt = (MLoopTri *)MEM_callocN(sizeof(*looptri)*totfaces, __func__);
+  BKE_mesh_recalc_looptri(me->mloop, me->mpoly, me->mvert, me->totloop, me->totpoly, looptri);
+  for (int i=0; i<totfaces; ++i, ++lt)
+  {
+      f[i*3+0] = me->mloop[lt->tri[0]].v;
+      f[i*3+1] = me->mloop[lt->tri[1]].v;
+      f[i*3+2] = me->mloop[lt->tri[2]].v;
+  }
+  MEM_freeN(looptri);
+  looptri = NULL;
+}
+
+void admmpd_dealloc(ADMMPDInterfaceData *iface)
+{
+  if (!iface) { return; }
+  memset(iface->last_error, 0, sizeof(iface->last_error));
+
+  if (!iface->idata) { return; }
+  iface->idata->options.reset();
+  iface->idata->data.reset();
+  iface->idata->collision.reset();
+  iface->idata->mesh.reset();
+  MEM_freeN((void*)iface->idata);
+  iface->idata = nullptr;
+}
+
+static inline int admmpd_init_with_tetgen(ADMMPDInterfaceData *iface, Object *ob, float (*vertexCos)[3]);
+
+static inline int admmpd_init_with_lattice(ADMMPDInterfaceData *iface, Object *ob, float (*vertexCos)[3])
+{
+  std::vector<float> v;
+  std::vector<unsigned int> f;
+  vecs_from_object(ob,vertexCos,v,f);
+  iface->idata->mesh = std::make_shared<admmpd::EmbeddedMesh>();
+  bool success = iface->idata->mesh->create(
+    iface->idata->options.get(),
+    v.data(),
+    v.size()/3,
+    f.data(),
+    f.size()/3,
+    nullptr,
+    0);
+
+  if (!success) { // soft unknown fail
+    strcpy_error(iface, "EmbeddedMesh failed on creation");
+    return 0;
+  }
+
+  iface->idata->collision = std::make_shared<admmpd::EmbeddedMeshCollision>();
+  return 1;
+}
+
+static inline int admmpd_init_as_cloth(ADMMPDInterfaceData *iface, Object *ob, float (*vertexCos)[3])
+{
+  std::vector<float> v;
+  std::vector<unsigned int> f;
+  vecs_from_object(ob,vertexCos,v,f);
+
+  iface->idata->mesh = std::make_shared<admmpd::TriangleMesh>();
+  bool success = iface->idata->mesh->create(
+    iface->idata->options.get(),
+    v.data(),
+    v.size()/3,
+    f.data(),
+    f.size()/3,
+    nullptr,
+    0);
+
+  if (!success) {
+    strcpy_error(iface, "TriangleMesh failed on creation");
+    return 0;
+  }
+
+  iface->idata->collision = nullptr; // TODO, triangle mesh collision
+  return 1;
+}
+
+void admmpd_compute_lattice(
+    int subdiv,
+    float *in_verts, int in_nv,
+    unsigned int *in_faces, int in_nf,
+    float **out_verts, int *out_nv,
+    unsigned int **out_tets, int *out_nt)
+{
+
+  admmpd::EmbeddedMesh emesh;
+  admmpd::Options opt;
+  opt.lattice_subdiv = subdiv;
+  bool success = emesh.create(
+    &opt,
+    in_verts, in_nv,
+    in_faces, in_nf,
+    nullptr,
+    0);
+
+  if (!success) {
+    return;
+  }
+
+  const Eigen::MatrixXd &vt = *emesh.rest_prim_verts();
+  const Eigen::MatrixXi &t = *emesh.prims();
+  if (vt.rows()==0 || t.rows()==0) {
+    return;
+  }
+
+  *out_nv = vt.rows();
+  *out_verts = (float*)MEM_callocN(sizeof(float)*3*(vt.rows()), "ADMMPD_lattice_verts");
+  *out_nt = t.rows();
+  *out_tets = (unsigned int*)MEM_callocN(sizeof(unsigned int)*4*(t.rows()), "ADMMPD_lattice_tets");
+
+  for (int i=0; i<vt.rows(); ++i) {
+    (*out_verts)[i*3+0] = vt(i,0);
+    (*out_verts)[i*3+1] = vt(i,1);
+    (*out_verts)[i*3+2] = vt(i,2);
+  }
+
+  for (int i=0; i<t.rows(); ++i) {
+    (*out_tets)[i*4+0] = t(i,0);
+    (*out_tets)[i*4+1] = t(i,1);
+    (*out_tets)[i*4+2] = t(i,2);
+    (*out_tets)[i*4+3] = t(i,3);
+  }
+}
+
+int admmpd_mesh_needs_update(ADMMPDInterfaceData *iface, Object *ob)
+{
+  if (!iface) { return 0; }
+  if (!ob) { return 0; }
+  if (!ob->soft) { return 0; }
+  if (!ob->data) { return 0; }
+  if (!iface->idata) { return 1; }
+  if (!iface->idata->mesh) { return 1; }
+  Mesh *mesh = (Mesh*)ob->data;
+  if (!mesh) { return 0; }
+
+  // Mode or topology change?
+  int mode = ob->soft->admmpd_mesh_mode;
+  int mesh_type = iface->idata->mesh->type();
+
+  if (mode != mesh_type) { return 1; }
+  if (!iface->idata->mesh->rest_facet_verts()) { return 1; }
+  int nx = iface->idata->mesh->rest_facet_verts()->rows();
+  if (nx != mesh->totvert) { return 1; }
+
+  return 0;
+}
+
+int admmpd_update_mesh(ADMMPDInterfaceData *iface, Object *ob, float (*vertexCos)[3])
+{
+  if (!iface) { return 0; }
+  if (!ob) { return 0; }
+  if (!ob->soft) { return 0; }
+
+  if (!iface->idata) {
+    iface->idata = (ADMMPDInternalData*)MEM_callocN(sizeof(ADMMPDInternalData), "ADMMPD_idata");
+  }
+
+  if (!iface->idata->options) {
+    iface->idata->options = std::make_shared<admmpd::Options>();
+  }
+
+  options_from_object(iface,NULL,ob,iface->idata->options.get(),false);
+  int mode = ob->soft->admmpd_mesh_mode;
+  iface->idata->mesh.reset();
+
+  // Try to initialize the mesh
+  const Eigen::MatrixXd *x0 = nullptr;
+  try {
+    int gen_success = 0;
+    switch (mode)
+    {
+      default:
+      case MESHTYPE_EMBEDDED: {
+        gen_success = admmpd_init_with_lattice(iface,ob,vertexCos);
+        if (gen_success) {
+          x0 = iface->idata->mesh->rest_prim_verts();
+        }
+      } break;
+      case MESHTYPE_TET: {
+        gen_success = admmpd_init_with_tetgen(iface,ob,vertexCos);
+        if (gen_success) {
+          x0 = iface->idata->mesh->rest_prim_verts();
+        }
+      } break;
+      case MESHTYPE_TRIANGLE: {
+        gen_success = admmpd_init_as_cloth(iface,ob,vertexCos);
+        if (gen_success) {
+          x0 = iface->idata->mesh->rest_facet_verts();
+        }
+      } break;
+    }
+    if (!gen_success || !iface->idata->mesh || x0==nullptr) {
+      return 0;
+    }
+  }
+  catch(const std::exception &e) {
+    strcpy_error(iface, e.what());
+    return 0;
+  }
+
+  // Set up softbody to store defo verts
+  int n_defo_verts = x0->rows();
+  SoftBody *sb = ob->soft;
+  if (sb->bpoint) {
+    MEM_freeN(sb->bpoint);
+  }
+
+  sb->totpoint = n_defo_verts;
+  sb->totspring = 0;
+  sb->bpoint = (BodyPoint*)MEM_callocN(n_defo_verts*sizeof(BodyPoint), "ADMMPD_bpoint");
+
+  // Copy init data to BodyPoint
+  BodyPoint *pts = sb->bpoint;
+  for (int i=0; i<n_defo_verts; ++i) {
+    BodyPoint *pt = &pts[i];
+    for(int j=0; j<3; ++j) {
+      pt->pos[j] = x0->operator()(i,j);
+      pt->vec[j] = 0;
+    }
+  }
+
+  return 1;
+}
+
+int admmpd_solver_needs_update(ADMMPDInterfaceData *iface, Scene *sc, Object *ob)
+{
+  (void)(sc);
+  if (!iface) { return 0; }
+  if (!ob) { return 0; }
+  if (!ob->soft) { return 0; }
+  if (!iface->idata) { return 1; }
+  if (!iface->idata->options) { return 1; }
+  if (!iface->idata->data) { return 1; }
+
+  auto big_diff = [](const double &a, const double &b) {
+    if (std::abs(a-b) > 1e-4) { return true; }
+    return false;
+  };
+
+  admmpd::Options *opt = iface->idata->options.get();
+  SoftBody *sb = ob->soft;
+  if (sb->admmpd_material != opt->elastic_material) { return 1; }
+  if (sb->admmpd_substeps != opt->substeps) { return 1; }
+  double youngs = std::pow(10.f, std::max(0.f,sb->admmpd_youngs_exp));
+  if (big_diff(youngs, opt->youngs)) { return 1; }
+  if (big_diff(sb->admmpd_density_kgm3, opt->density_kgm3)) { return 1; }
+  if (big_diff(sb->admmpd_poisson, opt->poisson)) { return 1; }
+
+  return 0;
+}
+
+int admmpd_update_solver(ADMMPDInterfaceData *iface,  Scene *sc, Object *ob, float (*vertexCos)[3])
+{
+  if (!iface) { return 0; }
+  if (!ob) { return 0; }
+  if (!ob->soft) { return 0; }
+  (void)(vertexCos);
+
+  // idata is created in admmpd_update_mesh
+  if (!iface->idata) { return 0; }
+  if (!iface->idata->mesh) { return 0; }
+
+  // Reset options and data
+  if (!iface->idata->options) {
+    iface->idata->options = std::make_shared<admmpd::Options>();
+  }
+  iface->idata->data = std::make_shared<admmpd::SolverData>();
+  iface->idata->obs_x0.clear();
+  iface->idata->obs_x1.clear();
+  iface->idata->obs_F.clear();
+
+  admmpd::Options *op = iface->idata->options.get();
+  options_from_object(iface,sc,ob,op,false);
+
+  try {
+    admmpd::Solver().init(
+      iface->idata->mesh.get(),
+      iface->idata->options.get(),
+      iface->idata->data.get());
+  }
+  catch(const std::exception &e) {
+    strcpy_error(iface, e.what());
+    return 0;
+  }
+
+  return 1;
+}
+
+void admmpd_copy_from_object(ADMMPDInterfaceData *iface, Object *ob)
+{
+  if (!iface) { return; }
+  if (!iface->idata) { return; }
+  if (!iface->idata->data) { return; }
+  if (!ob) { return; }
+  if (!ob->soft) { return; }
+  if (!ob->soft->bpoint) { return; }
+
+  // Should be the same, but if not we'll just
+  // copy over up to the amount that we can.
+  int nx = iface->idata->data->x.rows();
+  int nv = std::min(ob->soft->totpoint, nx);
+  for (int i=0; i<nv; ++i) {
+    const BodyPoint *pt = &ob->soft->bpoint[i];
+    for(int j=0; j<3; ++j) {
+      iface->idata->data->x(i,j)=pt->pos[j];
+      iface->idata->data->v(i,j)=pt->vec[j];
+    }
+  }
+}
+
+void admmpd_copy_to_object(ADMMPDInterfaceData *iface, Object *ob, float (*vertexCos)[3])
+{
+  if (!iface) { return; }
+  if (!iface->idata) { return; }
+  if (!iface->idata->data) { return; }
+  if (!iface->idata->mesh) { return; }
+  if (!iface->idata->mesh->rest_facet_verts()) { return; }
+
+  int nx = iface->idata->data->x.rows();
+
+  if (ob && ob->soft) {
+    SoftBody *sb = ob->soft;
+
+    if (!sb->bpoint) {
+      if (!ob->soft->bpoint) {
+        sb->bpoint = (BodyPoint*)MEM_callocN(nx*sizeof(BodyPoint), "ADMMPD_bpoint");
+      }
+      sb->totpoint = nx;
+      sb->totspring = 0;
+    }
+
+    if (sb->totpoint != nx && sb->totpoint>0) {
+      MEM_freeN(sb->bpoint);
+      sb->bpoint = (BodyPoint*)MEM_callocN(nx*sizeof(BodyPoint), "ADMMPD_bpoint");
+      sb->totpoint = nx;
+      sb->totspring = 0;
+    }
+
+    // Copy internal data to BodyPoint
+    int np = std::min(ob->soft->totpoint, nx);
+    for (int i=0; i<np; ++i) {
+      BodyPoint *pt = &ob->soft->bpoint[i];
+      for(int j=0; j<3; ++j) {
+        pt->pos[j] = iface->idata->data->x(i,j);
+        pt->vec[j] = iface->idata->data->v(i,j);
+      }
+    }
+  } // end has ob ptr
+
+  // Copy to vertexCos
+  int nfv = iface->idata->mesh->rest_facet_verts()->rows();
+  if (vertexCos != NULL) {
+    for (int i=0; i<nfv; ++i) {
+      Eigen::Vector3d xi =
+        iface->idata->mesh->get_mapped_facet_vertex(
+        &iface->idata->data->x, i);
+      vertexCos[i][0] = xi[0];
+      vertexCos[i][1] = xi[1];
+      vertexCos[i][2] = xi[2];
+      if (ob && ob->soft && ob->soft->local==0) {
+        mul_m4_v3(ob->imat, vertexCos[i]);
+      }
+    }
+  }
+}
+
+static inline void admmpd_update_goals(ADMMPDInterfaceData *iface, Object *ob, float (*vertexCos)[3])
+{
+  if (!iface) { return; }
+  if (!iface->idata) { return; }
+  if (!iface->idata->mesh) { return; }
+  if (!ob) { return; }
+  if (!ob->soft) { return; }
+
+  SoftBody *sb = ob->soft;
+  Mesh *me = (Mesh*)ob->data;
+  if (!me) { return; }
+
+  // Goal positions turned off
+  if (!(ob->softflag & OB_SB_GOAL)) {
+    iface->idata->mesh->clear_pins();
+    return;
+  }
+
+  int defgroup_index = me->dvert ? (sb->vertgroup - 1) : -1;
+  int nv = me->totvert;
+
+  for (int i=0; i<nv; i++) {
+    double k = 0.1;
+    if ((ob->softflag & OB_SB_GOAL) && (defgroup_index != -1)) {
+      MDeformWeight *dw = BKE_defvert_find_index(&(me->dvert[i]), defgroup_index);
+      k = dw ? dw->weight : 0.0f;
+    }
+
+    Eigen::Vector3d goal_pos(0,0,0);
+    float vi[3];
+    vi[0] = vertexCos[i][0];
+    vi[1] = vertexCos[i][1];
+    vi[2] = vertexCos[i][2];
+    mul_m4_v3(ob->obmat, vi);
+    for (int j=0; j<3; ++j) {
+      goal_pos[j] = vi[j];
+    }
+
+    // We want to call set_pin for every vertex, even
+    // if stiffness is zero. This allows us to animate pins on/off
+    // without calling Mesh::clear_pins().
+    iface->idata->mesh->set_pin(i,goal_pos,k);
+
+  } // end loop verts
+} // end update goals
+
+static inline void update_selfcollision_group(ADMMPDInterfaceData *iface, Object *ob)
+{
+  if (!iface) { return; }
+  if (!iface->idata) { return; }
+  if (!iface->idata->options) { return; }
+  if (!iface->idata->data) { return; }
+  if (!iface->idata->options->self_collision) { return; }
+
+  if (!ob) { return; }
+  if (!ob->soft) { return; }
+  Mesh *me = (Mesh*)ob->data;
+  if (!me) { return; }
+  SoftBody *sb = ob->soft;
+
+  int defgroup_idx_selfcollide = me->dvert ?
+    BKE_object_defgroup_name_index(ob, sb->admmpd_namedVG_selfcollision) : -1;
+
+  // If we do not have a self collision vertex group, we want to
+  // do self collision on all vertices. If the selfcollision_verts set
+  // is empty, the collider will test all verts.
+  iface->idata->data->col.selfcollision_verts.clear();
+  if (defgroup_idx_selfcollide == -1) {
+    return;
+  }
+
+  // Otherwise, we need to set which vertices are to be tested.
+  int nv = me->totvert;
+  for (int i=0; i<nv; i++) {
+    MDeformWeight *dw = BKE_defvert_find_index(&(me->dvert[i]), defgroup_idx_selfcollide);
+    float wi = dw ? dw->weight : 0.0f;
+    if (wi > 1e-2f) { // I guess we can use the weight as a threshold...
+      iface->idata->data->col.selfcollision_verts.emplace(i);
+    }
+  }
+}
+
+int admmpd_solve(ADMMPDInterfaceData *iface, Object *ob, float (*vertexCos)[3])
+{
+  if (!iface || !ob || !ob->soft) {
+    strcpy_error(iface, "NULL input");
+    return 0;
+  }
+
+  if (!iface->idata || !iface->idata->options ||
+      !iface->idata->data || !iface->idata->mesh) {
+    strcpy_error(iface, "NULL internal data");
+    return 0;
+  }
+
+  std::string meshname(ob->id.name);
+
+  // Set to true if certain conditions should
+  // throw a warning flag.
+  bool return_warning = false;
+
+  // Change only options that do not cause a reset of the solver.
+  bool skip_solver_reset = true;
+  options_from_object(
+    iface,
+    nullptr, // scene, ignored if null
+    ob,
+    iface->idata->options.get(),
+    skip_solver_reset);
+
+  // Disable self collision flag if the mesh does not support it.
+  if (iface->idata->options->self_collision &&
+    !iface->idata->mesh->self_collision_allowed()) {
+    // Special message if embedded, in which the mesh is not closed.
+    std::string err = "Cannot do self collisions on object "+meshname+" for selected mesh type";
+    if (iface->idata->mesh->type() == MESHTYPE_EMBEDDED) {
+      err = "Cannot do self collisions on object "+meshname+", mesh is not closed.";
+    }
+    strcpy_error(iface, err.c_str());
+    iface->idata->options->self_collision = false;
+    return_warning = true;
+  }
+
+  // Goals and self collision group can change
+  // between time steps. If the goal indices/weights change,
+  // it will trigger a refactorization in the solver.
+  admmpd_update_goals(iface,ob,vertexCos);
+  update_selfcollision_group(iface,ob);
+
+  // Obstacle collisions not yet implemented
+  // for cloth or tet mesh.
+  if ((ob->soft->admmpd_mesh_mode == MESHTYPE_TET ||
+    ob->soft->admmpd_mesh_mode == MESHTYPE_TRIANGLE) &&
+    iface->idata->obs_x0.size()>0)
+  {
+    return_warning = true;
+    strcpy_error(iface, "Obstacle collision not yet available for selected mesh mode.");
+  }
+
+  // Changing the location of the obstacles requires a recompuation
+  // of the SDF. So we'll only do that if:
+  // a) we are substepping (need to lerp)
+  // b) the obstacle positions have changed from the last frame
+  bool has_obstacles = 
+    iface->idata->collision &&
+    iface->idata->obs_x0.size() > 0 &&
+    iface->idata->obs_x1.size() > 0 &&
+    iface->idata->obs_x0[0].size()==iface->idata->obs_x1[0].size();
+
+  int substeps = std::max(1,iface->idata->options->substeps);
+  int n_obs = iface->idata->obs_x0.size();
+  if (has_obstacles && substeps == 1) { // no lerp necessary
+    std::string set_obs_error = "";
+    if (!iface->idata->collision->set_obstacles(
+        iface->idata->obs_x0, iface->idata->obs_x1, iface->idata->obs_F,
+        &set_obs_error)) {
+      strcpy_error(iface, set_obs_error.c_str());
+      return_warning = true;
+    }
+  }
+
+  try
+  {
+    std::vector<Eigen::MatrixXd> obs_x1_t;
+    for (int i=0; i<substeps; ++i) {
+
+      // Interpolate obstacles
+      if (has_obstacles && substeps>1) {
+        float t = float(i)/float(substeps-1);
+        obs_x1_t.resize(n_obs);
+        for (int j=0; j<n_obs; ++j) {
+          obs_x1_t[j] = (1.f-t)*iface->idata->obs_x0[j] + t*iface->idata->obs_x1[j];
+        }
+        std::string set_obs_error = "";
+        if (!iface->idata->collision->set_obstacles(
+            iface->idata->obs_x0, iface->idata->obs_x1, iface->idata->obs_F,
+            &set_obs_error)) {
+          strcpy_error(iface, set_obs_error.c_str());
+          return_warning = true;
+        }
+      }
+
+      admmpd::Solver().solve(
+        iface->idata->mesh.get(),
+        iface->idata->options.get(),
+        iface->idata->data.get(),
+        iface->idata->collision.get());
+    }
+  }
+  catch(const std::exception &e) {
+    // This is a more important error than set obstacle error,
+    // so if we had an exception we'll report that instead.
+    iface->idata->data->x = iface->idata->data->x_start;
+    strcpy_error(iface, e.what());
+    return 0;
+  }
+
+  if (return_warning) {
+    // We've already copied the error message.
+    return -1;
+  }
+
+  return 1;
+}
+
+void admmpd_update_obstacles(ADMMPDInterfaceData *iface, Object **obstacles, int numobjects)
+{
+  // Because substepping may occur, we'll buffer the start and end states
+  // of the obstacles. They will not be copied over to the collision pointer
+  // until solve(), depending on the number of substeps, in which case
+  // they are LERP'd
+  iface->idata->obs_x0.clear();
+  iface->idata->obs_x1.clear();
+  iface->idata->obs_F.clear();
+  if (!iface) { return; }
+  if (!iface->idata) { return; }
+  if (!obstacles || numobjects==0) { return; }
+
+  for (int i = 0; i < numobjects; ++i) {
+    Object *ob = obstacles[i];
+    if (!ob) {
+      continue; // uh?
+    }
+    if (ob->type != OB_MESH) {
+      continue; // is not a mesh type
+    }
+    if (!ob->pd || !ob->pd->deflect) {
+      continue; // is a non-collider
+    }
+    if (strcmp(ob->id.name,iface->name)==0) {
+      continue; // skip self
+    }
+
+    CollisionModifierData *cmd = (CollisionModifierData *)BKE_modifiers_findby_type(
+        ob, eModifierType_Collision);
+    if (!cmd) {
+      continue;
+    }
+
+    int idx = iface->idata->obs_x0.size();
+    iface->idata->obs_x0.emplace_back(Eigen::MatrixXd(cmd->mvert_num,3));
+    iface->idata->obs_x1.emplace_back(Eigen::MatrixXd(cmd->mvert_num,3));
+    iface->idata->obs_F.emplace_back(Eigen::MatrixXi(cmd->tri_num,3));
+
+    for (int j=0; j<cmd->mvert_num; ++j) {
+      for (int k=0; k<3; ++k) {
+        iface->idata->obs_x0[idx](j,k) = cmd->x[j].co[k];
+        iface->idata->obs_x1[idx](j,k) = cmd->xnew[j].co[k];
+      }
+    }
+
+    for (int j=0; j<cmd->tri_num; ++j) {
+      for (int k=0; k<3; ++k) {
+        iface->idata->obs_F[idx](j,k) = cmd->tri[j].tri[k];
+      }
+    }
+  }
+}
+
+#ifdef WITH_TETGEN
+
+static void make_tetgenio(
+  float *verts,
+  unsigned int *faces,
+  int numverts,
+  int numfaces,
+  tetgenio &tgio )
+{
+  tgio.initialize();
+  tgio.firstnumber = 0;
+  tgio.mesh_dim = 3;
+  tgio.numberofpoints = numverts;
+  tgio.pointlist = new REAL[tgio.numberofpoints * 3];
+//	tgio.pointlist = (REAL *)MEM_malloc_arrayN(
+//		tgio.numberofpoints, 3 * sizeof(REAL), "tetgen remesh out verts");
+  for (int i=0; i < tgio.numberofpoints; ++i)
+    {
+    tgio.pointlist[i*3+0] = verts[i*3+0];
+    tgio.pointlist[i*3+1] = verts[i*3+1];
+    tgio.pointlist[i*3+2] = verts[i*3+2];
+  }
+  tgio.numberoffacets = numfaces;
+  tgio.facetlist = new tetgenio::facet[tgio.numberoffacets];
+//	tgio.facetlist = (tetgenio::facet *)MEM_malloc_arrayN(
+//		tgio.numberoffacets, sizeof(tetgenio::facet), "tetgen remesh out facets");  
+  tgio.facetmarkerlist = new int[tgio.numberoffacets];
+//	tgio.facetmarkerlist = (int *)MEM_malloc_arrayN(
+//		tgio.numberoffacets, sizeof(int), "tetgen remesh out marker list");
+  for (int i=0; i<numfaces; ++i)
+    {
+    tgio.facetmarkerlist[i] = i;
+    tetgenio::facet *f = &tgio.facetlist[i];
+    f->numberofholes = 0;
+    f->holelist = NULL;
+    f->numberofpolygons = 1;
+    f->polygonlist = new tetgenio::polygon[1];
+    tetgenio::polygon *p = &f->polygonlist[0];
+    p->numberofvertices = 3;
+    p->vertexlist = new int[3];
+    p->vertexlist[0] = faces[i*3+0];
+    p->vertexlist[1] = faces[i*3+1];
+    p->vertexlist[2] = faces[i*3+2];
+  }
+}
+
+static inline int admmpd_init_with_tetgen(ADMMPDInterfaceData *iface, Object *ob, float (*vertexCos)[3])
+{
+  if (!iface) { return 0; }
+  if (!iface->idata) { return 0; }
+  if (!ob) { return 0; }
+
+  iface->idata->mesh = std::make_shared<admmpd::TetMesh>();
+  iface->idata->collision.reset(); // TODO
+
+  std::vector<float> v;
+  std::vector<unsigned int> f;
+  vecs_from_object(ob,vertexCos,v,f);
+
+	// Set up the switches
+  //double quality = 1.4; // changes topology
+	std::stringstream switches;
+	switches << "Q"; // quiet
+  //if (quality > 0) { switches << "q" << quality; }
+  //if (maxvol > 0) { switches << "a" << maxvol; }
+
+  tetgenio in;
+  make_tetgenio(v.data(), f.data(), v.size()/3, f.size()/3, in);
+  tetgenio out;
+  out.initialize();
+  char *c_switches = (char *)switches.str().c_str();
+  tetrahedralize(c_switches, &in, &out);
+
+	if( out.numberoftetrahedra == 0 || out.numberofpoints == 0 )
+  {
+    strcpy_error(iface, "TetGen failed to generate");
+    return 0;
+  }
+
+  // We'll create our custom list of facets to render
+  // with blender. These are all of the triangles that
+  // make up the inner and outer faces, without duplicates.
+  // To avoid duplicates, we'll hash them as a string.
+  // While not super efficient, neither is tetrahedralization...
+  struct face {
+      int f0, f1, f2;
+      face(int f0_, int f1_, int f2_) : f0(f0_), f1(f1_), f2(f2_) {}
+  };
+  auto face_hash = [](int f0, int f1, int f2){
+      return std::to_string(f0)+" "+std::to_string(f1)+" "+std::to_string(f2);
+  };
+  std::unordered_map<std::string,face> faces_map;
+
+  int nt = out.numberoftetrahedra;
+  std::vector<unsigned int> tets(nt*4);
+  for (int i=0; i<nt; ++i)
+  {
+    tets[i*4+0] = out.tetrahedronlist[i*4+0];
+    tets[i*4+1] = out.tetrahedronlist[i*4+1];
+    tets[i*4+2] = out.tetrahedronlist[i*4+2];
+    tets[i*4+3] = out.tetrahedronlist[i*4+3];
+
+    // Append faces
+    for(int j=0; j<4; ++j)
+    {
+      int f0 = tets[i*4+j];
+      int f1 = tets[i*4+(j+1)%4];
+      int f2 = tets[i*4+(j+2)%4];
+      std::string hash = face_hash(f0,f1,f2);
+      if (faces_map.count(hash)!=0) {
+        continue;
+      }
+      faces_map.emplace(hash, face(f0,f1,f2));
+    }
+  }
+
+  int nf = faces_map.size();
+  std::vector<unsigned int> faces(nf*3);
+  int f_idx = 0;
+  for (std::unordered_map<std::string,face>::iterator it = faces_map.begin();
+    it != faces_map.end(); ++it, ++f_idx)
+  {
+    faces[f_idx*3+0] = it->second.f0;
+    faces[f_idx*3+1] = it->second.f1;
+    faces[f_idx*3+2] = it->second.f2;    
+  }
+
+  int nv = out.numberofpoints;
+  std::vector<float> verts(nv*3);
+  for (int i=0; i<out.numberofpoints; ++i)
+  {
+    verts[i*3+0] = out.pointlist[i*3+0];
+    verts[i*3+1] = out.pointlist[i*3+1];
+    verts[i*3+2] = out.pointlist[i*3+2];
+  }
+
+  // In the future we can compute a mapping if the tetrahedralization
+  // changes the surface vertices. In fact, we'll want to if we want to use
+  // other tetrahedralization code. For now report an error if the
+  // surface changes.
+  for (int i=0; i<nv; ++i)
+  {
+    for (int j=0; j<3; ++j)
+    {
+      float diff = std::abs(v[i*3+j]-verts[i*3+j]);
+      if (diff > 1e-10)
+      {
+        strcpy_error(iface, "TetGen error: change in surface vertices");
+        return 0;
+      }
+    }
+  }
+
+  iface->idata->mesh = std::make_shared<admmpd::TetMesh>();
+  bool success = iface->idata->mesh->create(
+    iface->idata->options.get(),
+    verts.data(),
+    nv,
+    faces.data(),
+    nf,
+    tets.data(),
+    nt);
+
+  if (!success) {
+    strcpy_error(iface, "Error on mesh creation");
+    return 0;
+  }
+
+  return 1;
+}
+
+#else
+
+static inline int admmpd_init_with_tetgen(ADMMPDInterfaceData *iface, Object *ob, float (*vertexCos)[3])
+{
+  if (!iface) { return 0; }
+  (void)(iface); (void)(ob); (void)(vertexCos);
+  strcpy_error(iface, "TetGen not enabled");
+  return 0;
+}
+
+#endif // WITH_TETGEN

intern/softbody/admmpd_api.h

@@ -0,0 +1,107 @@
+/*
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+ *
+ * The Original Code is Copyright (C) 2013 Blender Foundation,
+ * All rights reserved.
+ */
+
+/** \file
+ * \ingroup admmpd
+ */
+
+#ifndef ADMMPD_API_H
+#define ADMMPD_API_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "DNA_object_types.h"
+#include "DNA_scene_types.h"
+
+typedef struct ADMMPDInterfaceData {
+    /* So that ADMMPD data can be stored in a linked list. */
+    struct ADMMPDInterfaceData *next, *prev;
+    /* The name of the object that uses this data. */
+    char name[MAX_ID_NAME];
+    /* If API returns 0, error stored here. */
+    char last_error[256];
+    /* internal data is NULL until update_mesh or update_solver. */
+    struct ADMMPDInternalData *idata;
+} ADMMPDInterfaceData;
+
+/* Frees ADMMPDInternalData */
+void admmpd_dealloc(ADMMPDInterfaceData*);
+
+/* Standalone function to compute embedding lattice
+* but without the embedding info (for visual debugging) */
+void admmpd_compute_lattice(
+    int subdiv,
+    float *in_verts, int in_nv,
+    unsigned int *in_faces, int in_nf,
+    float **out_verts, int *out_nv,
+    unsigned int **out_tets, int *out_nt);
+
+/* Test if the mesh topology has changed in a way that requires re-initialization.
+* Returns 0 (no update needed) or 1 (needs update) */
+int admmpd_mesh_needs_update(ADMMPDInterfaceData*, Object*);
+
+/* Initialize the mesh.
+* The SoftBody object's (ob->soft) bpoint array is also updated.
+* Returns 1 on success, 0 on failure, -1 on warning */
+int admmpd_update_mesh(ADMMPDInterfaceData*, Object*, float (*vertexCos)[3]);
+
+/* Test if certain parameter changes require re-initialization.
+* Returns 0 (no update needed) or 1 (needs update) */
+int admmpd_solver_needs_update(ADMMPDInterfaceData*, Scene*, Object*);
+
+/* Initialize solver variables.
+* Returns 1 on success, 0 on failure, -1 on warning */
+int admmpd_update_solver(ADMMPDInterfaceData*, Scene*, Object*, float (*vertexCos)[3]);
+
+/* Copies BodyPoint data (from SoftBody)
+* to internal vertex position and velocity */
+void admmpd_copy_from_object(ADMMPDInterfaceData*, Object*);
+
+/* Copies ADMM-PD data to SoftBody::bpoint and vertexCos.
+* If vertexCos is NULL, it is ignored. */
+void admmpd_copy_to_object(ADMMPDInterfaceData*, Object*, float (*vertexCos)[3]);
+
+/* Sets the obstacle data for collisions. */
+void admmpd_update_obstacles(
+    ADMMPDInterfaceData*,
+    Object**,
+    int numobjects);
+
+/* Sets the obstacle data for collisions.
+* Update obstacles has a different interface because of the
+* complexity of grabbing obstacle mesh data. We'll leave that in softbody.c */
+//void admmpd_update_obstacles(
+//    ADMMPDInterfaceData*,
+//    float *in_verts_0,
+//    float *in_verts_1,
+//    int nv,
+//    unsigned int *in_faces,
+//    int nf);
+
+/* Performs a time step. Object and vertexCos are not changed.
+* Returns 1 on success, 0 on failure, -1 on warning */
+int admmpd_solve(ADMMPDInterfaceData*, Object*, float (*vertexCos)[3]);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // ADMMPD_API_H

intern/tetgen/CMakeLists.txt

@@ -0,0 +1,39 @@
+# ***** BEGIN GPL LICENSE BLOCK *****
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software Foundation,
+# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+#
+# The Original Code is Copyright (C) 2019, Blender Foundation
+# All rights reserved.
+# ***** END GPL LICENSE BLOCK *****
+
+set(INC
+  .
+  ../guardedalloc
+)
+
+set(INC_SYS
+  ../../extern/tetgen
+)
+
+set(SRC
+  tetgen_api.cpp
+  tetgen_api.h
+)
+
+set(LIB
+  extern_tetgen
+)
+
+blender_add_lib(bf_intern_tetgen "${SRC}" "${INC}" "${INC_SYS}" "${LIB}")

intern/tetgen/tetgen_api.cpp

@@ -0,0 +1,191 @@
+
+#include "tetgen_api.h"
+#include "tetgen.h"
+#include "MEM_guardedalloc.h"
+#include <cmath>
+#include <algorithm>
+#include <sstream>
+#include <cstring>
+#include <unordered_map>
+#include <vector>
+
+void init_tetgenremeshdata(TetGenRemeshData *data)
+{
+    data->in_verts = NULL;
+    data->in_faces = NULL;
+    data->in_totverts = 0;
+    data->in_totfaces = 0;
+    data->out_verts = NULL;
+    data->out_facets = NULL;
+    data->out_tets = NULL;
+    data->out_totverts = 0;
+    data->out_totfacets = 0;
+    data->out_tottets = 0;
+}
+
+// Finds the largest edge length of the mesh and computes the volume
+// if that were an edge of a tet using (e^3 / (6*sqrt(2)))
+//static float compute_maxvol(float *verts, unsigned int *faces, int num_faces)
+//{
+//    auto squared_norm = [](float *v0, float *v1)
+//    {
+//        return (v0[0]-v1[0])*(v0[0]-v1[0]) +
+//            (v0[1]-v1[1])*(v0[1]-v1[1]) +
+//            (v0[2]-v1[2])*(v0[2]-v1[2]);
+//    };
+//    float max_sq_edge_len = 0;
+//    for (int i=0; i<num_faces; ++i)
+//    {
+//        unsigned int f[3] = {faces[i*3], faces[i*3+1], faces[i*3+2]};
+//        float v0[3] = {verts[f[0]*3], verts[f[0]*3+1], verts[f[0]*3+2]};
+//        float v1[3] = {verts[f[1]*3], verts[f[1]*3+1], verts[f[1]*3+2]};
+//        float v2[3] = {verts[f[2]*3], verts[f[2]*3+1], verts[f[2]*3+2]};
+//        float max_sq_e = std::max(std::max(
+//            squared_norm(v0,v1),
+//            squared_norm(v1,v2)),
+//            squared_norm(v2,v0));
+//
+//        if( max_sq_e > max_sq_edge_len)
+//            max_sq_edge_len = max_sq_e;
+//    }
+//
+//    double e = std::sqrt(max_sq_edge_len);
+//    return (e*e*e) / (6.f*std::sqrt(2.f));
+//} // end compute maxvol
+
+static void make_tetgenio(
+        float *verts,
+        unsigned int *faces,
+        int numverts,
+        int numfaces,
+		tetgenio &tgio )
+{
+    tgio.initialize();
+	tgio.firstnumber = 0;
+	tgio.mesh_dim = 3;
+	tgio.numberofpoints = numverts;
+	tgio.pointlist = new REAL[tgio.numberofpoints * 3];
+//	tgio.pointlist = (REAL *)MEM_malloc_arrayN(
+//		tgio.numberofpoints, 3 * sizeof(REAL), "tetgen remesh out verts");
+	for (int i=0; i < tgio.numberofpoints; ++i)
+    {
+		tgio.pointlist[i*3+0] = verts[i*3+0];
+		tgio.pointlist[i*3+1] = verts[i*3+1];
+		tgio.pointlist[i*3+2] = verts[i*3+2];
+	}
+	tgio.numberoffacets = numfaces;
+	tgio.facetlist = new tetgenio::facet[tgio.numberoffacets];
+//	tgio.facetlist = (tetgenio::facet *)MEM_malloc_arrayN(
+//		tgio.numberoffacets, sizeof(tetgenio::facet), "tetgen remesh out facets");  
+	tgio.facetmarkerlist = new int[tgio.numberoffacets];
+//	tgio.facetmarkerlist = (int *)MEM_malloc_arrayN(
+//		tgio.numberoffacets, sizeof(int), "tetgen remesh out marker list");
+	for (int i=0; i<numfaces; ++i)
+    {
+		tgio.facetmarkerlist[i] = i;
+		tetgenio::facet *f = &tgio.facetlist[i];
+		f->numberofholes = 0;
+		f->holelist = NULL;
+		f->numberofpolygons = 1;
+		f->polygonlist = new tetgenio::polygon[1];
+		tetgenio::polygon *p = &f->polygonlist[0];
+		p->numberofvertices = 3;
+		p->vertexlist = new int[3];
+		p->vertexlist[0] = faces[i*3+0];
+		p->vertexlist[1] = faces[i*3+1];
+		p->vertexlist[2] = faces[i*3+2];
+	}
+}
+
+bool tetgen_resmesh(TetGenRemeshData *tg)
+{
+//	float maxvol = compute_maxvol(tg->in_verts, tg->in_faces, tg->in_totfaces);
+//    float quality = 1.4;
+
+	// Set up the switches
+	std::stringstream switches;
+//	switches << "Q"; // quiet
+//    switches << "a" << maxvol;
+//	if (quality>0)
+//        switches << "q" << quality;
+
+
+    tetgenio in;
+    make_tetgenio(tg->in_verts, tg->in_faces, tg->in_totverts, tg->in_totfaces, in);
+	tetgenio out;
+    out.initialize();
+    char *c_switches = (char *)switches.str().c_str();
+	tetrahedralize(c_switches, &in, &out);
+
+	if( out.numberoftetrahedra == 0 || out.numberofpoints == 0 )
+    {
+        printf("\n\n\n\n*****FAILED TETGEN\n");
+		return false;
+	}
+
+    // We'll create our custom list of facets to render
+    // with blender. These are all of the triangles that
+    // make up the inner and outer faces, without duplicates.
+    // To avoid duplicates, we'll hash them as a string.
+    // While not super efficient, neither is tetrahedralization...
+    // TODO
+    struct face {
+        int f0, f1, f2;
+        face(int f0_, int f1_, int f2_) : f0(f0_), f1(f1_), f2(f2_) {}
+    };
+    auto face_hash = [](int f0, int f1, int f2){
+        return std::to_string(f0)+" "+std::to_string(f1)+" "+std::to_string(f2);
+    };
+    std::unordered_map<std::string,face> faces;
+
+    // Tets:
+    tg->out_tottets = out.numberoftetrahedra;
+	tg->out_tets = (unsigned int *)MEM_malloc_arrayN(
+		out.numberoftetrahedra, 4 * sizeof(unsigned int), "tetgen remesh tets");
+	for (int i=0; i<out.numberoftetrahedra; ++i)
+    {
+        tg->out_tets[i*4+0] = out.tetrahedronlist[i*4+0];
+        tg->out_tets[i*4+1] = out.tetrahedronlist[i*4+1];
+        tg->out_tets[i*4+2] = out.tetrahedronlist[i*4+2];
+        tg->out_tets[i*4+3] = out.tetrahedronlist[i*4+3];
+
+        // Append faces
+        for(int j=0; j<4; ++j)
+        {
+            int f0 = tg->out_tets[i*4+j];
+            int f1 = tg->out_tets[i*4+(j+1)%4];
+            int f2 = tg->out_tets[i*4+(j+2)%4];
+            std::string hash = face_hash(f0,f1,f2);
+            if (faces.count(hash)!=0)
+                continue;
+            
+            faces.emplace(hash, face(f0,f1,f2));
+        }
+	}
+
+    // Faces:
+    tg->out_totfacets = faces.size();
+	tg->out_facets = (unsigned int *)MEM_malloc_arrayN(
+		tg->out_totfacets, 3 * sizeof(unsigned int), "tetgen remesh facets");
+    int f_idx = 0;
+    for (std::unordered_map<std::string,face>::iterator it = faces.begin();
+        it != faces.end(); ++it, ++f_idx)
+    {
+        tg->out_facets[f_idx*3+0] = it->second.f0;
+        tg->out_facets[f_idx*3+1] = it->second.f1;
+        tg->out_facets[f_idx*3+2] = it->second.f2;    
+    }
+
+    // Vertices:
+    tg->out_totverts = out.numberofpoints;
+	tg->out_verts = (float *)MEM_malloc_arrayN(
+		out.numberofpoints, 3 * sizeof(float), "tetgen remesh verts");
+	for (int i=0; i<out.numberofpoints; ++i)
+    {
+        tg->out_verts[i*3+0] = out.pointlist[i*3+0];
+        tg->out_verts[i*3+1] = out.pointlist[i*3+1];
+        tg->out_verts[i*3+2] = out.pointlist[i*3+2];
+	}
+
+    return true;
+}

intern/tetgen/tetgen_api.h

@@ -0,0 +1,32 @@
+
+#ifndef TETGEN_API_H
+#define TETGEN_API_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct TetGenRemeshData {
+  float *in_verts;
+  unsigned int *in_faces;
+  int in_totfaces;
+  int in_totverts;
+
+  float *out_verts;
+  unsigned int *out_facets;
+  unsigned int *out_tets;
+  int out_totverts;
+  int out_totfacets;
+  int out_tottets;
+} TetGenRemeshData;
+
+void init_tetgenremeshdata(TetGenRemeshData *data);
+
+// Returns true on success
+bool tetgen_resmesh(TetGenRemeshData *tg);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // TETGEN_API_H

release/scripts/startup/bl_ui/properties_data_mesh.py

@@ -502,7 +502,7 @@ class DATA_PT_remesh(MeshButtonsPanel, Panel):
         row = layout.row()
 
         mesh = context.mesh
-        row.prop(mesh, "remesh_mode", text="Mode", expand=True)
+        row.prop(mesh, "remesh_mode", text="Mode", expand=False)
         col = layout.column()
         if mesh.remesh_mode == 'VOXEL':
             col.prop(mesh, "remesh_voxel_size")
@@ -518,8 +518,13 @@ class DATA_PT_remesh(MeshButtonsPanel, Panel):
                 col.prop(mesh, "use_remesh_preserve_vertex_colors", text="Vertex Colors")
 
             col.operator("object.voxel_remesh", text="Voxel Remesh")
-        else:
+        elif mesh.remesh_mode == 'QUAD':
             col.operator("object.quadriflow_remesh", text="QuadriFlow Remesh")
+        elif mesh.remesh_mode == 'TET':
+            col.operator("object.tetgen_remesh", text="Tetrahedralize")
+        elif mesh.remesh_mode == 'TETLATTICE':
+            col.operator("object.tetlattice_remesh", text="Generate Lattice")
+            
 
 
 class DATA_PT_customdata(MeshButtonsPanel, Panel):

release/scripts/startup/bl_ui/properties_physics_softbody.py

@@ -32,7 +32,6 @@ COMPAT_OB_TYPES = {'MESH', 'LATTICE', 'CURVE', 'SURFACE', 'FONT'}
 def softbody_panel_enabled(md):
     return (md.point_cache.is_baked is False)
 
-
 class PhysicButtonsPanel:
     bl_space_type = 'PROPERTIES'
     bl_region_type = 'WINDOW'
@@ -55,8 +54,13 @@ class PHYSICS_PT_softbody(PhysicButtonsPanel, Panel):
         md = context.soft_body
         softbody = md.settings
 
-        layout.prop(softbody, "collision_collection")
-
+        layout.prop(softbody, "solver_mode")
+    
+        if softbody.solver_mode=='LEGACY':
+            # Moved to collision dropdown in ADMMPD
+            layout.prop(softbody, "collision_collection")
+        elif softbody.solver_mode=='ADMMPD':
+            layout.prop(softbody, "admmpd_mesh_mode")
 
 class PHYSICS_PT_softbody_object(PhysicButtonsPanel, Panel):
     bl_label = "Object"
@@ -75,15 +79,33 @@ class PHYSICS_PT_softbody_object(PhysicButtonsPanel, Panel):
         layout.enabled = softbody_panel_enabled(md)
         flow = layout.grid_flow(row_major=True, columns=0, even_columns=True, even_rows=False, align=True)
 
-        col = flow.column()
-        col.prop(softbody, "friction")
+        if softbody.solver_mode=='LEGACY':
 
-        col.separator()
+            col = flow.column()
+            col.prop(softbody, "friction")
 
-        col = flow.column()
-        col.prop(softbody, "mass")
+            col.separator()
 
-        col.prop_search(softbody, "vertex_group_mass", ob, "vertex_groups", text="Control Point")
+            col = flow.column()
+            col.prop(softbody, "mass")
+
+            col.prop_search(softbody, "vertex_group_mass", ob, "vertex_groups", text="Control Point")
+
+        elif softbody.solver_mode=='ADMMPD':
+
+            col = flow.column()
+
+            if softbody.admmpd_mesh_mode == 'EMBEDDED':
+                col.prop(softbody, "admmpd_material")
+
+            col.prop(softbody, "admmpd_youngs_exp")
+            col.prop(softbody, "admmpd_poisson")
+            col.prop(softbody, "admmpd_density_kgm3")
+
+            if softbody.admmpd_mesh_mode == 'CLOTH':
+                col.prop(softbody, "admmpd_strainlimit_max")
+            elif softbody.admmpd_mesh_mode == 'EMBEDDED':
+                col.prop(softbody, "admmpd_embed_res")
 
 
 class PHYSICS_PT_softbody_simulation(PhysicButtonsPanel, Panel):
@@ -92,6 +114,12 @@ class PHYSICS_PT_softbody_simulation(PhysicButtonsPanel, Panel):
     bl_options = {'DEFAULT_CLOSED'}
     COMPAT_ENGINES = {'BLENDER_RENDER', 'BLENDER_EEVEE', 'BLENDER_WORKBENCH'}
 
+    @classmethod
+    def poll(cls, context):
+        md = context.soft_body
+        softbody = md.settings
+        return (softbody.solver_mode=='LEGACY')
+
     def draw(self, context):
         layout = self.layout
         layout.use_property_split = True
@@ -139,6 +167,9 @@ class PHYSICS_PT_softbody_goal(PhysicButtonsPanel, Panel):
 
         layout.prop_search(softbody, "vertex_group_goal", ob, "vertex_groups", text="Vertex Group")
 
+        if softbody.solver_mode == 'ADMMPD':
+            layout.prop(softbody, "admmpd_pk_exp")
+
 
 class PHYSICS_PT_softbody_goal_strengths(PhysicButtonsPanel, Panel):
     bl_label = "Strengths"
@@ -146,6 +177,12 @@ class PHYSICS_PT_softbody_goal_strengths(PhysicButtonsPanel, Panel):
     bl_options = {'DEFAULT_CLOSED'}
     COMPAT_ENGINES = {'BLENDER_RENDER', 'BLENDER_EEVEE', 'BLENDER_WORKBENCH'}
 
+    @classmethod
+    def poll(cls, context):
+        md = context.soft_body
+        softbody = md.settings
+        return (softbody.solver_mode=='LEGACY')
+
     def draw(self, context):
         layout = self.layout
         layout.use_property_split = True
@@ -172,6 +209,12 @@ class PHYSICS_PT_softbody_goal_settings(PhysicButtonsPanel, Panel):
     bl_options = {'DEFAULT_CLOSED'}
     COMPAT_ENGINES = {'BLENDER_RENDER', 'BLENDER_EEVEE', 'BLENDER_WORKBENCH'}
 
+    @classmethod
+    def poll(cls, context):
+        md = context.soft_body
+        softbody = md.settings
+        return (softbody.solver_mode=='LEGACY')
+
     def draw(self, context):
         layout = self.layout
         layout.use_property_split = True
@@ -195,6 +238,12 @@ class PHYSICS_PT_softbody_edge(PhysicButtonsPanel, Panel):
     bl_options = {'DEFAULT_CLOSED'}
     COMPAT_ENGINES = {'BLENDER_RENDER', 'BLENDER_EEVEE', 'BLENDER_WORKBENCH'}
 
+    @classmethod
+    def poll(cls, context):
+        md = context.soft_body
+        softbody = md.settings
+        return (softbody.solver_mode=='LEGACY')
+
     def draw_header(self, context):
         softbody = context.soft_body.settings
 
@@ -282,6 +331,37 @@ class PHYSICS_PT_softbody_edge_stiffness(PhysicButtonsPanel, Panel):
 
         layout.prop(softbody, "shear")
 
+class PHYSICS_PT_softbody_admmpdcollision(PhysicButtonsPanel, Panel):
+    bl_label = "Collision"
+    bl_parent_id = 'PHYSICS_PT_softbody'
+    bl_options = {'DEFAULT_CLOSED'}
+    COMPAT_ENGINES = {'BLENDER_RENDER', 'BLENDER_EEVEE', 'BLENDER_WORKBENCH'}
+
+    @classmethod
+    def poll(cls, context):
+        md = context.soft_body
+        softbody = md.settings
+        return (softbody.solver_mode=='ADMMPD' and softbody.admmpd_mesh_mode == 'EMBEDDED')
+
+    def draw(self, context):
+        layout = self.layout
+        layout.use_property_split = True
+        md = context.soft_body
+        softbody = md.settings
+        layout.enabled = softbody_panel_enabled(md)
+        ob = context.object
+
+        layout.prop(softbody, "collision_collection")
+
+        layout.prop(softbody, "admmpd_ck_exp")
+        layout.prop(softbody, "admmpd_floor_z")
+        layout.prop(softbody, "admmpd_self_collision")
+        flow = layout.grid_flow(row_major=True, columns=0, even_columns=True, even_rows=False, align=True)
+        col = flow.column()
+        col.active = softbody.admmpd_self_collision
+        col.prop_search(softbody, "vertex_group_selfcollide", ob, "vertex_groups")
+
+
 
 class PHYSICS_PT_softbody_collision(PhysicButtonsPanel, Panel):
     bl_label = "Self Collision"
@@ -289,6 +369,12 @@ class PHYSICS_PT_softbody_collision(PhysicButtonsPanel, Panel):
     bl_options = {'DEFAULT_CLOSED'}
     COMPAT_ENGINES = {'BLENDER_RENDER', 'BLENDER_EEVEE', 'BLENDER_WORKBENCH'}
 
+    @classmethod
+    def poll(cls, context):
+        md = context.soft_body
+        softbody = md.settings
+        return (softbody.solver_mode=='LEGACY')
+
     def draw_header(self, context):
         softbody = context.soft_body.settings
 
@@ -334,14 +420,25 @@ class PHYSICS_PT_softbody_solver(PhysicButtonsPanel, Panel):
         layout.active = softbody_panel_enabled(md)
         flow = layout.grid_flow(row_major=True, columns=0, even_columns=True, even_rows=False, align=True)
 
-        col = flow.column(align=True)
-        col.prop(softbody, "step_min", text="Step Size Min")
-        col.prop(softbody, "step_max", text="Max")
+        if softbody.solver_mode == 'LEGACY':
 
-        col = flow.column()
-        col.prop(softbody, "use_auto_step", text="Auto-Step")
-        col.prop(softbody, "error_threshold")
+            col = flow.column(align=True)
+            col.prop(softbody, "step_min", text="Step Size Min")
+            col.prop(softbody, "step_max", text="Max")
 
+            col = flow.column()
+            col.prop(softbody, "use_auto_step", text="Auto-Step")
+            col.prop(softbody, "error_threshold")
+
+        elif softbody.solver_mode == 'ADMMPD':
+
+            col = flow.column(align=True)
+            col.prop(softbody, "admmpd_linsolver")
+            col.prop(softbody, "admmpd_substeps")
+            col.prop(softbody, "admmpd_max_admm_iters")
+            col.prop(softbody, "admmpd_converge_eps")
+            col.prop(softbody, "admmpd_gravity")
+            col.prop(softbody, "admmpd_loglevel")
 
 class PHYSICS_PT_softbody_solver_diagnostics(PhysicButtonsPanel, Panel):
     bl_label = "Diagnostics"
@@ -349,6 +446,12 @@ class PHYSICS_PT_softbody_solver_diagnostics(PhysicButtonsPanel, Panel):
     bl_options = {'DEFAULT_CLOSED'}
     COMPAT_ENGINES = {'BLENDER_RENDER', 'BLENDER_EEVEE', 'BLENDER_WORKBENCH'}
 
+    @classmethod
+    def poll(cls, context):
+        md = context.soft_body
+        softbody = md.settings
+        return (softbody.solver_mode=='LEGACY')
+
     def draw(self, context):
         layout = self.layout
         layout.use_property_split = True
@@ -368,6 +471,12 @@ class PHYSICS_PT_softbody_solver_helpers(PhysicButtonsPanel, Panel):
     bl_options = {'DEFAULT_CLOSED'}
     COMPAT_ENGINES = {'BLENDER_RENDER', 'BLENDER_EEVEE', 'BLENDER_WORKBENCH'}
 
+    @classmethod
+    def poll(cls, context):
+        md = context.soft_body
+        softbody = md.settings
+        return (softbody.solver_mode=='LEGACY')
+
     def draw(self, context):
         layout = self.layout
         layout.use_property_split = True
@@ -391,6 +500,12 @@ class PHYSICS_PT_softbody_field_weights(PhysicButtonsPanel, Panel):
     bl_options = {'DEFAULT_CLOSED'}
     COMPAT_ENGINES = {'BLENDER_RENDER', 'BLENDER_EEVEE', 'BLENDER_WORKBENCH'}
 
+    @classmethod
+    def poll(cls, context):
+        md = context.soft_body
+        softbody = md.settings
+        return (softbody.solver_mode=='LEGACY')
+
     def draw(self, context):
         md = context.soft_body
         softbody = md.settings
@@ -409,6 +524,7 @@ classes = (
     PHYSICS_PT_softbody_edge,
     PHYSICS_PT_softbody_edge_aerodynamics,
     PHYSICS_PT_softbody_edge_stiffness,
+    PHYSICS_PT_softbody_admmpdcollision,
     PHYSICS_PT_softbody_collision,
     PHYSICS_PT_softbody_solver,
     PHYSICS_PT_softbody_solver_diagnostics,

source/blender/blenkernel/BKE_mesh_remesh_voxel.h

@@ -56,7 +56,13 @@ struct Mesh *BKE_mesh_remesh_quadriflow_to_mesh_nomain(struct Mesh *mesh,
                                                        bool adaptive_scale,
                                                        void *update_cb,
                                                        void *update_cb_data);
-
+struct Mesh *BKE_mesh_remesh_tetgen_to_mesh_nomain(struct Mesh *mesh,
+                                                   unsigned int **tets,
+                                                   int *numtets);
+struct Mesh *BKE_mesh_remesh_tetlattice_to_mesh_nomain(struct Mesh *mesh,
+                                                   int subdivisions,
+                                                   unsigned int **tets,
+                                                   int *numtets);
 /* Data reprojection functions */
 void BKE_mesh_remesh_reproject_paint_mask(struct Mesh *target, struct Mesh *source);
 void BKE_remesh_reproject_vertex_paint(struct Mesh *target, struct Mesh *source);

source/blender/blenkernel/BKE_softbody.h

@@ -30,6 +30,7 @@ struct Depsgraph;
 struct Object;
 struct Scene;
 struct SoftBody;
+struct ReportList;
 
 typedef struct BodyPoint {
   float origS[3], origE[3], origT[3], pos[3], vec[3], force[3];
@@ -49,6 +50,12 @@ typedef struct BodyPoint {
 /* allocates and initializes general main data */
 extern struct SoftBody *sbNew(struct Scene *scene);
 
+/* copies external solver data from src to dest */
+extern void sbExternalCopy(struct Object *dest, struct Object *src);
+
+/* initializes settings for external solvers */
+extern void sbExternalSetDefault(struct SoftBody *sb);
+
 /* frees internal data and softbody itself */
 extern void sbFree(struct Object *ob);
 

source/blender/blenkernel/CMakeLists.txt

@@ -45,6 +45,7 @@ set(INC
   ../../../intern/iksolver/extern
   ../../../intern/atomic
   ../../../intern/clog
+  ../../../intern/softbody
   ../../../intern/libmv
   ../../../intern/mantaflow/extern
   ../../../intern/memutil
@@ -433,6 +434,7 @@ set(LIB
   bf_ikplugin
   bf_imbuf
   bf_intern_clog
+  bf_intern_admmpd
   bf_intern_ghost
   bf_intern_guardedalloc
   bf_intern_libmv  # Uses stub when disabled.
@@ -685,6 +687,16 @@ if(WITH_QUADRIFLOW)
   add_definitions(-DWITH_QUADRIFLOW)
 endif()
 
+if(WITH_TETGEN)
+  list(APPEND INC
+     ../../../intern/tetgen
+  )
+  list(APPEND LIB
+     bf_intern_tetgen
+  )
+  add_definitions(-DWITH_TETGEN)
+endif()
+
 if(WITH_XR_OPENXR)
   add_definitions(-DWITH_XR_OPENXR)
 endif()

source/blender/blenkernel/intern/mesh_remesh_voxel.c

@@ -48,6 +48,8 @@
 
 #include "bmesh_tools.h"
 
+#include "admmpd_api.h"
+
 #ifdef WITH_OPENVDB
 #  include "openvdb_capi.h"
 #endif
@@ -56,6 +58,10 @@
 #  include "quadriflow_capi.hpp"
 #endif
 
+#ifdef WITH_TETGEN
+#  include "tetgen_api.h"
+#endif
+
 #ifdef WITH_OPENVDB
 struct OpenVDBLevelSet *BKE_mesh_remesh_voxel_ovdb_mesh_to_level_set_create(
     Mesh *mesh, struct OpenVDBTransform *transform)
@@ -163,11 +169,11 @@ static Mesh *BKE_mesh_remesh_quadriflow(Mesh *input_mesh,
                                         void *update_cb,
                                         void *update_cb_data)
 {
-  /* Ensure that the triangulated mesh data is up to data */
+  /* Ensure that the triangulated mesh data is up to data. */
   BKE_mesh_runtime_looptri_recalc(input_mesh);
   const MLoopTri *looptri = BKE_mesh_runtime_looptri_ensure(input_mesh);
 
-  /* Gather the required data for export to the internal quadiflow mesh format */
+  /* Gather the required data for export to the internal quadiflow mesh format. */
   MVertTri *verttri = MEM_callocN(sizeof(*verttri) * BKE_mesh_runtime_looptri_len(input_mesh),
                                   "remesh_looptri");
   BKE_mesh_runtime_verttri_from_looptri(
@@ -193,7 +199,7 @@ static Mesh *BKE_mesh_remesh_quadriflow(Mesh *input_mesh,
     faces[i * 3 + 2] = vt->tri[2];
   }
 
-  /* Fill out the required input data */
+  /* Fill out the required input data. */
   QuadriflowRemeshData qrd;
 
   qrd.totfaces = totfaces;
@@ -211,7 +217,7 @@ static Mesh *BKE_mesh_remesh_quadriflow(Mesh *input_mesh,
 
   qrd.out_faces = NULL;
 
-  /* Run the remesher */
+  /* Run the remesher. */
   QFLOW_quadriflow_remesh(&qrd, update_cb, update_cb_data);
 
   MEM_freeN(verts);
@@ -219,7 +225,7 @@ static Mesh *BKE_mesh_remesh_quadriflow(Mesh *input_mesh,
   MEM_freeN(verttri);
 
   if (qrd.out_faces == NULL) {
-    /* The remeshing was canceled */
+    /* The remeshing was canceled. */
     return NULL;
   }
 
@@ -260,6 +266,228 @@ static Mesh *BKE_mesh_remesh_quadriflow(Mesh *input_mesh,
 }
 #endif
 
+#ifdef WITH_TETGEN
+static Mesh *BKE_mesh_remesh_tetgen(Mesh *input_mesh, unsigned int **tets, int *numtets)
+{
+  /* Ensure that the triangulated mesh data is up to data. */
+  BKE_mesh_runtime_looptri_recalc(input_mesh);
+  const MLoopTri *looptri = BKE_mesh_runtime_looptri_ensure(input_mesh);
+
+  /* Gather the required data. */
+  MVertTri *verttri = MEM_callocN(sizeof(*verttri) * BKE_mesh_runtime_looptri_len(input_mesh),
+                                  "remesh_looptri");
+  BKE_mesh_runtime_verttri_from_looptri(
+      verttri, input_mesh->mloop, looptri, BKE_mesh_runtime_looptri_len(input_mesh));
+
+  unsigned int totfaces = BKE_mesh_runtime_looptri_len(input_mesh);
+  unsigned int totverts = input_mesh->totvert;
+  float *verts = (float *)MEM_malloc_arrayN(totverts * 3, sizeof(float), "remesh_input_verts");
+  unsigned int *faces = (unsigned int *)MEM_malloc_arrayN(
+      totfaces * 3, sizeof(unsigned int), "remesh_intput_faces");
+
+  for (unsigned int i = 0; i < totverts; i++) {
+    MVert *mvert = &input_mesh->mvert[i];
+    verts[i * 3] = mvert->co[0];
+    verts[i * 3 + 1] = mvert->co[1];
+    verts[i * 3 + 2] = mvert->co[2];
+  }
+
+  for (unsigned int i = 0; i < totfaces; i++) {
+    MVertTri *vt = &verttri[i];
+    faces[i * 3] = vt->tri[0];
+    faces[i * 3 + 1] = vt->tri[1];
+    faces[i * 3 + 2] = vt->tri[2];
+  }
+
+  /* Call the tetgen remesher. */
+  TetGenRemeshData tg;
+  init_tetgenremeshdata(&tg);
+  tg.in_totfaces = totfaces;
+  tg.in_totverts = totverts;
+  tg.in_verts = verts;
+  tg.in_faces = faces;
+  bool success = tetgen_resmesh(&tg);
+
+  MEM_freeN(verts);
+  verts = NULL;
+  tg.in_verts = NULL;
+  MEM_freeN(faces);
+  faces = NULL;
+  tg.in_faces = NULL;
+  MEM_freeN(verttri);
+  verttri = NULL;
+
+  Mesh *mesh = NULL;
+  if (success) {
+    /* Construct the new output mesh. */
+    mesh = BKE_mesh_new_nomain(tg.out_totverts, 0, 0, (tg.out_totfacets * 3), tg.out_totfacets);
+
+    for (int i = 0; i < tg.out_totverts; i++) {
+      copy_v3_v3(mesh->mvert[i].co, &tg.out_verts[i * 3]);
+    }
+
+    MPoly *mp = mesh->mpoly;
+    MLoop *ml = mesh->mloop;
+    for (int i = 0; i < tg.out_totfacets; i++, mp++, ml += 3) {
+      mp->loopstart = (int)(ml - mesh->mloop);
+      mp->totloop = 3;
+      ml[0].v = tg.out_facets[i * 3];
+      ml[1].v = tg.out_facets[i * 3 + 1];
+      ml[2].v = tg.out_facets[i * 3 + 2];
+    }
+    BKE_mesh_calc_edges(mesh, false, false);
+    BKE_mesh_calc_normals(mesh);
+
+    *numtets = tg.out_tottets;
+    *tets = (unsigned int *)MEM_malloc_arrayN(
+        tg.out_tottets * 4, sizeof(unsigned int), "remesh_output_tets");
+    memcpy(*tets, tg.out_tets, tg.out_tottets * 4 * sizeof(unsigned int));
+
+  }
+
+  if (tg.out_verts != NULL) {
+    MEM_freeN(tg.out_verts);
+    tg.out_verts = NULL;
+  }
+
+  if (tg.out_facets != NULL) {
+    MEM_freeN(tg.out_facets);
+    tg.out_facets = NULL;
+  }
+
+  if (tg.out_tets != NULL) {
+    MEM_freeN(tg.out_tets);
+    tg.out_tets = NULL;
+  }
+
+  return mesh;
+}
+#endif
+
+struct Mesh *BKE_mesh_remesh_tetgen_to_mesh_nomain(struct Mesh *mesh,
+                                                   unsigned int **tets,
+                                                   int *numtets)
+{
+#ifdef WITH_TETGEN
+  return BKE_mesh_remesh_tetgen(mesh, tets, numtets);
+#else
+  UNUSED_VARS(mesh, tets, numtets);
+#endif
+  return NULL;
+}
+
+static Mesh *BKE_mesh_remesh_tetlattice(struct Mesh *input_mesh,
+                                        int subdivisions,
+                                        unsigned int **tets,
+                                        int *numtets)
+{
+
+  /* Ensure that the triangulated mesh data is up to data. */
+  BKE_mesh_runtime_looptri_recalc(input_mesh);
+  const MLoopTri *looptri = BKE_mesh_runtime_looptri_ensure(input_mesh);
+
+  /* Gather the required data. */
+  MVertTri *verttri = MEM_callocN(sizeof(*verttri) * BKE_mesh_runtime_looptri_len(input_mesh),
+                                  "remesh_looptri");
+  BKE_mesh_runtime_verttri_from_looptri(
+      verttri, input_mesh->mloop, looptri, BKE_mesh_runtime_looptri_len(input_mesh));
+
+  unsigned int totfaces = BKE_mesh_runtime_looptri_len(input_mesh);
+  unsigned int totverts = input_mesh->totvert;
+  float *verts = (float *)MEM_malloc_arrayN(totverts * 3, sizeof(float), "remesh_input_verts");
+  unsigned int *faces = (unsigned int *)MEM_malloc_arrayN(
+      totfaces * 3, sizeof(unsigned int), "remesh_intput_faces");
+
+  for (unsigned int i = 0; i < totverts; i++) {
+    MVert *mvert = &input_mesh->mvert[i];
+    verts[i * 3] = mvert->co[0];
+    verts[i * 3 + 1] = mvert->co[1];
+    verts[i * 3 + 2] = mvert->co[2];
+  }
+
+  for (unsigned int i = 0; i < totfaces; i++) {
+    MVertTri *vt = &verttri[i];
+    faces[i * 3] = vt->tri[0];
+    faces[i * 3 + 1] = vt->tri[1];
+    faces[i * 3 + 2] = vt->tri[2];
+  }
+
+  float *out_verts = NULL;
+  int out_totverts = 0;
+  admmpd_compute_lattice(
+    subdivisions,
+    verts, totverts,
+    faces, totfaces,
+    &out_verts, &out_totverts,
+    tets, numtets);
+  bool success = out_totverts>0 && *numtets>0;
+
+  MEM_freeN(verts);
+  verts = NULL;
+  MEM_freeN(faces);
+  faces = NULL;
+  MEM_freeN(verttri);
+  verttri = NULL;
+
+  Mesh *mesh = NULL;
+  if (success) {
+
+    int nt = *numtets;
+    int nf = *numtets * 4;
+
+    /* Construct the new output mesh. */
+    mesh = BKE_mesh_new_nomain(out_totverts, 0, 0, (nf*3), nf);
+
+    for (int i = 0; i < out_totverts; i++) {
+      copy_v3_v3(mesh->mvert[i].co, &out_verts[i * 3]);
+    }
+
+    MPoly *mp = mesh->mpoly;
+    MLoop *ml = mesh->mloop;
+    for (int i=0; i<nt; ++i) {
+  
+      int tet[4];
+      tet[0] = (*tets)[i*4+0];
+      tet[1] = (*tets)[i*4+1];
+      tet[2] = (*tets)[i*4+2];
+      tet[3] = (*tets)[i*4+3];
+
+      int tet_facets[4*3] = {
+        0, 2, 1,
+        0, 1, 3,
+        0, 3, 2,
+        1, 2, 3
+      };
+
+      for (int j = 0; j < 4; j++, mp++, ml += 3) {
+        mp->loopstart = (int)(ml - mesh->mloop);
+        mp->totloop = 3;
+        ml[0].v = tet[tet_facets[j*3+0]];
+        ml[1].v = tet[tet_facets[j*3+1]];
+        ml[2].v = tet[tet_facets[j*3+2]];
+      }
+    }
+
+  }
+  BKE_mesh_calc_edges(mesh, false, false);
+  BKE_mesh_calc_normals(mesh);
+
+  if (out_verts != NULL) {
+    MEM_freeN(out_verts);
+    out_verts = NULL;
+  }
+
+  return mesh;
+}
+
+struct Mesh *BKE_mesh_remesh_tetlattice_to_mesh_nomain(struct Mesh *mesh,
+                                                   int subdivisions,
+                                                   unsigned int **tets,
+                                                   int *numtets)
+{
+  return BKE_mesh_remesh_tetlattice(mesh, subdivisions, tets, numtets);
+}
+
 Mesh *BKE_mesh_remesh_quadriflow_to_mesh_nomain(Mesh *mesh,
                                                 int target_faces,
                                                 int seed,

source/blender/blenkernel/intern/object.c

@@ -1402,6 +1402,8 @@ void BKE_object_copy_softbody(struct Object *ob_dst, const struct Object *ob_src
 
   sbn = MEM_dupallocN(sb);
 
+  sbExternalCopy(ob_dst,ob_src);
+
   if ((flag & LIB_ID_COPY_CACHES) == 0) {
     sbn->totspring = sbn->totpoint = 0;
     sbn->bpoint = NULL;

source/blender/blenkernel/intern/softbody.c

@@ -44,6 +44,8 @@
 
 #include "CLG_log.h"
 
+#include "admmpd_api.h"
+
 #include "MEM_guardedalloc.h"
 
 /* types */
@@ -77,6 +79,8 @@
 #include "DEG_depsgraph.h"
 #include "DEG_depsgraph_query.h"
 
+#include "../windowmanager/WM_api.h"
+
 #include "PIL_time.h"
 
 static CLG_LogRef LOG = {"bke.softbody"};
@@ -934,8 +938,8 @@ static void free_softbody_intern(SoftBody *sb)
         }
       }
       MEM_freeN(sb->bpoint);
+      sb->bpoint = NULL;
     }
-
     if (sb->bspring) {
       MEM_freeN(sb->bspring);
     }
@@ -3120,7 +3124,6 @@ static void sb_new_scratch(SoftBody *sb)
 SoftBody *sbNew(Scene *scene)
 {
   SoftBody *sb;
-
   sb = MEM_callocN(sizeof(SoftBody), "softbody");
 
   sb->mediafrict = 0.5f;
@@ -3163,6 +3166,8 @@ SoftBody *sbNew(Scene *scene)
   sb->shared = MEM_callocN(sizeof(*sb->shared), "SoftBody_Shared");
   sb->shared->pointcache = BKE_ptcache_add(&sb->shared->ptcaches);
 
+  sbExternalSetDefault(sb);
+
   if (!sb->effector_weights) {
     sb->effector_weights = BKE_effector_add_weights(NULL);
   }
@@ -3182,8 +3187,34 @@ void sbFree(Object *ob)
 
   free_softbody_intern(sb);
 
+  /* Only free shared data on non-CoW copies */
   if ((ob->id.tag & LIB_TAG_NO_MAIN) == 0) {
-    /* Only free shared data on non-CoW copies */
+
+    /* Clear the ADMMPD linked list. Because the linked
+    * list pointer is copied to all soft body objects, we
+    * want to be careful not to free twice. */
+    {
+      ADMMPDInterfaceData *admmpd;
+      int cleared_admmpd_list = 0;
+      if (sb->shared->admmpd_list) {
+        while ((admmpd = BLI_pophead(sb->shared->admmpd_list))) {
+          cleared_admmpd_list = 1;
+          if (admmpd != NULL) {
+            admmpd_dealloc(admmpd);
+            MEM_freeN(admmpd);
+          }
+        }
+      }
+      /* If we deleted data from the linked list
+      * we can delete the head ptr */
+      if (cleared_admmpd_list) {
+        MEM_freeN(sb->shared->admmpd_list);
+      }
+      /* Otherwise we've allready freed, just
+      * set the ptr to NULL */
+      sb->shared->admmpd_list = NULL;
+    }
+
     BKE_ptcache_free_list(&sb->shared->ptcaches);
     sb->shared->pointcache = NULL;
     MEM_freeN(sb->shared);
@@ -3205,7 +3236,6 @@ void sbFreeSimulation(SoftBody *sb)
 void sbObjectToSoftbody(Object *ob)
 {
   // ob->softflag |= OB_SB_REDO;
-
   free_softbody_intern(ob->soft);
 }
 
@@ -3531,6 +3561,186 @@ static void sbStoreLastFrame(struct Depsgraph *depsgraph, Object *object, float
   object_orig->soft->last_frame = framenr;
 }
 
+static inline ADMMPDInterfaceData* get_admmpd_for_object(Object *ob)
+{
+  SoftBody *sb = ob->soft;
+  if (!sb) {
+    return NULL;
+  }
+  int found = 0;
+  ADMMPDInterfaceData *admmpd = sb->shared->admmpd_list->first;
+  while (admmpd != NULL) {
+      if (strcmp(admmpd->name,ob->id.name)==0) {
+        found = 1;
+        break;
+      }
+      admmpd = admmpd->next;
+  }
+  if (!found) {
+    return NULL;
+  }
+  return admmpd;
+}
+
+static void update_collider_admmpd(Depsgraph *depsgraph,
+                                   Collection *collection,
+                                   Object *vertexowner)
+{
+
+  SoftBody *sb = vertexowner->soft;
+  if (!sb) {
+    return;
+  }
+
+  ADMMPDInterfaceData *admmpd = get_admmpd_for_object(vertexowner);
+  if (!admmpd) {
+    CLOG_ERROR(&LOG, "No ADMM-PD data found for object %s", vertexowner->id.name);
+    return;
+  }
+
+  unsigned int numobjects;
+  Object **objects = BKE_collision_objects_create(
+      depsgraph, vertexowner, collection, &numobjects, eModifierType_Collision);
+
+  admmpd_update_obstacles(admmpd,objects,numobjects);
+
+  BKE_collision_objects_free(objects);
+
+#if 0
+  /* How many faces and vertices do we need to allocate? */
+  int tot_verts = 0;
+  int tot_faces = 0;
+  for (int i = 0; i < numobjects; ++i) {
+    Object *ob = objects[i];
+    if (ob->type != OB_MESH) {
+      continue;
+    }
+    /* If the collision object is the same as the softbody object,
+    * we don't want to add it. Otherwise it creates a duplicate obstacle
+    * at the initial location of the softbody. */
+    if (strcmp(ob->id.name,vertexowner->id.name)==0) {
+      continue;
+    }
+    if (ob->pd && ob->pd->deflect) {
+      CollisionModifierData *cmd = (CollisionModifierData *)BKE_modifiers_findby_type(
+          ob, eModifierType_Collision);
+      if (!cmd)
+        continue;
+      tot_verts += cmd->mvert_num;
+      tot_faces += cmd->tri_num;
+    }
+  }
+
+  float *obs_v0 = MEM_callocN(sizeof(float) * 3 * tot_verts, __func__);
+  float *obs_v1 = MEM_callocN(sizeof(float) * 3 * tot_verts, __func__);
+  unsigned int *obs_faces = MEM_callocN(sizeof(unsigned int) * 3 * tot_faces, __func__);
+
+  /* Copy over vertices and faces */
+  int curr_verts = 0;
+  int curr_faces = 0;
+  for (int i = 0; i < numobjects; ++i) {
+    Object *ob = objects[i];
+    if (ob->type == OB_MESH) {
+      if (ob->pd && ob->pd->deflect) {
+        CollisionModifierData *cmd = (CollisionModifierData *)BKE_modifiers_findby_type(
+            ob, eModifierType_Collision);
+        if (!cmd)
+          continue;
+
+        for (int j = 0; j < cmd->mvert_num; ++j) {
+          int v_idx = j * 3 + curr_verts * 3;
+          for (int k = 0; k < 3; ++k) {
+            obs_v0[v_idx + k] = cmd->x[j].co[k];
+            obs_v1[v_idx + k] = cmd->xnew[j].co[k];
+          }
+        }
+
+        for (int j = 0; j < cmd->tri_num; ++j) {
+          int f_idx = j * 3 + curr_faces * 3;
+          for (int k = 0; k < 3; ++k) {
+            obs_faces[f_idx + k] = cmd->tri[j].tri[k] + curr_verts;
+          }
+        }
+
+        curr_verts += cmd->mvert_num;
+        curr_faces += cmd->tri_num;
+      }
+    }
+  }
+
+  /* Pass obstacle data to ADMMPD */
+  admmpd_update_obstacles(admmpd, obs_v0, obs_v1, tot_verts, obs_faces, tot_faces);
+
+  MEM_freeN(obs_v0);
+  MEM_freeN(obs_v1);
+  MEM_freeN(obs_faces);
+  BKE_collision_objects_free(objects);
+#endif
+}
+
+void sbExternalCopy(Object *dest_ob, Object *src_ob)
+{
+  (void)(src_ob);
+  if (!dest_ob) {
+    CLOG_ERROR(&LOG, "No dest object in sbExternalCopy");
+    return;
+  }
+  SoftBody *dest = dest_ob->soft;
+  if (!dest) {
+    CLOG_ERROR(&LOG, "No softbody in sbExternalCopy");
+    return;
+  }
+  if (!dest->shared) {
+    CLOG_ERROR(&LOG, "No shared in sbExternalCopy");
+    return;
+  }
+  /* This can happen on readfile: */
+  if (!dest->shared->admmpd_list) {
+    dest->shared->admmpd_list = MEM_callocN(sizeof(ListBase), "SoftBody_ADMMPD_List");
+  }
+  /* Create ADMM-PD data for this object if it does not already exist. */
+  ADMMPDInterfaceData *admmpd = get_admmpd_for_object(dest_ob);
+  if (admmpd==NULL) {
+    admmpd = MEM_callocN(sizeof(ADMMPDInterfaceData), "ADMMPD");
+    strcpy(admmpd->name, dest_ob->id.name);
+    BLI_addtail(dest->shared->admmpd_list, admmpd);
+  }
+}
+
+void sbExternalSetDefault(SoftBody *sb)
+{
+  if (!sb) {
+    return;
+  }
+  if (sb->shared == NULL) {
+    sb->shared = MEM_callocN(sizeof(*sb->shared), "SoftBody_Shared");
+  }
+  if (sb->shared->admmpd_list == NULL) {
+    sb->shared->admmpd_list = MEM_callocN(sizeof(ListBase), "SoftBody_ADMMPD_List");
+  }
+
+  sb->solver_mode = SOLVER_MODE_LEGACY;
+  sb->admmpd_mesh_mode = 0; /* Embedded. */
+  sb->admmpd_substeps = 1;
+  sb->admmpd_max_admm_iters = 20;
+  sb->admmpd_self_collision = 0;
+  sb->admmpd_material = 0;  /* ARAP. */
+  sb->admmpd_embed_res = 3; /* Max subdivisions for embedded lattice. */
+  sb->admmpd_converge_eps = 1e-4; /* Solver residual stop condition. */
+  sb->admmpd_youngs_exp = 6; /* Exponent of Young's mod. */
+  sb->admmpd_poisson = 0.399; 
+  sb->admmpd_density_kgm3 = 1522; /* Density of rubber. */
+  sb->admmpd_ck_exp = 7; /* Exponent of collision stiffness. */
+  sb->admmpd_pk_exp = 4; /* Exponent of pin stiffness. */
+  sb->admmpd_floor_z = -999;
+  sb->admmpd_gravity = -9.8;
+  sb->admmpd_strainlimit_min = 0;    /* 0 = compression allowed. */
+  sb->admmpd_strainlimit_max = 100;  /* 100 = 100x strech allowed. */
+  sb->admmpd_maxthreads = -1; /* Auto detect. */
+  sb->admmpd_loglevel = 1;   /* Low terminal output. */
+  sb->admmpd_linsolver = 1;  /* PCG solver. */
+}
+
 /* simulates one step. framenr is in frames */
 void sbObjectStep(struct Depsgraph *depsgraph,
                   Scene *scene,
@@ -3539,7 +3749,19 @@ void sbObjectStep(struct Depsgraph *depsgraph,
                   float (*vertexCos)[3],
                   int numVerts)
 {
+
   SoftBody *sb = ob->soft;
+  if (!sb) {
+    return;
+  }
+
+  ADMMPDInterfaceData *admmpd = get_admmpd_for_object(ob);
+  if (admmpd == NULL) {
+    CLOG_ERROR(&LOG, "No ADMM-PD data in sbObjectStep");
+    WM_report(RPT_ERROR, "No ADMM-PD data in sbObjectStep");
+    return;
+  }
+
   PointCache *cache;
   PTCacheID pid;
   float dtime, timescale;
@@ -3555,9 +3777,11 @@ void sbObjectStep(struct Depsgraph *depsgraph,
 
   /* check for changes in mesh, should only happen in case the mesh
    * structure changes during an animation */
-  if (sb->bpoint && numVerts != sb->totpoint) {
-    BKE_ptcache_invalidate(cache);
-    return;
+  if (sb->solver_mode == SOLVER_MODE_LEGACY) {
+    if (sb->bpoint && numVerts != sb->totpoint) {
+      BKE_ptcache_invalidate(cache);
+      return;
+    }
   }
 
   /* clamp frame ranges */
@@ -3570,38 +3794,92 @@ void sbObjectStep(struct Depsgraph *depsgraph,
   }
 
   /* verify if we need to create the softbody data */
-  if (sb->bpoint == NULL ||
-      ((ob->softflag & OB_SB_EDGES) && !ob->soft->bspring && object_has_edges(ob))) {
+  if (sb->solver_mode == SOLVER_MODE_ADMMPD) {
+    /* If it's the first frame, we probably want to intialize. */
+    bool is_first_frame = framenr == startframe;
 
-    switch (ob->type) {
-      case OB_MESH:
-        mesh_to_softbody(scene, ob);
-        break;
-      case OB_LATTICE:
-        lattice_to_softbody(scene, ob);
-        break;
-      case OB_CURVE:
-      case OB_SURF:
-        curve_surf_to_softbody(scene, ob);
-        break;
-      default:
-        renew_softbody(scene, ob, numVerts, 0);
-        break;
+    /* Do we need to initialize the ADMM-PD mesh?
+     * a) Has never been initialized.
+     * b) The mesh topology has changed. */
+    int init_mesh = 0;
+    if (is_first_frame || admmpd_mesh_needs_update(admmpd, ob)) {
+      init_mesh = admmpd_update_mesh(admmpd, ob, vertexCos);
+      if (init_mesh == 0) {
+        CLOG_ERROR(&LOG, "%s", admmpd->last_error);
+        WM_report(RPT_ERROR, admmpd->last_error);
+        return;
+      }
+      else if (init_mesh == -1) {
+        WM_report(RPT_WARNING, admmpd->last_error);
+      }
     }
 
-    softbody_update_positions(ob, sb, vertexCos, numVerts);
-    softbody_reset(ob, sb, vertexCos, numVerts);
+    /* Do we need to initialize the ADMM-PD solver?
+     * a) Has never been initialized
+     * b) Some settings require re-initialization
+     * c) The mesh has changed */
+    int init_solver = 0;
+    if (is_first_frame || init_mesh || admmpd_solver_needs_update(admmpd, scene, ob)) {
+      init_solver = admmpd_update_solver(admmpd, scene, ob, vertexCos);
+      if (init_solver == 0) {
+        CLOG_ERROR(&LOG, "%s", admmpd->last_error);
+        WM_report(RPT_ERROR, admmpd->last_error);
+        return;
+      }
+      else if (init_solver == -1) {
+        WM_report(RPT_WARNING, admmpd->last_error);
+      }
+    }
+
+    /* In case of paramter change, ob->soft->bpoint has not
+     * been set yet. So we need to resize which can be done
+     * in the copy_to_object function while leaving vertexCos to null. */
+    admmpd_copy_to_object(admmpd, ob, NULL);
+
+    if (init_mesh || init_solver) {
+      BKE_ptcache_invalidate(cache);
+    }
+  }
+  else if (sb->solver_mode == SOLVER_MODE_LEGACY) {
+    if (sb->bpoint == NULL ||
+        ((ob->softflag & OB_SB_EDGES) && !ob->soft->bspring && object_has_edges(ob))) {
+
+      switch (ob->type) {
+        case OB_MESH:
+          mesh_to_softbody(scene, ob);
+          break;
+        case OB_LATTICE:
+          lattice_to_softbody(scene, ob);
+          break;
+        case OB_CURVE:
+        case OB_SURF:
+          curve_surf_to_softbody(scene, ob);
+          break;
+        default:
+          renew_softbody(scene, ob, numVerts, 0);
+          break;
+      }
+
+      softbody_update_positions(ob, sb, vertexCos, numVerts);
+      softbody_reset(ob, sb, vertexCos, numVerts);
+    }
   }
 
   /* still no points? go away */
   if (sb->totpoint == 0) {
     return;
   }
+
   if (framenr == startframe) {
     BKE_ptcache_id_reset(scene, &pid, PTCACHE_RESET_OUTDATED);
 
     /* first frame, no simulation to do, just set the positions */
-    softbody_update_positions(ob, sb, vertexCos, numVerts);
+    if (sb->solver_mode == SOLVER_MODE_ADMMPD) {
+      admmpd_copy_from_object(admmpd, ob);
+    }
+    else if (sb->solver_mode == SOLVER_MODE_LEGACY) {
+      softbody_update_positions(ob, sb, vertexCos, numVerts);
+    }
 
     BKE_ptcache_validate(cache, framenr);
     cache->flag &= ~PTCACHE_REDO_NEEDED;
@@ -3620,17 +3898,25 @@ void sbObjectStep(struct Depsgraph *depsgraph,
 
   if (cache_result == PTCACHE_READ_EXACT || cache_result == PTCACHE_READ_INTERPOLATED ||
       (!can_simulate && cache_result == PTCACHE_READ_OLD)) {
-    softbody_to_object(ob, vertexCos, numVerts, sb->local);
+
+    if (sb->solver_mode == SOLVER_MODE_ADMMPD) {
+      /* We have read the cache into softbody, so we need to pass it to ADMM-PD */
+      admmpd_copy_from_object(admmpd, ob);
+      /* Now that we have the updated ADMM-PD vertices, we have
+       * to map them to surface vertices (vertexCos) */
+      admmpd_copy_to_object(admmpd, ob, vertexCos);
+    }
+    else if (sb->solver_mode == SOLVER_MODE_LEGACY) {
+      softbody_to_object(ob, vertexCos, numVerts, sb->local);
+    }
 
     BKE_ptcache_validate(cache, framenr);
-
     if (cache_result == PTCACHE_READ_INTERPOLATED && cache->flag & PTCACHE_REDO_NEEDED &&
         can_write_cache) {
       BKE_ptcache_write(&pid, framenr);
     }
 
     sbStoreLastFrame(depsgraph, ob, framenr);
-
     return;
   }
   if (cache_result == PTCACHE_READ_OLD) {
@@ -3656,18 +3942,29 @@ void sbObjectStep(struct Depsgraph *depsgraph,
     BKE_ptcache_write(&pid, startframe);
   }
 
-  softbody_update_positions(ob, sb, vertexCos, numVerts);
-
-  /* checking time: */
-  dtime = framedelta * timescale;
-
-  /* do simulation */
-  softbody_step(depsgraph, scene, ob, sb, dtime);
-
-  softbody_to_object(ob, vertexCos, numVerts, 0);
+  if (sb->solver_mode == SOLVER_MODE_ADMMPD) {
+    admmpd_copy_from_object(admmpd, ob);
+    update_collider_admmpd(depsgraph, sb->collision_group, ob);
+    int solve_retval = admmpd_solve(admmpd, ob, vertexCos);
+    if (solve_retval == 0) {
+      CLOG_ERROR(&LOG, "%s", admmpd->last_error);
+      WM_report(RPT_ERROR, admmpd->last_error);
+    }
+    else if (solve_retval == -1) {
+      WM_report(RPT_WARNING, admmpd->last_error);
+    }
+    admmpd_copy_to_object(admmpd, ob, vertexCos);
+  }
+  else if (sb->solver_mode == SOLVER_MODE_LEGACY) {
+    softbody_update_positions(ob, sb, vertexCos, numVerts);
+    /* checking time: */
+    dtime = framedelta * timescale;
+    /* do simulation */
+    softbody_step(depsgraph, scene, ob, sb, dtime);
+    softbody_to_object(ob, vertexCos, numVerts, 0);
+  }
 
   BKE_ptcache_validate(cache, framenr);
   BKE_ptcache_write(&pid, framenr);
-
   sbStoreLastFrame(depsgraph, ob, framenr);
 }

source/blender/blenloader/intern/readfile.c

@@ -155,6 +155,7 @@
 #include "BKE_sequencer.h"
 #include "BKE_shader_fx.h"
 #include "BKE_simulation.h"
+#include "BKE_softbody.h"
 #include "BKE_sound.h"
 #include "BKE_volume.h"
 #include "BKE_workspace.h"
@@ -4894,9 +4895,11 @@ static void direct_link_object(BlendDataReader *reader, Object *ob)
   if (ob->soft) {
     SoftBody *sb = ob->soft;
 
-    sb->bpoint = NULL;  // init pointers so it gets rebuilt nicely
+    /* init pointers so it gets rebuilt nicely */
+    sb->bpoint = NULL;
     sb->bspring = NULL;
     sb->scratch = NULL;
+
     /* although not used anymore */
     /* still have to be loaded to be compatible with old files */
     BLO_read_pointer_array(reader, (void **)&sb->keys);