diff --git a/CMakePresets.json b/CMakePresets.json index 29dd152ed01..270433b7103 100644 --- a/CMakePresets.json +++ b/CMakePresets.json @@ -167,6 +167,14 @@ "type": "BOOL", "value": "ON" }, + "SOFA_FETCH_SOFADISTANCEGRID": { + "type": "BOOL", + "value": "ON" + }, + "PLUGIN_SOFADISTANCEGRID": { + "type": "BOOL", + "value": "ON" + }, "PLUGIN_SOFADISTANCEGRID_CUDA": { "type": "BOOL", "value": "ON" @@ -248,10 +256,6 @@ "type": "BOOL", "value": "ON" }, - "PLUGIN_SOFADISTANCEGRID": { - "type": "BOOL", - "value": "ON" - }, "PLUGIN_SOFAEULERIANFLUID": { "type": "BOOL", "value": "ON" diff --git a/applications/plugins/CMakeLists.txt b/applications/plugins/CMakeLists.txt index 736fbf72423..17f28750931 100644 --- a/applications/plugins/CMakeLists.txt +++ b/applications/plugins/CMakeLists.txt @@ -72,5 +72,5 @@ else() message("Sofa.GL not found; disabling VolumetricRendering plugin") endif() -sofa_add_subdirectory(plugin SofaDistanceGrid SofaDistanceGrid) # Also defines SofaDistanceGrid.CUDA +sofa_add_external(plugin SofaDistanceGrid GIT_REF master GIT_REPOSITORY https://www.github.com/sofa-framework/SofaDistanceGrid.git) # Also defines SofaDistanceGrid.CUDA sofa_add_subdirectory(plugin SofaImplicitField SofaImplicitField) diff --git a/applications/plugins/SofaDistanceGrid/CMakeLists.txt b/applications/plugins/SofaDistanceGrid/CMakeLists.txt deleted file mode 100644 index 643ddab08b1..00000000000 --- a/applications/plugins/SofaDistanceGrid/CMakeLists.txt +++ /dev/null @@ -1,94 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(SofaDistanceGrid LANGUAGES CXX) - -option(SOFADISTANCEGRID_USE_MINIFLOWVR "Build and use the miniFlowVR library with DistanceGrid." OFF) -if(SOFADISTANCEGRID_USE_MINIFLOWVR) - add_subdirectory(extlibs/miniFlowVR) - set_target_properties(miniFlowVR PROPERTIES DEBUG_POSTFIX "_d") - #set_target_properties(miniFlowVR PROPERTIES FOLDER "SofaExtlibs") -endif() - -option(SOFADISTANCEGRID_USE_MESHCONV "Build meshconv, mainly used to generate DistanceGrid file. Needs miniFlowVR." OFF) -if(SOFADISTANCEGRID_USE_MESHCONV) - find_package(MiniFlowVR QUIET) - if(MiniFlowVR_FOUND) - sofa_add_subdirectory(application applications/meshconv meshconv OFF) - else() - message(WARNING "meshconv needs miniFlowVR; enable SOFADISTANCEGRID_USE_MINIFLOWVR to build it.") - endif() -endif() - -set(SOFADISTANCEGRID_SRC "src/${PROJECT_NAME}") - -set(HEADER_FILES - ${SOFADISTANCEGRID_SRC}/config.h.in - ${SOFADISTANCEGRID_SRC}/initSofaDistanceGrid.h - ${SOFADISTANCEGRID_SRC}/DistanceGrid.h - ${SOFADISTANCEGRID_SRC}/components/collision/FFDDistanceGridDiscreteIntersection.h - ${SOFADISTANCEGRID_SRC}/components/collision/FFDDistanceGridDiscreteIntersection.inl - ${SOFADISTANCEGRID_SRC}/components/collision/RigidDistanceGridDiscreteIntersection.h - ${SOFADISTANCEGRID_SRC}/components/collision/RigidDistanceGridDiscreteIntersection.inl - ${SOFADISTANCEGRID_SRC}/components/collision/DistanceGridCollisionModel.h - ${SOFADISTANCEGRID_SRC}/components/forcefield/DistanceGridForceField.h - ${SOFADISTANCEGRID_SRC}/components/forcefield/DistanceGridForceField.inl -) - -set(SOURCE_FILES - ${SOFADISTANCEGRID_SRC}/initSofaDistanceGrid.cpp - ${SOFADISTANCEGRID_SRC}/DistanceGrid.cpp - ${SOFADISTANCEGRID_SRC}/RegisterModelToCollisionFactory.cpp - ${SOFADISTANCEGRID_SRC}/components/forcefield/DistanceGridForceField.cpp - ${SOFADISTANCEGRID_SRC}/components/collision/FFDDistanceGridDiscreteIntersection.cpp - ${SOFADISTANCEGRID_SRC}/components/collision/RayDistanceGridContact.cpp - ${SOFADISTANCEGRID_SRC}/components/collision/RigidDistanceGridDiscreteIntersection.cpp - ${SOFADISTANCEGRID_SRC}/components/collision/DistanceGridCollisionModel.cpp - ${SOFADISTANCEGRID_SRC}/components/collision/BarycentricPenalityContact_DistanceGrid.cpp -) - -set(EXTRA_FILES - README.md - examples/README.md - examples/DistanceGridForceField_liver.scn - examples/FFDDistanceGridCollisionModel_liver_DefaultAnimationLoop.scn - examples/FFDDistanceGridCollisionModel_liver_FreeMotionAnimationLoop.scn - examples/RigidDistanceGridCollisionModel_liver_DefaultAnimationLoop.scn - examples/RigidDistanceGridCollisionModel_liver_FreeMotionAnimationLoop.scn - #examples/RigidDistanceGridCollisionModel_skull_FreeMotionAnimationLoop.scn - ) - - -find_package(MiniFlowVR QUIET) -sofa_find_package(Sofa.Core REQUIRED) -sofa_find_package(Sofa.Component.Collision REQUIRED) -sofa_find_package(Sofa.GL QUIET) - -add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${SOURCE_FILES} ${EXTRA_FILES}) -target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.Core) -target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.Component.Collision) - -if(Sofa.GL_FOUND) - target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.GL) -endif() - -if(MiniFlowVR_FOUND) - target_link_libraries(${PROJECT_NAME} PRIVATE miniFlowVR) # Private because not exported in API - message(STATUS "SofaDistanceGrid: MiniFlowVR found.") -else() - message(STATUS "SofaDistanceGrid: MiniFlowVR not found, some features will not be compiled.") -endif() - -if(SOFA_BUILD_TESTS) - add_subdirectory(SofaDistanceGrid_test) -endif() - -## Install rules for the library and headers; CMake package configurations files -sofa_create_package_with_targets( - PACKAGE_NAME ${PROJECT_NAME} - PACKAGE_VERSION ${Sofa_VERSION} - TARGETS ${PROJECT_NAME} AUTO_SET_TARGET_PROPERTIES - INCLUDE_SOURCE_DIR "src" - INCLUDE_INSTALL_DIR "${PROJECT_NAME}" - RELOCATABLE "plugins" - ) - -sofa_add_subdirectory(plugin extensions/CUDA SofaDistanceGrid.CUDA) diff --git a/applications/plugins/SofaDistanceGrid/README.md b/applications/plugins/SofaDistanceGrid/README.md deleted file mode 100644 index e289f6ec213..00000000000 --- a/applications/plugins/SofaDistanceGrid/README.md +++ /dev/null @@ -1,3 +0,0 @@ -= The SofaDistanceGrid plugin. - - diff --git a/applications/plugins/SofaDistanceGrid/SofaDistanceGridConfig.cmake.in b/applications/plugins/SofaDistanceGrid/SofaDistanceGridConfig.cmake.in deleted file mode 100644 index d2e411e2f59..00000000000 --- a/applications/plugins/SofaDistanceGrid/SofaDistanceGridConfig.cmake.in +++ /dev/null @@ -1,27 +0,0 @@ -# CMake package configuration file for the SofaDistanceGrid plugin - -@PACKAGE_GUARD@ -@PACKAGE_INIT@ - -find_package(Sofa.Core QUIET REQUIRED) -find_package(Sofa.Component.Collision QUIET REQUIRED) - -set(SOFADISTANCEGRID_HAVE_MINIFLOWVR @SOFADISTANCEGRID_HAVE_MINIFLOWVR@) -set(SOFADISTANCEGRID_HAVE_SOFA_GL @SOFADISTANCEGRID_HAVE_SOFA_GL@) - -if(SOFADISTANCEGRID_HAVE_MINIFLOWVR) - find_package(MiniFlowVR QUIET REQUIRED) -endif() - -if(SOFADISTANCEGRID_HAVE_SOFA_GL) - find_package(Sofa.GL QUIET REQUIRED) -endif() - -if(NOT TARGET SofaDistanceGrid) - include("${CMAKE_CURRENT_LIST_DIR}/SofaDistanceGridTargets.cmake") -endif() - -check_required_components(SofaDistanceGrid) -set(SofaDistanceGrid_LIBRARIES SOFADISTANCEGRID) -set(SofaDistanceGrid_INCLUDE_DIRS @PACKAGE_SOFADISTANCEGRID_INCLUDE_DIR@ ${SOFADISTANCEGRID_INCLUDE_DIR}) - diff --git a/applications/plugins/SofaDistanceGrid/SofaDistanceGrid_test/CMakeLists.txt b/applications/plugins/SofaDistanceGrid/SofaDistanceGrid_test/CMakeLists.txt deleted file mode 100644 index 5fc39e20534..00000000000 --- a/applications/plugins/SofaDistanceGrid/SofaDistanceGrid_test/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -cmake_minimum_required(VERSION 3.22) - -project(SofaDistanceGrid_test) - -set(SOURCE_FILES - DistanceGrid_test.cpp -) - -add_executable(${PROJECT_NAME} ${SOURCE_FILES}) -target_link_libraries(${PROJECT_NAME} Sofa.Testing SofaDistanceGrid) - -add_test(NAME ${PROJECT_NAME} COMMAND ${PROJECT_NAME}) diff --git a/applications/plugins/SofaDistanceGrid/SofaDistanceGrid_test/DistanceGrid_test.cpp b/applications/plugins/SofaDistanceGrid/SofaDistanceGrid_test/DistanceGrid_test.cpp deleted file mode 100644 index de3d9ebe118..00000000000 --- a/applications/plugins/SofaDistanceGrid/SofaDistanceGrid_test/DistanceGrid_test.cpp +++ /dev/null @@ -1,102 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#include -#include - -#include -using sofa::component::container::DistanceGrid ; - -namespace sofa -{ -namespace component -{ -namespace container -{ -namespace _distancegrid_ -{ -using sofa::type::Vec3 ; - -struct DistanceGrid_test : public sofa::testing::NumericTest -{ - void chekcValidConstructorsCube(){ - EXPECT_MSG_NOEMIT(Warning, Error) ; - - DistanceGrid grid(10, 10, 10, - DistanceGrid::Coord(-1,-1,-1), - DistanceGrid::Coord(1.0,1.0,1.0)) ; - - EXPECT_EQ(grid.getNx(), 10) ; - EXPECT_EQ(grid.getNy(), 10) ; - EXPECT_EQ(grid.getNz(), 10) ; - - EXPECT_FALSE(grid.inBBox(type::Vec3(-2, 0, 0), 0.0f)) ; - EXPECT_FALSE(grid.inBBox(type::Vec3( 0,-2, 0), 0.0f)) ; - EXPECT_FALSE(grid.inBBox(type::Vec3( 0, 0,-2), 0.0f)) ; - EXPECT_FALSE(grid.inBBox(type::Vec3( 2, 0, 0), 0.0f)) ; - EXPECT_FALSE(grid.inBBox(type::Vec3( 0, 2, 0), 0.0f)) ; - EXPECT_FALSE(grid.inBBox(type::Vec3( 0, 0, 2), 0.0f)) ; - - EXPECT_EQ(grid.size(), 10*10*10); - - //todo(dmarchal:2017-05-02) This "isCube" & "getCubeDim" stuff is ugly as hell ! - EXPECT_FALSE(grid.isCube()); - } - - void checInvalidConstructorsCube(int x, int y, int z, - float mx, float my, float mz, - float ex, float ey, float ez){ - std::cout << "x-y-z:" << x << ", " << y << ", " << z << std::endl ; - std::cout << "mx-my-mz:" << mx << ", " << my << ", " << mz << std::endl ; - std::cout << "ex-ey-ez:" << ex << ", " << ey << ", " << ez << std::endl ; - - DistanceGrid grid(x, y, z, - DistanceGrid::Coord(mx,my,mz), - DistanceGrid::Coord(ex,ey,ez)) ; - } -}; - -TEST_F(DistanceGrid_test, chekcValidConstructorsCube) { - ASSERT_NO_THROW(this->chekcValidConstructorsCube()) ; -} - -TEST_F(DistanceGrid_test, chekcInvalidConstructorsCube) { - std::vector< std::vector< float >> values = { - {-10, 10, 10, -1,-1,-1, 1, 1,1}, - { 10,-10, 10, -1,-1,-1, 1, 1,1}, - { 10, 10,-10, -1,-1,-1, 1, 1,1}, - { 10, 10, 0, -1,-1,-1, 1, 1,1}, - { 10, 0, 10, -1,-1,-1, 1, 1,1}, - { 0, 10, 0, -1,-1,-1, 1, 1,1}, - { 0, 10, 0, -1, 1,-1, 1,-1,1} }; - for(auto& v : values ){ - ASSERT_NO_THROW( this->checInvalidConstructorsCube( - (int)v[0], (int)v[1], (int)v[2], - v[3], v[4], v[5], - v[6], v[7], v[8])) ; - } -} - - -} // __distance_grid__ -} // container -} // component -} // sofa diff --git a/applications/plugins/SofaDistanceGrid/applications/meshconv/CMakeLists.txt b/applications/plugins/SofaDistanceGrid/applications/meshconv/CMakeLists.txt deleted file mode 100644 index 7b73ed3a729..00000000000 --- a/applications/plugins/SofaDistanceGrid/applications/meshconv/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(meshconv) - -find_package(MiniFlowVR REQUIRED) - -add_executable(${PROJECT_NAME} meshconv.cpp tesselate.cpp) - -target_link_libraries(${PROJECT_NAME} miniFlowVR) - diff --git a/applications/plugins/SofaDistanceGrid/applications/meshconv/meshconv.cpp b/applications/plugins/SofaDistanceGrid/applications/meshconv/meshconv.cpp deleted file mode 100644 index 3d5c6b76a8e..00000000000 --- a/applications/plugins/SofaDistanceGrid/applications/meshconv/meshconv.cpp +++ /dev/null @@ -1,293 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* 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, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -/******* COPYRIGHT ************************************************ -* * -* FlowVR Render * -* Parallel Rendering Modules * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 2005 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU GPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: ./src/utils/meshconv.cpp * -* * -* Contacts: * -* * -******************************************************************/ -#include -#include -#include - -#include -#include - -using namespace flowvr::render; - -extern void tesselateMesh(Mesh& obj, int rec=1, bool onSphere=false); - -int main(int argc, char** argv) -{ - bool normalize = false; - bool flip = false; - bool wnormals = false; - bool rnormals = false; - bool rtexcoords = false; - bool closemesh = false; - float closedist = 0.0f; - float mergedist = -1.0f; - Vec3f translation; - Vec3f rotation; - Vec3f scale(1,1,1); - float dilate = 0.0f; - int tesselate = 0; - bool sphere = false; - bool dist = false; - int res = 16; - int rx=0,ry=0,rz=0; - float border = 0.25f; - float vsize = 0.0f; - ftl::CmdLine cmd("Usage: meshconv [options] mesh.input [mesh.output]"); - cmd.opt("normalize",'n',"transform points so that the center is at <0,0,0> and the max coodinate is 1",&normalize); - cmd.opt("flip",'f',"flip normals",&flip); - cmd.opt("wnormals",'N',"force writing of normals",&wnormals); - cmd.opt("rnormals",'O',"remove normals",&rnormals); - cmd.opt("rtexcoords",'T',"remove texcoords",&rtexcoords); - cmd.opt("close",'c',"close mesh",&closemesh); - cmd.opt("close2",'C',"close mesh creating intermediate vertices no further appart than given dist",&closedist); - cmd.opt("merge",'m',"merge vertices closer than the given distance",&mergedist); - cmd.opt("translate",'t',"translate the mesh",&translation); - cmd.opt("rotate",'r',"rotate the mesh using euler angles in degree",&rotation); - cmd.opt("scale",'s',"scale the mesh using 3 coefficients",&scale); - cmd.opt("dilate",'d',"dilate (i.e. displace vertices of the given distance along their normals)",&dilate); - cmd.opt("tesselate",'a',"tesselate (split each edge in 2 resursivly n times)",&tesselate); - cmd.opt("sphere",'S',"consider the mesh as a sphere for tesselation",&sphere); - cmd.opt("dist",'D',"compute distance field",&dist); - cmd.opt("res",'R',"resolution of distance field",&res); - cmd.opt("rx",'X',"X resolution of distance field",&rx); - cmd.opt("ry",'Y',"Y resolution of distance field",&ry); - cmd.opt("rz",'Z',"Z resolution of distance field",&rz); - cmd.opt("vsize",'V',"size of each voxel in distance field",&vsize); - cmd.opt("border",'B',"distance field border size relative to the object's BBox size (or negative for exact size)",&border); - bool error=false; - if (!cmd.parse(argc,argv,&error)) - return error?1:0; - - if (cmd.args.size()<1 || cmd.args.size()>2) - { - std::cerr << cmd.help() << std::endl; - return 1; - } - - std::string file_in = cmd.args[0]; - std::string file_out; - if (cmd.args.size()>=2) file_out = cmd.args[1]; - - Mesh obj; - - if (!obj.load(file_in.c_str())) - { - std::cerr << "Failed to read "<size) size=d; - } - Vec3f center = (bb.a+bb.b)*0.5; - float sc = 2/size; - Mat4x4f m; m.identity(); - m(0,0)=sc; m(0,3) = -center[0]*sc; - m(1,1)=sc; m(1,3) = -center[1]*sc; - m(2,2)=sc; m(2,3) = -center[2]*sc; - for (int i=0; imat<<" = "; - obj.distmap->mat = xform * obj.distmap->mat; - std::cout << obj.distmap->mat<= 0.0f) - { - std::cout << "Merging vertices closer than " << mergedist < 0 || sphere) - { - std::cout << "Tesselating mesh..."< 0) - { - rx = (int)ceilf(bbsize[0]/vsize); - ry = (int)ceilf(bbsize[1]/vsize); - rz = (int)ceilf(bbsize[2]/vsize); - } - std::cout << "Computing "<. * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#include -#include -#include - -#include - -using namespace flowvr::render; -using ftl::Vec3f; - -void projectOnSphere(Vec3f& p, Vec3f center, float radius) -{ - p -= center; - p = p * (radius / p.norm()); - p += center; -} - -void projectOnSphere(Vec3f& p,Vec3f& n, Vec3f center, float radius) -{ - p -= center; - n = p; - n.normalize(); - p = p * (radius / p.norm()); - p += center; -} - -void tesselateMesh(Mesh& obj, int rec=1, bool onSphere=false) -{ - BBox bb = obj.calcBBox(); - std::cout << "Mesh bbox="<::iterator it = obj.edges[e1].begin(), itend = obj.edges[e1].end(); it != itend; ++it) - { - int g2 = obj.getPG(it->first); - - int f1p1 = -1, f1p2 = -1; - int f2p1 = -1, f2p2 = -1; - int i1 = -1; - if (it->second.f1 >= 0) - { - Vec3i fp = obj.getFP(it->second.f1); - f1p1 = fp[0], f1p2 = fp[1]; - if (obj.getPG(f1p1) != g1) - { - f1p1 = fp[1]; f1p2 = fp[2]; - if (obj.getPG(f1p1) != g1) - { - f1p1 = fp[2]; f1p2 = fp[0]; // this is the last possible edge - } - } - if (obj.getPG(f1p1) != g1 || (obj.getPG(f1p2) != g2)) - { - std::cerr << "ERROR: Edge "<second.f1<<" = "<second.f1 = -1; - continue; - } - Mesh::Vertex v; - v = obj.getP(f1p1); - v += obj.getP(f1p2); - v.mean(2); - //if (onSphere) v.p = projectOnSphere(v.p, center, radius[0]); - i1 = obj.addP(v); - // replace the face index with the new point index - it->second.f1 = i1; - } - - if (it->second.f2 >= 0) - { - Vec3i fp = obj.getFP(it->second.f2); - f2p1 = fp[0], f2p2 = fp[1]; - if (obj.getPG(f2p1) != g2) - { - f2p1 = fp[1]; f2p2 = fp[2]; - if (obj.getPG(f2p1) != g2) - { - f2p1 = fp[2]; f2p2 = fp[0]; // this is the last possible edge - } - } - if (obj.getPG(f2p1) != g2 || (obj.getPG(f2p2) != g1)) - { - std::cerr << "ERROR: Edge "<second.f2<<" = "<second.f2 = -1; - continue; - } - if (f1p1 == f2p2 && f1p2 == f2p1) - { - // same point as other face - it->second.f2 = i1; - } - else - { - Mesh::Vertex v; - v = obj.getP(f2p1); - v += obj.getP(f2p2); - v.mean(2); - //if (onSphere) v.p = projectOnSphere(v.p, center, radius[0]); - int i2; - if (i1 == -1) // no other edge, create a new group - i2 = obj.addP(v); - else - { - // see of all points are on the same subgroup - int gf1p1 = g1; - while (obj.getGP0(gf1p1+1) <= -f1p1) ++gf1p1; - int gf1p2 = g2; - while (obj.getGP0(gf1p2+1) <= -f1p2) ++gf1p2; - int gf2p1 = g2; - while (obj.getGP0(gf2p1+1) <= -f2p1) ++gf2p1; - int gf2p2 = g1; - while (obj.getGP0(gf2p2+1) <= -f2p2) ++gf2p2; - int g = obj.getPG(i1); - i2 = obj.addP(v,g); - if (gf1p1 != gf2p2 || gf1p2 != gf2p1) - { - // create a subgroup - obj.GP0(obj.nbg()) = -i2; - } - } - // replace the face index with the new point index - it->second.f2 = i2; - } - } - } - } - - // then create new faces - std::cout << "Creating new faces..."< faces_p; - int nbf0 = obj.nbf(); - faces_p.reserve(nbf0*4); - for(int i=0; i= (unsigned)obj.nbp() || (unsigned)edges[1] >= (unsigned)obj.nbp() || (unsigned)edges[2] >= (unsigned)obj.nbp()) - { - std::cerr << "ERROR: invalid edge points " << edges << " in face " < - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/applications/plugins/SofaDistanceGrid/examples/FFDDistanceGridCollisionModel_liver_DefaultAnimationLoop.scn b/applications/plugins/SofaDistanceGrid/examples/FFDDistanceGridCollisionModel_liver_DefaultAnimationLoop.scn deleted file mode 100644 index 7fa3491bd6c..00000000000 --- a/applications/plugins/SofaDistanceGrid/examples/FFDDistanceGridCollisionModel_liver_DefaultAnimationLoop.scn +++ /dev/null @@ -1,92 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/applications/plugins/SofaDistanceGrid/examples/FFDDistanceGridCollisionModel_liver_FreeMotionAnimationLoop.scn b/applications/plugins/SofaDistanceGrid/examples/FFDDistanceGridCollisionModel_liver_FreeMotionAnimationLoop.scn deleted file mode 100644 index 55564bf78c7..00000000000 --- a/applications/plugins/SofaDistanceGrid/examples/FFDDistanceGridCollisionModel_liver_FreeMotionAnimationLoop.scn +++ /dev/null @@ -1,97 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/applications/plugins/SofaDistanceGrid/examples/README.md b/applications/plugins/SofaDistanceGrid/examples/README.md deleted file mode 100644 index f55d3e021dc..00000000000 --- a/applications/plugins/SofaDistanceGrid/examples/README.md +++ /dev/null @@ -1,2 +0,0 @@ -= Examples -- ... diff --git a/applications/plugins/SofaDistanceGrid/examples/RigidDistanceGridCollisionModel_liver_DefaultAnimationLoop.scn b/applications/plugins/SofaDistanceGrid/examples/RigidDistanceGridCollisionModel_liver_DefaultAnimationLoop.scn deleted file mode 100644 index 345637a7303..00000000000 --- a/applications/plugins/SofaDistanceGrid/examples/RigidDistanceGridCollisionModel_liver_DefaultAnimationLoop.scn +++ /dev/null @@ -1,80 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/applications/plugins/SofaDistanceGrid/examples/RigidDistanceGridCollisionModel_liver_FreeMotionAnimationLoop.scn b/applications/plugins/SofaDistanceGrid/examples/RigidDistanceGridCollisionModel_liver_FreeMotionAnimationLoop.scn deleted file mode 100644 index ea0d05a2e8b..00000000000 --- a/applications/plugins/SofaDistanceGrid/examples/RigidDistanceGridCollisionModel_liver_FreeMotionAnimationLoop.scn +++ /dev/null @@ -1,83 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/applications/plugins/SofaDistanceGrid/extensions/CUDA/CMakeLists.txt b/applications/plugins/SofaDistanceGrid/extensions/CUDA/CMakeLists.txt deleted file mode 100644 index 2cc359986e0..00000000000 --- a/applications/plugins/SofaDistanceGrid/extensions/CUDA/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(SofaDistanceGrid.CUDA LANGUAGES CUDA CXX) - -set(HEADER_FILES - src/SofaDistanceGrid/CUDA/init.h - src/SofaDistanceGrid/CUDA/config.h.in - - src/SofaDistanceGrid/CUDA/CudaDistanceGridCollisionModel.h - src/SofaDistanceGrid/CUDA/CudaDistanceGridContactMapper.h - src/SofaDistanceGrid/CUDA/CudaCollisionDetection.h -) - -set(SOURCE_FILES - src/SofaDistanceGrid/CUDA/init.cpp - - src/SofaDistanceGrid/CUDA/CudaDistanceGridCollisionModel.cpp - src/SofaDistanceGrid/CUDA/CudaCollisionDistanceGrid.cpp - src/SofaDistanceGrid/CUDA/CudaCollisionDetection.cpp -) - -set(CUDA_SOURCES - src/SofaDistanceGrid/CUDA/CudaCollisionDetection.cu - src/SofaDistanceGrid/CUDA/CudaDistanceGridContactMapper.cu -) - -sofa_find_package(SofaDistanceGrid REQUIRED) -sofa_find_package(SofaCUDA REQUIRED) - -add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${SOURCE_FILES} ${CUDA_SOURCES}) - -target_link_libraries(${PROJECT_NAME} SofaDistanceGrid) -target_link_libraries(${PROJECT_NAME} SofaCUDA) -get_property(SOFACUDA_CUDA_ARCHITECTURES TARGET SofaCUDA PROPERTY CUDA_ARCHITECTURES) -set_target_properties(${PROJECT_NAME} PROPERTIES CUDA_ARCHITECTURES "${SOFACUDA_CUDA_ARCHITECTURES}" ) - -sofa_create_package_with_targets( - PACKAGE_NAME ${PROJECT_NAME} - PACKAGE_VERSION ${Sofa_VERSION} - TARGETS ${PROJECT_NAME} AUTO_SET_TARGET_PROPERTIES - INCLUDE_SOURCE_DIR "src" - INCLUDE_INSTALL_DIR "${PROJECT_NAME}" - RELOCATABLE "plugins" -) diff --git a/applications/plugins/SofaDistanceGrid/extensions/CUDA/SofaDistanceGrid.CUDAConfig.cmake.in b/applications/plugins/SofaDistanceGrid/extensions/CUDA/SofaDistanceGrid.CUDAConfig.cmake.in deleted file mode 100644 index 62faad7f3c6..00000000000 --- a/applications/plugins/SofaDistanceGrid/extensions/CUDA/SofaDistanceGrid.CUDAConfig.cmake.in +++ /dev/null @@ -1,9 +0,0 @@ -# CMake package configuration file for the VolumetricRendering.CUDA library - -@PACKAGE_GUARD@ -@PACKAGE_INIT@ - -find_package(SofaDistanceGrid QUIET REQUIRED) -find_package(SofaCUDA QUIET REQUIRED) - -check_required_components(SofaDistanceGrid.CUDA) diff --git a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaCollisionDetection.cpp b/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaCollisionDetection.cpp deleted file mode 100644 index 88718d82630..00000000000 --- a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaCollisionDetection.cpp +++ /dev/null @@ -1,381 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#include "CudaCollisionDetection.h" -#include - -namespace sofa -{ - -namespace gpu -{ - -namespace cuda -{ - -using namespace sofa::core::collision; -using namespace sofa::component::collision::geometry; - -extern "C" -{ - void CudaCollisionDetection_runTests(unsigned int nbTests, unsigned int maxPoints, const void* tests, void* nresults); -} - -void registerCudaCollisionDetection(sofa::core::ObjectFactory* factory) -{ - factory->registerObjects(sofa::core::ObjectRegistrationData("GPU-based collision detection using CUDA.") - .add< CudaCollisionDetection >()); -} - -void CudaCollisionDetection::beginNarrowPhase() -{ - Inherit2::beginNarrowPhase(); - for (TestMap::iterator it = tests.begin(), itend = tests.end(); it != itend; ++it) - if (it->second.index >= 0) - it->second.index = -1; - else - --it->second.index; -} - -void CudaCollisionDetection::addCollisionPair( const std::pair& cmPair ) -{ - core::CollisionModel *finalcm1 = cmPair.first->getLast(); - core::CollisionModel *finalcm2 = cmPair.second->getLast(); - - Entry& entry = tests[std::make_pair(finalcm1, finalcm2)]; - if (entry.test == NULL) - { - entry.test = createTest(finalcm1, finalcm2); - } - entry.index = 0; // active pair -} - -void CudaCollisionDetection::endNarrowPhase() -{ - // first clean up old pairs - for (TestMap::iterator it = tests.begin(), itend = tests.end(); it != itend;) - if (it->second.index < -100) - { - TestMap::iterator it2 = it; - ++it; - tests.erase(it2); - } - else ++it; - - // compute number of tests - int ntests = 0; - for (TestMap::iterator it = tests.begin(), itend = tests.end(); it != itend; ++it) - if (it->second.index >= 0) - { - Test* test = it->second.test; - if (test->useGPU()) - { - it->second.index = ntests; - ntests += test->init(); - } - } - if (ntests > 0) - { - // resize vectors - gputests.fastResize(ntests); - gpuresults.fastResize(ntests); - - // init tests - for (TestMap::iterator it = tests.begin(), itend = tests.end(); it != itend; ++it) - if (it->second.index >= 0) - { - Test* test = it->second.test; - if (test->useGPU()) - { - test->fillInfo(&gputests[it->second.index]); - } - } - - // Launch GPU test - int maxPoints = 0; - for (int t=0; t maxPoints) - maxPoints = gputests[t].nbPoints; - CudaCollisionDetection_runTests(ntests, maxPoints, gputests.deviceRead(), gpuresults.deviceWrite()); - - } - - // compute CPU-side collisions - for (TestMap::iterator it = tests.begin(), itend = tests.end(); it != itend; ++it) - if (it->second.index >= 0) - { - Test* test = it->second.test; - if (!test->useGPU()) - { - core::CollisionModel *cm1 = it->first.first->getFirst(); //->getNext(); - core::CollisionModel *cm2 = it->first.second->getFirst(); //->getNext(); - addCollisionPair( std::make_pair(cm1, cm2) ); - } - } - - if (ntests > 0) - { - // gather gpu results - const int* results = (const int*)gpuresults.hostRead(); - int total = 0; - for (int i=0; isecond.index >= 0) - { - Test* test = it->second.test; - if (test->useGPU()) - { - GPUOutputVector* tresults = &it->second.test->results; - this->getDetectionOutputs(it->first.first, it->first.second) = tresults; - int newIndex = 0; - for (unsigned int t=0; tnbTests(); t++) - { - tresults->wtest(t).curSize = results[it->second.index + t]; - tresults->wtest(t).newIndex = newIndex; - newIndex += tresults->rtest(t).curSize; - } - //test->fillContacts(this->outputsMap[it->first], results+it->second.index); - } - } - } - - Inherit2::endNarrowPhase(); -} - - -CudaCollisionDetection::Test* CudaCollisionDetection::createTest(core::CollisionModel* model1, core::CollisionModel* model2) -{ - if (CudaRigidDistanceGridCollisionModel* rigid1 = dynamic_cast(model1)) - { - if (CudaRigidDistanceGridCollisionModel* rigid2 = dynamic_cast(model2)) - return new RigidRigidTest(rigid1, rigid2); - else if (CudaSphereCollisionModel* sphere2 = dynamic_cast(model2)) - return new SphereRigidTest(sphere2, rigid1); - else if (CudaPointCollisionModel* point2 = dynamic_cast(model2)) - return new PointRigidTest(point2, rigid1); - } - else if (CudaSphereCollisionModel* sphere1 = dynamic_cast(model1)) - { - if (CudaRigidDistanceGridCollisionModel* rigid2 = dynamic_cast(model2)) - return new SphereRigidTest(sphere1, rigid2); - } - else if (CudaPointCollisionModel* point1 = dynamic_cast(model1)) - { - if (CudaRigidDistanceGridCollisionModel* rigid2 = dynamic_cast(model2)) - return new PointRigidTest(point1, rigid2); - } - std::cout << "CudaCollisionDetection::CPUTest "<getClassName()<<" - "<getClassName()<getClassName()<<" - "<getClassName()<isActive() || !model2->isActive()) return 0; - bool useP1 = model1->usePoints.getValue(); - bool useP2 = model2->usePoints.getValue(); - if (!useP1 && !useP2) return 0; - int i0 = model1->getSize(); - for (CudaRigidDistanceGridCollisionElement e1 = CudaRigidDistanceGridCollisionElement(model1->begin()); e1!=model1->end(); ++e1) - for (CudaRigidDistanceGridCollisionElement e2 = CudaRigidDistanceGridCollisionElement(model2->begin()); e2!=model2->end(); ++e2) - { - CudaDistanceGrid* g1 = e1.getGrid(); - CudaDistanceGrid* g2 = e2.getGrid(); - if (g1 && g2) - { - if (useP1 && !g1->meshPts.empty()) results.addTest(std::make_pair(e1.getIndex(), i0+e2.getIndex()), g1->meshPts.size()); - if (useP2 && !g2->meshPts.empty()) results.addTest(std::make_pair(i0+e2.getIndex(), e1.getIndex()), g2->meshPts.size()); - } - } - return results.nbTests(); -} - -/// Fill the info to send to the graphics card -void CudaCollisionDetection::RigidRigidTest::fillInfo(GPUTest* tests) -{ - if (results.nbTests()==0) return; - GPUContact* gresults = (GPUContact*)results.results.deviceWrite(); - GPUContactPoint* gresults1 = (GPUContactPoint*)results.results1.deviceWrite(); - GPUContactPoint* gresults2 = (GPUContactPoint*)results.results2.deviceWrite(); - int i0 = model1->getSize(); - for (unsigned int i=0; i& p1 = elem1.getGrid()->meshPts; - CudaDistanceGrid& g2 = *elem2.getGrid(); - test.nbPoints = p1.size(); - test.result = gresults + e.firstIndex; - test.result1 = gresults1 + e.firstIndex; - test.result2 = gresults2 + e.firstIndex; - test.points = p1.deviceRead(); - test.radius = NULL; - test.gridnx = g2.getNx(); - test.gridny = g2.getNy(); - test.gridnz = g2.getNz(); - test.gridbbmin = g2.getBBMin(); - test.gridbbmax = g2.getBBMax(); - test.gridp0 = g2.getPMin(); - test.gridinvdp = g2.getInvCellWidth(); - test.grid = g2.getDists().deviceRead(); - test.margin = 0; - test.rotation = elem2.getRotation().multTranspose(elem1.getRotation()); - test.translation = elem2.getRotation().multTranspose(elem1.getTranslation()-elem2.getTranslation()); - } -} - -CudaCollisionDetection::SphereRigidTest::SphereRigidTest(CudaSphereCollisionModel*model1, CudaRigidDistanceGridCollisionModel* model2 ) - : model1(model1), model2(model2) -{ - std::cout << "CudaCollisionDetection::SphereRigidTest "<getClassName()<<" - "<getClassName()<isActive() || !model2->isActive()) return 0; - const CudaVector& p1 = model1->getMechanicalState()->read(sofa::core::vec_id::read_access::position)->getValue(); - if (p1.empty()) return 0; - - for (CudaRigidDistanceGridCollisionElement e2 = CudaRigidDistanceGridCollisionElement(model2->begin()); e2!=model2->end(); ++e2) - { - CudaDistanceGrid* g2 = e2.getGrid(); - if (g2) - results.addTest(std::make_pair(0, e2.getIndex()), p1.size()); - } - return results.nbTests(); -} - -/// Fill the info to send to the graphics card -void CudaCollisionDetection::SphereRigidTest::fillInfo(GPUTest* tests) -{ - - if (results.nbTests()==0) return; - GPUContact* gresults = (GPUContact*)results.results.deviceWrite(); - GPUContactPoint* gresults1 = (GPUContactPoint*)results.results1.deviceWrite(); - GPUContactPoint* gresults2 = (GPUContactPoint*)results.results2.deviceWrite(); - const CudaVector& p1 = model1->getMechanicalState()->read(sofa::core::vec_id::read_access::position)->getValue(); - - for (unsigned int i=0; igetR().deviceRead(); - test.grid = g2->getDists().deviceRead(); - test.gridnx = g2->getNx(); - test.gridny = g2->getNy(); - test.gridnz = g2->getNz(); - test.gridbbmin = g2->getBBMin(); - test.gridbbmax = g2->getBBMax(); - test.gridp0 = g2->getPMin(); - test.gridinvdp = g2->getInvCellWidth(); - test.margin = 0; //model1->getRadius(0); - test.rotation.transpose(Mat3x3f(elem2.getRotation())); - test.translation = test.rotation*(-elem2.getTranslation()); - - } - -} - -CudaCollisionDetection::PointRigidTest::PointRigidTest( CudaPointCollisionModel* model1, CudaRigidDistanceGridCollisionModel* model2 ) - : model1(model1), model2(model2) -{ - std::cout << "CudaCollisionDetection::PointRigidTest "<getClassName()<<" - "<getClassName()<isActive() || !model2->isActive()) return 0; - for (CudaPoint e1 = CudaPoint(model1->begin()); e1!=model1->end(); ++e1) - for (CudaRigidDistanceGridCollisionElement e2 = CudaRigidDistanceGridCollisionElement(model2->begin()); e2!=model2->end(); ++e2) - { - CudaDistanceGrid* g2 = e2.getGrid(); - if (g2) - results.addTest(std::make_pair(e1.getIndex(), e2.getIndex()), e1.getSize()); - } - return results.nbTests(); -} - -/// Fill the info to send to the graphics card -void CudaCollisionDetection::PointRigidTest::fillInfo(GPUTest* tests) -{ - if (results.nbTests()==0) return; - GPUContact* gresults = (GPUContact*)results.results.deviceWrite(); - GPUContactPoint* gresults1 = (GPUContactPoint*)results.results1.deviceWrite(); - GPUContactPoint* gresults2 = (GPUContactPoint*)results.results2.deviceWrite(); - for (unsigned int i=0; i& p1 = model1->getMechanicalState()->read(sofa::core::vec_id::read_access::position)->getValue(); - CudaDistanceGrid& g2 = *elem2.getGrid(); - test.nbPoints = elem1.getSize(); - test.result = gresults + e.firstIndex; - test.result1 = gresults1 + e.firstIndex; - test.result2 = gresults2 + e.firstIndex; - test.points = p1.deviceReadAt(elem1.i0()); - test.radius = NULL; - test.gridnx = g2.getNx(); - test.gridny = g2.getNy(); - test.gridnz = g2.getNz(); - test.gridbbmin = g2.getBBMin(); - test.gridbbmax = g2.getBBMax(); - test.gridp0 = g2.getPMin(); - test.gridinvdp = g2.getInvCellWidth(); - test.grid = g2.getDists().deviceRead(); - test.margin = 0; - test.rotation.transpose(elem2.getRotation()); - test.translation = elem2.getRotation().multTranspose(-elem2.getTranslation()); - } -} - -} // namespace cuda - -} // namespace gpu - -} // namespace sofa diff --git a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaCollisionDetection.cu b/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaCollisionDetection.cu deleted file mode 100644 index e14a17af8b7..00000000000 --- a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaCollisionDetection.cu +++ /dev/null @@ -1,235 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#include -#include -#include -#include "cuda.h" - -extern "C" -{ - void CudaCollisionDetection_runTests(unsigned int nbTests, unsigned int maxPoints, const void* tests, void* nresults); -} - -struct /*__align__(16)*/ GPUContactPoint -{ - CudaVec3 p; - int elem; -}; - -struct /*__align__(16)*/ GPUContact -{ -// int p1; -// CudaVec3 p2; - float distance; - CudaVec3 normal; -}; - -struct GPUTest -{ - GPUContact* result; - GPUContactPoint* result1; - GPUContactPoint* result2; - const CudaVec3* points; - const float* radius; - const float* grid; - //matrix3 rotation; - CudaVec3 rotation_x,rotation_y,rotation_z; - CudaVec3 translation; - float margin; - int nbPoints; - int gridnx, gridny, gridnz; - CudaVec3 gridbbmin, gridbbmax; - CudaVec3 gridp0, gridinvdp; -}; - -struct GPUDeformedCube -{ - int elem; - int ix,iy,iz; - int points0, nbp; - CudaVec3 initP0, invDP; -}; - -struct GPUDeformedCubeState -{ - CudaVec4 faces[6]; - CudaVec3 C0, Dx, Dy, Dz, Dxy, Dxz, Dyz, Dxyz; - CudaVec3 center, radius; -}; - -struct GPUDeformedCubeBSphere -{ - CudaVec3 center; - float radius; -}; - -struct GPUTestFFD -{ - GPUContact* result; - GPUContactPoint* result1; - GPUContactPoint* result2; - const CudaVec3* points; - const float* radius; - const float* grid; - const GPUDeformedCube* ffdCubes; - GPUDeformedCubeState* ffdState; - GPUDeformedCubeBSphere* ffdBSphere; - float margin; - int nbPoints; - int nbCubes; - int gridnx, gridny, gridnz; - CudaVec3 gridbbmin, gridbbmax; - CudaVec3 gridp0, gridinvdp; -}; - -////////////////////// -// GPU-side methods // -////////////////////// - -__shared__ GPUTest curTest; - -__global__ void CudaCollisionDetection_runTests_kernel(const GPUTest* tests, int* nresults) -{ - if (threadIdx.x == 0) - curTest = tests[blockIdx.x]; - - - __syncthreads(); - - //! Dynamically allocated shared memory to compact results - extern __shared__ int scan[]; - - CudaVec3 p0,p; - float distance; - CudaVec3 grad = CudaVec3::make(0,0,0); - //CudaVec3 normal; - int n = 0; - if (threadIdx.x < curTest.nbPoints) - { - p0 = curTest.points[threadIdx.x]; - //p = curTest.rotation * p; - p = CudaVec3::make(dot(curTest.rotation_x, p0), dot(curTest.rotation_y, p0), dot(curTest.rotation_z, p0)); - p += curTest.translation; - - CudaVec3 coefs = mul(p-curTest.gridp0, curTest.gridinvdp); - int x = __float2int_rd(coefs.x); - int y = __float2int_rd(coefs.y); - int z = __float2int_rd(coefs.z); - if ((unsigned)x < curTest.gridnx-1 - && (unsigned)y < curTest.gridny-1 - && (unsigned)z < curTest.gridnz-1) - { - int nx = curTest.gridnx; - int nxny = nx*curTest.gridny; - coefs.x -= __int2float_rd(x); - coefs.y -= __int2float_rd(y); - coefs.z -= __int2float_rd(z); - const float* gval = curTest.grid + (x+nx*y+nxny*z); - float d000 = gval[0 ]; - float d100_d000 = gval[1 ] - d000; - float d010 = gval[ nx ]; - float d110_d010 = gval[1+nx ] - d010; - float d001 = gval[ nxny]; - float d101_d001 = gval[1 +nxny] - d001; - float d011 = gval[ nx+nxny]; - float d111_d011 = gval[1+nx+nxny] - d011; - float dx00 = d000 + (d100_d000)*coefs.x; - float dx10_dx00 = d010 + (d110_d010)*coefs.x - dx00; - float dx01 = d001 + (d101_d001)*coefs.x; - float dx11_dx01 = d011 + (d111_d011)*coefs.x - dx01; - float dy0 = dx00 + (dx10_dx00)*coefs.y; - float dy1_dy0 = dx01 + (dx11_dx01)*coefs.y - dy0; - distance = dy0 + (dy1_dy0)*coefs.z; - float r = 0; - if (curTest.radius) - { - r = curTest.radius[threadIdx.x]; - } - if (distance < curTest.margin+r) - { - n = 1; - grad.z = dy1_dy0; - grad.y = (dx10_dx00) + ((dx11_dx01)-(dx10_dx00))*coefs.z; - dy0 = d100_d000 + (d110_d010 - d100_d000)*coefs.y; - dy1_dy0 = d101_d001 + (d111_d011 - d101_d001)*coefs.y - dy0; - grad.x = dy0 + (dy1_dy0)*coefs.z; - grad *= invnorm(grad); - //normal = grad; - p -= grad*distance; - //distance -= r; - distance = r; - //grad = CudaVec3::make(0,1,1); - } - } - } - - scan[threadIdx.x] = n; - - for (int i=1; i=i) - scan[threadIdx.x] = scan[threadIdx.x] + scan[threadIdx.x - i]; - } - - if (n) - { - int i = scan[threadIdx.x]-1; - GPUContact c; - //c.p1 = threadIdx.x; - //c.p2 = p; - c.distance = distance; - c.normal = -grad; - //c.normal = normal; //CudaVec3::make(-grad.x,-grad.y,-grad.z); //-grad; - curTest.result[i] = c; - //curTest.result[scan[threadIdx.x]-1].p1 = threadIdx.x; - //curTest.result[scan[threadIdx.x]-1].p2 = p; - //curTest.result[scan[threadIdx.x]-1].distance = distance; - //curTest.result[scan[threadIdx.x]-1].normal = normal; - GPUContactPoint cp1; - cp1.elem = threadIdx.x; - cp1.p = p0; - curTest.result1[i] = cp1; - GPUContactPoint cp2; - cp2.elem = 0; - cp2.p = p; - curTest.result2[i] = cp2; - } - if (threadIdx.x == curTest.nbPoints-1) - nresults[blockIdx.x] = scan[curTest.nbPoints-1]; -} - -////////////////////// -// CPU-side methods // -////////////////////// - -void CudaCollisionDetection_runTests(unsigned int nbTests, unsigned int maxPoints, const void* tests, void* nresults) -{ - sofa::gpu::cuda::mycudaPrintf("sizeof(GPUTest)=%d\nsizeof(GPUContact)=%d\nsizeof(matrix3)=%d\n",sizeof(GPUTest),sizeof(GPUContact),sizeof(matrix3)); - const GPUTest* gputests = (const GPUTest*)tests; - // round up to 16 - //maxPoints = (maxPoints+15)&-16; - dim3 threads(maxPoints,1); - dim3 grid(nbTests,1); - {CudaCollisionDetection_runTests_kernel<<< grid, threads, threads.x*sizeof(int) >>>(gputests, (int*)nresults); mycudaDebugError("CudaCollisionDetection_runTests_kernel");} - -} diff --git a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaCollisionDetection.h b/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaCollisionDetection.h deleted file mode 100644 index 14a0c9753d9..00000000000 --- a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaCollisionDetection.h +++ /dev/null @@ -1,218 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_GPU_CUDA_CUDACOLLISIONDETECTION_H -#define SOFA_GPU_CUDA_CUDACOLLISIONDETECTION_H - -#include -#include -#include -#include -#include -#include -#include - - -namespace sofa -{ - -namespace core -{ - -namespace collision -{ - -using type::Vec3f; -using type::Mat3x3f; -using sofacuda::GPUDetectionOutputVector; - -template<> -class TDetectionOutputVector : public GPUDetectionOutputVector -{ -}; - -template<> -class TDetectionOutputVector : public GPUDetectionOutputVector -{ -}; - -template<> -class TDetectionOutputVector : public GPUDetectionOutputVector -{ -}; - -} // namespace collision - -} // namespace core - -namespace gpu -{ - -namespace cuda -{ - - - -class CudaCollisionDetection - : public sofa::component::collision::detection::algorithm::BruteForceBroadPhase - , public sofa::core::collision::NarrowPhaseDetection -{ -public: - SOFA_CLASS2(CudaCollisionDetection, sofa::component::collision::detection::algorithm::BruteForceBroadPhase, sofa::core::collision::NarrowPhaseDetection); - struct GPUTest - { - void* result; - void* result1; - void* result2; - const void* points; - const void* radius; - const void* grid; - Mat3x3f rotation; - Vec3f translation; - float margin; - int nbPoints; - int gridnx, gridny, gridnz; - Vec3f gridbbmin, gridbbmax; - Vec3f gridp0, gridinvdp; - }; - - /*struct GPUContact - { - int p1; - Vec3f p2; - float distance; - Vec3f normal; - };*/ - //typedef sofa::core::collision::GPUDetectionOutput GPUContact; - typedef sofa::core::collision::GPUDetectionOutputVector GPUOutputVector; - - CudaVector gputests; - CudaVector gpuresults; ///< number of contact detected on each test - - typedef sofa::core::collision::DetectionOutputVector DetectionOutputVector; - - class Test - { - public: - GPUOutputVector results; - Test() {} - virtual ~Test() - { - } - virtual bool useGPU()=0; - /// Returns how many tests are required - virtual int init()=0; - /// Fill the info to send to the graphics card - virtual void fillInfo(GPUTest* tests)=0; - /// Create the list of SOFA contacts from the contacts detected by the GPU - //virtual void fillContacts(DetectionOutputVector& contacts, const int* nresults)=0; - }; - - class CPUTest : public Test - { - public: - CPUTest() {} - bool useGPU() { return false; } - int init() { return 0; } - void fillInfo(GPUTest* /*tests*/) {} - //void fillContacts(DetectionOutputVector& /*contacts*/, const int* /*nresults*/) {} - }; - - class RigidRigidTest : public Test - { - public: - CudaRigidDistanceGridCollisionModel* model1; - CudaRigidDistanceGridCollisionModel* model2; - RigidRigidTest(CudaRigidDistanceGridCollisionModel* model1, CudaRigidDistanceGridCollisionModel* model2); - bool useGPU() { return true; } - /// Returns how many tests are required - virtual int init(); - /// Fill the info to send to the graphics card - virtual void fillInfo(GPUTest* tests); - /// Create the list of SOFA contacts from the contacts detected by the GPU - //void fillContacts(DetectionOutputVector& contacts, const int* nresults); - - protected: - //void fillInfo(GPUTest& test, CudaVector& gpucontacts, CudaRigidDistanceGridCollisionElement elem1, CudaRigidDistanceGridCollisionElement elem2); - //void fillContacts(DetectionOutputVector& contacts, int nresults, CudaVector& gpucontacts, CudaRigidDistanceGridCollisionElement e1, CudaRigidDistanceGridCollisionElement e2, bool invert); - }; - - class SphereRigidTest : public Test - { - public: - CudaSphereCollisionModel* model1; - CudaRigidDistanceGridCollisionModel* model2; - SphereRigidTest(CudaSphereCollisionModel *model1, CudaRigidDistanceGridCollisionModel* model2); - bool useGPU() { return true; } - /// Returns how many tests are required - virtual int init(); - /// Fill the info to send to the graphics card - virtual void fillInfo(GPUTest* tests); - /// Create the list of SOFA contacts from the contacts detected by the GPU - //void fillContacts(DetectionOutputVector& contacts, const int* nresults); - }; - - class PointRigidTest : public Test - { - public: - CudaPointCollisionModel* model1; - CudaRigidDistanceGridCollisionModel* model2; - PointRigidTest(CudaPointCollisionModel* model1, CudaRigidDistanceGridCollisionModel* model2); - bool useGPU() { return true; } - /// Returns how many tests are required - virtual int init(); - /// Fill the info to send to the graphics card - virtual void fillInfo(GPUTest* tests); - /// Create the list of SOFA contacts from the contacts detected by the GPU - //void fillContacts(DetectionOutputVector& contacts, const int* nresults); - - protected: - //void fillInfo(GPUTest& test, CudaVector& gpucontacts, CudaRigidDistanceGridCollisionElement elem1, CudaRigidDistanceGridCollisionElement elem2); - //void fillContacts(DetectionOutputVector& contacts, int nresults, CudaVector& gpucontacts, CudaRigidDistanceGridCollisionElement e1, CudaRigidDistanceGridCollisionElement e2, bool invert); - }; - - struct Entry - { - int index; // negative if not active - Test* test; - Entry() : index(-1), test(NULL) {} - ~Entry() { if (test!=NULL) delete test; } - }; - - typedef std::map< std::pair, Entry > TestMap; - - TestMap tests; - - virtual void beginNarrowPhase() override; - virtual void addCollisionPair (const std::pair& cmPair) override; - virtual void endNarrowPhase() override; - -protected: - Test* createTest(core::CollisionModel* model1, core::CollisionModel* model2); -}; - -} // namespace cuda - -} // namespace gpu - -} // namespace sofa - -#endif diff --git a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaCollisionDistanceGrid.cpp b/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaCollisionDistanceGrid.cpp deleted file mode 100644 index 7245bc839ae..00000000000 --- a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaCollisionDistanceGrid.cpp +++ /dev/null @@ -1,162 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#include -#include -#include -#include -#include "CudaDistanceGridCollisionModel.h" -#include -#include - -#include -#include - - -namespace sofa::component::collision::response::contact -{ - -using namespace sofa::gpu::cuda; - -template <> -void BarycentricPenalityContact::setDetectionOutputs(OutputVector* o) -{ - GPUDetectionOutputVector& outputs = *dynamic_cast(o); - //const bool printLog = this->f_printLog.getValue(); - if (ff==NULL) - { - MechanicalState1* mstate1 = mapper1.createMapping("contactPointsCUDA"); - MechanicalState2* mstate2 = mapper2.createMapping("contactPointsCUDA"); - ff = sofa::core::objectmodel::New(mstate1,mstate2); - ff->setName( getName() ); - ff->init(); - } - - mapper1.setPoints1(&outputs); - mapper2.setPoints2(&outputs); - const double d0 = intersectionMethod->getContactDistance() + model1->getContactDistance() + model2->getContactDistance(); // - 0.001; -#if 0 - int insize = outputs.size(); - int size = insize; - ff->clear(size); - //int i = 0; - for (int i=0; ielem.first); - //CollisionElement2 elem2(o->elem.second); - //int index1 = elem1.getIndex(); - //int index2 = elem2.getIndex(); - int index1 = index; - int index2 = index; - //// Create mapping for first point - //index1 = mapper1.addPoint(o->point[0], index1); - //// Create mapping for second point - //index2 = mapper2.addPoint(o->point[1], index2); - double distance = d0 + outputs.get(i)->distance; // + mapper1.radius(elem1) + mapper2.radius(elem2); - double stiffness = (model1->getContactStiffness(0) * model1->getContactStiffness(0))/distance; - double mu_v = (model1->getContactFriction(0) + model1->getContactFriction(0)); - ff->addContact(index1, index2, outputs.get(i)->normal, (float)distance, (float)stiffness, (float)mu_v, (float)mu_v, index); - } -#else - double distance = d0; // + mapper1.radius(elem1) + mapper2.radius(elem2); - double stiffness = (model1->getContactStiffness(0) * model1->getContactStiffness(0)); ///distance; - ff->setContacts((float)distance, (float)stiffness, &outputs, true); -#endif - // Update mappings - mapper1.update(); - mapper2.update(); -} - -template <> -void BarycentricPenalityContact::setDetectionOutputs(OutputVector* o) -{ - - GPUDetectionOutputVector& outputs = *dynamic_cast(o); - //const bool printLog = this->f_printLog.getValue(); - if (ff==NULL) - { - MechanicalState1* mstate1 = mapper1.createMapping("contactPointsCUDA"); - MechanicalState2* mstate2 = mapper2.createMapping("contactPointsCUDA"); - ff = sofa::core::objectmodel::New(mstate1,mstate2); - ff->setName( getName() ); - ff->init(); - } - - mapper1.setPoints1(&outputs); - mapper2.setPoints2(&outputs); - const double d0 = intersectionMethod->getContactDistance() + model1->getContactDistance() + model2->getContactDistance(); // - 0.001; -#if 0 - int insize = outputs.size(); - int size = insize; - ff->clear(size); - //int i = 0; - for (int i=0; ielem.first); - //CollisionElement2 elem2(o->elem.second); - //int index1 = elem1.getIndex(); - //int index2 = elem2.getIndex(); - int index1 = index; - int index2 = index; - //// Create mapping for first point - //index1 = mapper1.addPoint(o->point[0], index1); - //// Create mapping for second point - //index2 = mapper2.addPoint(o->point[1], index2); - double distance = d0 + outputs.get(i)->distance; // + mapper1.radius(elem1) + mapper2.radius(elem2); - double stiffness = (model1->getContactStiffness(0) * model1->getContactStiffness(0))/distance; - double mu_v = (model1->getContactFriction(0) + model1->getContactFriction(0)); - ff->addContact(index1, index2, outputs.get(i)->normal, (float)distance, (float)stiffness, (float)mu_v, (float)mu_v, index); - } -#else - double distance = d0; // + mapper1.radius(elem1) + mapper2.radius(elem2); - double stiffness = (model1->getContactStiffness(0) * model1->getContactStiffness(0)); ///distance; - ff->setContacts((float)distance, (float)stiffness, &outputs, true); -#endif - // Update mappings - mapper1.update(); - mapper2.update(); - -} - -} // namespace sofa::component::collision::response::contact - - -namespace sofa::gpu::cuda -{ - -using namespace sofa::component::collision; -using namespace sofa::component::collision::response::contact; - -//sofa::helper::Creator > CudaDistanceGridCudaDistanceGridContactClass("PenalityContactForceField", true); -sofa::helper::Creator > CudaPointCudaDistanceGridContactClass("PenalityContactForceField", true); -sofa::helper::Creator > CudaSphereCudaDistanceGridContactClass("PenalityContactForceField", true); -//sofa::helper::Creator > CudaDistanceGridDistanceGridContactClass("PenalityContactForceField", true); -//sofa::helper::Creator > CudaDistanceGridPointContactClass("PenalityContactForceField", true); -//sofa::helper::Creator > CudaDistanceGridSphereContactClass("PenalityContactForceField", true); -//sofa::helper::Creator > CudaDistanceGridTriangleContactClass("PenalityContactForceField", true); - -} // namespace sofa::gpu::cuda diff --git a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaDistanceGridCollisionModel.cpp b/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaDistanceGridCollisionModel.cpp deleted file mode 100644 index 084ba36a8eb..00000000000 --- a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaDistanceGridCollisionModel.cpp +++ /dev/null @@ -1,810 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ - -#include "CudaDistanceGridCollisionModel.h" -#include -#include -#include -#include -#include -#if SOFADISTANCEGRID_HAVE_MINIFLOWVR - #include -#endif // SOFADISTANCEGRID_HAVE_MINIFLOWVR -#include -#include - -namespace sofa -{ - -namespace gpu -{ - -namespace cuda -{ - -void registerCudaRigidDistanceGridCollisionModel(sofa::core::ObjectFactory* factory) -{ - factory->registerObjects(sofa::core::ObjectRegistrationData("GPU-based grid distance field using CUDA.") - .add< CudaRigidDistanceGridCollisionModel >()); -} - -using namespace defaulttype; - -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// - -CudaDistanceGrid::CudaDistanceGrid(int nx, int ny, int nz, Coord pmin, Coord pmax) - : nbRef(1), nx(nx), ny(ny), nz(nz), nxny(nx*ny), nxnynz(nx*ny*nz) - , pmin(pmin), pmax(pmax) - , cellWidth ((pmax[0]-pmin[0])/(nx-1), (pmax[1]-pmin[1])/(ny-1),(pmax[2]-pmin[2])/(nz-1)) - , invCellWidth((nx-1)/(pmax[0]-pmin[0]), (ny-1)/(pmax[1]-pmin[1]),(nz-1)/(pmax[2]-pmin[2])) - , cubeDim(0) -{ - dists.resize(nxnynz); -} - -CudaDistanceGrid::~CudaDistanceGrid() -{ - std::map& shared = getShared(); - std::map::iterator it = shared.begin(); - while (it != shared.end() && it->second != this) ++it; - if (it != shared.end()) - shared.erase(it); // remove this grid from the list of already loaded grids -} - -/// Add one reference to this grid. Note that loadShared already does this. -CudaDistanceGrid* CudaDistanceGrid::addRef() -{ - ++nbRef; - return this; -} - -/// Release one reference, deleting this grid if this is the last -bool CudaDistanceGrid::release() -{ - if (--nbRef != 0) - return false; - delete this; - return true; -} - -CudaDistanceGrid* CudaDistanceGrid::load(const std::string& filename, double scale, double sampling, int nx, int ny, int nz, Coord pmin, Coord pmax) -{ - if (filename == "#cube") - { - float dim = (float)scale; - int np = 5; - Coord bbmin(-dim, -dim, -dim), bbmax(dim,dim,dim); - std::cout << "bbox = <"<-<"<"< pmax[c]) pmax[c] = bbmax[c]; - } - } - std::cout << "Creating cube distance grid in <"<-<"<"<calcCubeDistance(dim, np); - if (sampling) - grid->sampleSurface(sampling); - std::cout << "Distance grid creation DONE."<4 && filename.substr(filename.length()-4) == ".raw") - { - CudaDistanceGrid* grid = new CudaDistanceGrid(nx, ny, nz, pmin, pmax); - std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary); - in.read((char*)&(grid->dists[0]), grid->nxnynz*sizeof(Real)); - if (scale != 1.0) - { - for (int i=0; i< grid->nxnynz; i++) - grid->dists[i] *= (float)scale; - } - grid->computeBBox(); - if (sampling) - grid->sampleSurface(sampling); - return grid; - } -#if SOFADISTANCEGRID_HAVE_MINIFLOWVR - else if (filename.length()>6 && filename.substr(filename.length()-6) == ".fmesh") - { - flowvr::render::Mesh mesh; - if (!mesh.load(filename.c_str())) - { - std::cerr << "ERROR loading FlowVR mesh file "<nx; - ny = mesh.distmap->ny; - nz = mesh.distmap->nz; - ftl::Vec3f fpmin = ftl::transform(mesh.distmap->mat,ftl::Vec3f(0,0,0))*(float)scale; - ftl::Vec3f fpmax = ftl::transform(mesh.distmap->mat,ftl::Vec3f((float)(nx-1),(float)(ny-1),(float)(nz-1)))*(float)scale; - pmin = Coord(fpmin.ptr()); - pmax = Coord(fpmax.ptr()); - std::cout << "Copying "<-<"<"<nxnynz; i++) - grid->dists[i] = mesh.distmap->data[i]*(float)scale; - - if (sampling) - grid->sampleSurface(sampling); - else if (mesh.getAttrib(flowvr::render::Mesh::MESH_POINTS_GROUP)) - { - int nbpos = 0; - for (int i=0; i= 0) - ++nbpos; - } - std::cout << "Copying "<meshPts.resize(nbpos); - int p = 0; - for (int i=0; i= 0) - grid->meshPts[p++] = Coord(mesh.getPP(p0).ptr())*scale; - } - } - else - { - int nbpos = mesh.nbp(); - std::cout << "Copying "<meshPts.resize(nbpos); - for (int i=0; imeshPts[i] = Coord(mesh.getPP(i).ptr())*scale; - } - grid->computeBBox(); - std::cout << "Distance grid creation DONE."<4 && filename.substr(filename.length()-4) == ".obj") - { - sofa::helper::io::Mesh* mesh = sofa::helper::io::Mesh::Create(filename); - const sofa::type::vector & vertices = mesh->getVertices(); - - std::cout << "Computing bbox."< bbmax[c]) bbmax[c] = (Real)vertices[i][c]; - } - bbmin *= scale; - bbmax *= scale; - } - std::cout << "bbox = <"<-<"<"< pmax[c]) pmax[c] = bbmax[c]; - } - } - std::cout << "Creating distance grid in <"<-<"<"<meshPts.resize(vertices.size()); - for(unsigned int i=0; imeshPts[i] = vertices[i]*scale; - const auto & facets = mesh->getFacets(); - int nbt = 0; - int nbq = 0; - for (unsigned int i=0; i= 3) - nbt += pts.size()-2; - } - grid->meshTriangles.resize(nbt); - grid->meshQuads.resize(nbq); - nbt=0; - nbq=0; - for (unsigned int i=0; imeshQuads[nbq++] = sofa::core::topology::BaseMeshTopology::Quad(pts[0],pts[1],pts[2],pts[3]); - else if (pts.size() >= 3) - for (unsigned int j=2; jmeshTriangles[nbt++] = sofa::core::topology::BaseMeshTopology::Triangle(pts[0],pts[j-1],pts[j]); - } - std::cout << "Computing distance field."<calcDistance(); - if (sampling) - grid->sampleSurface(sampling); - grid->computeBBox(); - std::cout << "Distance grid creation DONE."<4 && filename.substr(filename.length()-4) == ".raw") - { - std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary); - out.write((const char*)&(dists[0]), nxnynz*sizeof(Real)); - } - else - { - std::cerr << " CudaDistanceGrid::save(): Unsupported extension: "< bbmax[c]) bbmax[c] = (Real)meshPts[i][c]; - } - } - else - { - bbmin = pmin; - bbmax = pmax; - /// \TODO compute the real bbox from the grid content - } -} - -/// Sample the surface with points approximately separated by the given sampling distance (expressed in voxels if the value is negative) -void CudaDistanceGrid::sampleSurface(double sampling) -{ - std::cout << "CudaDistanceGrid: sample surface with sampling distance " << sampling << std::endl; - int stepX, stepY, stepZ; - if (sampling < 0) - { - stepX = stepY = stepZ = (int)(-sampling); - } - else - { - stepX = (int)(sampling/cellWidth[0]); - stepY = (int)(sampling/cellWidth[1]); - stepZ = (int)(sampling/cellWidth[2]); - } - if (stepX < 1) stepX = 1; - if (stepY < 1) stepY = 1; - if (stepZ < 1) stepZ = 1; - std::cout << "CudaDistanceGrid: sampling steps: " << stepX << " " << stepY << " " << stepZ << std::endl; - - SReal maxD = (SReal)sqrt((cellWidth[0]*stepX)*(cellWidth[0]*stepX) + (cellWidth[1]*stepY)*(cellWidth[1]*stepY) + (cellWidth[2]*stepZ)*(cellWidth[2]*stepZ)); - std::vector pts; - for (int z=1; z maxD) continue; - - type::Vec3 pos = coord(x,y,z); - type::Vec3 n = grad(index(x,y,z), Coord()); // note that there are some redundant computations between interp() and grad() - n.normalize(); - pos -= n * (d * 0.99); // push pos back to the surface - d = interp(pos); - int it = 1; - while (helper::rabs(d) > 0.01f*maxD && it < 10) - { - n = grad(pos); - n.normalize(); - pos -= n * (d * 0.99); // push pos back to the surface - d = interp(pos); - ++it; - } - if (it == 10 && helper::rabs(d) > 0.1f*maxD) - { - std::cout << "Failed to converge at ("< 1) - { - int nbp = np*np*np - (np-2)*(np-2)*(np-2); - //std::cout << "Copying "< dim2) { s[c] = dim2; out = true; } - } - Real d; - if (out) - d = (p - s).norm(); - else - d = helper::rmax(helper::rmax(helper::rabs(s[0]),helper::rabs(s[1])),helper::rabs(s[2])) - dim2; - dists[i] = d - (dim-dim2); - } - //computeBBox(); - bbmin = Coord(-dim,-dim,-dim); - bbmax = Coord( dim, dim, dim); -} - -/// Compute distance field from given mesh -void CudaDistanceGrid::calcDistance() -{ - - if (GLEW_EXT_framebuffer_object && GLEW_ARB_vertex_buffer_object) - { - static GLuint fbid = 0; - if (!fbid) - { - glGenFramebuffersEXT(1,&fbid); - glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, fbid); - } - - /* - if (!texcolor) - glGenTextures(1, &texcolor); - - glBindTexture(target, texcolor); - - glTexParameteri(target, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexParameteri(target, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - glTexParameteri(target, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); - glTexParameteri(target, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); - glTexParameteri(target, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE); - glTexImage2D(target, 0, GL_RGBA8, nx, ny, 0, - GL_RGBA, GL_UNSIGNED_BYTE, NULL); - - if (!texdepth) - glGenTextures(1, &texdepth); - glBindTexture(target, texdepth); - - glTexParameteri(target, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexParameteri(target, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - glTexParameteri(target, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); - glTexParameteri(target, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); - glTexParameteri(target, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE); - glTexImage2D(target, 0, GL_DEPTH_COMPONENT24, nx, ny, 0, - GL_DEPTH_COMPONENT, GL_FLOAT, NULL); - - glFramebufferTexture2DEXT(GL_FRAMEBUFFER_EXT, GL_COLOR_ATTACHMENT0_EXT, target, texcolor, 0);*/ - } - else - { - std::cerr << "ERROR: Unsupported OpenGL extensions EXT_framebuffer_object ARB_vertex_buffer_object" << std::endl; - } - -} - -CudaDistanceGrid* CudaDistanceGrid::loadShared(const std::string& filename, double scale, double sampling, int nx, int ny, int nz, Coord pmin, Coord pmax) -{ - CudaDistanceGridParams params; - params.filename = filename; - params.scale = scale; - params.sampling = sampling; - params.nx = nx; - params.ny = ny; - params.nz = nz; - params.pmin = pmin; - params.pmax = pmax; - std::map& shared = getShared(); - std::map::iterator it = shared.find(params); - if (it != shared.end()) - return it->second->addRef(); - else - { - return shared[params] = load(filename, scale, sampling, nx, ny, nz, pmin, pmax); - } -} - -std::map& CudaDistanceGrid::getShared() -{ - static std::map instance; - return instance; -} - - -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// - -CudaRigidDistanceGridCollisionModel::CudaRigidDistanceGridCollisionModel() - : modified(true) - , fileCudaRigidDistanceGrid( initData( &fileCudaRigidDistanceGrid, "fileCudaRigidDistanceGrid", "load distance grid from specified file")) - , scale( initData( &scale, 1.0, "scale", "scaling factor for input file")) - , sampling( initData( &sampling, 0.0, "sampling", "if not zero: sample the surface with points approximately separated by the given sampling distance (expressed in voxels if the value is negative)")) - , box( initData( &box, "box", "Field bounding box defined by xmin,ymin,zmin, xmax,ymax,zmax") ) - , nx( initData( &nx, 64, "nx", "number of values on X axis") ) - , ny( initData( &ny, 64, "ny", "number of values on Y axis") ) - , nz( initData( &nz, 64, "nz", "number of values on Z axis") ) - , dumpfilename( initData( &dumpfilename, "dumpfilename","write distance grid to specified file")) - , usePoints( initData( &usePoints, true, "usePoints", "use mesh vertices for collision detection")) -{ - rigid = NULL; - addAlias(&fileCudaRigidDistanceGrid,"filename"); -} - -CudaRigidDistanceGridCollisionModel::~CudaRigidDistanceGridCollisionModel() -{ - for (unsigned int i=0; irelease(); - if (elems[i].prevGrid!=NULL) elems[i].prevGrid->release(); - } -} - -void CudaRigidDistanceGridCollisionModel::init() -{ - std::cout << "> CudaRigidDistanceGridCollisionModel::init()"<core::CollisionModel::init(); - rigid = dynamic_cast< core::behavior::MechanicalState* > (getContext()->getMechanicalState()); - - CudaDistanceGrid* grid = NULL; - if (fileCudaRigidDistanceGrid.getValue().empty()) - { - if (elems.size()==0 || elems[0].grid==NULL) - std::cerr << "ERROR: CudaRigidDistanceGridCollisionModel requires an input filename.\n"; - // else the grid has already been set - return; - } - std::cout << "CudaRigidDistanceGridCollisionModel: creating "<-<"<"; - std::cout << std::endl; - grid = CudaDistanceGrid::loadShared(fileCudaRigidDistanceGrid.getFullPath(), scale.getValue(), sampling.getValue(), nx.getValue(),ny.getValue(),nz.getValue(),box.getValue()[0],box.getValue()[1]); - - resize(1); - elems[0].grid = grid; - if (grid && !dumpfilename.getValue().empty()) - { - std::cout << "CudaRigidDistanceGridCollisionModel: dump grid to "<save(dumpfilename.getFullPath()); - } - std::cout << "< CudaRigidDistanceGridCollisionModel::init()"<core::CollisionModel::resize(s); - elems.resize(s); -} - -void CudaRigidDistanceGridCollisionModel::setGrid(CudaDistanceGrid* surf, Index index) -{ - if (elems[index].grid == surf) return; - if (elems[index].grid!=NULL) elems[index].grid->release(); - elems[index].grid = surf->addRef(); - modified = true; -} - -void CudaRigidDistanceGridCollisionModel::setNewState(Index index, double dt, CudaDistanceGrid* grid, const Matrix3& rotation, const type::Vec3& translation) -{ - grid->addRef(); - if (elems[index].prevGrid!=NULL) - elems[index].prevGrid->release(); - elems[index].prevGrid = elems[index].grid; - elems[index].grid = grid; - elems[index].prevRotation = elems[index].rotation; - elems[index].rotation = rotation; - elems[index].prevTranslation = elems[index].translation; - elems[index].translation = translation; - if (!elems[index].isTransformed) - { - Matrix3 I; I.identity(); - if (!(rotation == I) || !(translation == type::Vec3())) - elems[index].isTransformed = true; - } - elems[index].prevDt = dt; - modified = true; -} - -using sofa::component::collision::geometry::CubeCollisionModel; - -/// Create or update the bounding volume hierarchy. -void CudaRigidDistanceGridCollisionModel::computeBoundingTree(int maxDepth) -{ - CubeCollisionModel* cubeModel = this->createPrevious(); - - if (!modified && !isMoving() && !cubeModel->empty()) return; // No need to recompute BBox if immobile - - updateGrid(); - - cubeModel->resize(size); - for (int i=0; i(elems[i])->recalcBBox(); - type::Vec3 emin, emax; - if (rigid) - { - const RigidTypes::Coord& xform = rigid->read(sofa::core::vec_id::read_access::position)->getValue()[i]; - elems[i].translation = xform.getCenter(); - xform.getOrientation().toMatrix(elems[i].rotation); - elems[i].isTransformed = true; - } - if (elems[i].isTransformed) - { - //std::cout << "Grid "< x + <"<"<getBBCorner(0); - emin = corner; - emax = emin; - for (int j=1; j<8; j++) - { - corner = elems[i].translation + elems[i].rotation * elems[i].grid->getBBCorner(j); - for(int c=0; c<3; c++) - if (corner[c] < emin[c]) emin[c] = corner[c]; - else if (corner[c] > emax[c]) emax[c] = corner[c]; - } - } - else - { - emin = elems[i].grid->getBBMin(); - emax = elems[i].grid->getBBMax(); - } - cubeModel->setParentOf(i, emin, emax); // define the bounding box of the current element - //std::cout << "Grid "<-<"<"<computeBoundingTree(maxDepth); - modified = false; -} - -void CudaRigidDistanceGridCollisionModel::updateGrid() -{ -} - -void CudaRigidDistanceGridCollisionModel::drawCollisionModel(const core::visual::VisualParams* vparams) -{ - if (vparams->displayFlags().getShowWireFrame()) - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - glDisable(GL_LIGHTING); - glColor4fv(getColor4f()); - glPointSize(3); - for (unsigned int i = 0; i < elems.size(); i++) - { - draw(vparams, i); - } - glPointSize(1); - if (vparams->displayFlags().getShowWireFrame()) - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); -} - -void CudaRigidDistanceGridCollisionModel::draw(const core::visual::VisualParams* , Index index) -{ -#if SOFACUDA_HAVE_SOFA_GL == 1 - if (elems[index].isTransformed) - { - glPushMatrix(); - // float m[16]; - // (*rigid->getX())[index].writeOpenGlMatrix( m ); - // glMultMatrixf(m); - Matrix4 m; - m.identity(); - m = elems[index].rotation; - m.transpose(); - m[3] = Vec4(elems[index].translation,1.0); - sofa::gl::glMultMatrix(m.ptr()); - } - - CudaDistanceGrid* grid = getGrid(index); - CudaDistanceGrid::Coord corners[8]; - for(unsigned int i=0; i<8; i++) - corners[i] = grid->getCorner(i); - //glEnable(GL_BLEND); - //glDepthMask(0); - if (!isMoving()) - glColor4f(0.25f, 0.25f, 0.25f, 0.1f); - else - glColor4f(0.5f, 0.5f, 0.5f, 0.1f); - glBegin(GL_LINES); - { - glVertex3fv(corners[0].ptr()); glVertex3fv(corners[4].ptr()); - glVertex3fv(corners[1].ptr()); glVertex3fv(corners[5].ptr()); - glVertex3fv(corners[2].ptr()); glVertex3fv(corners[6].ptr()); - glVertex3fv(corners[3].ptr()); glVertex3fv(corners[7].ptr()); - glVertex3fv(corners[0].ptr()); glVertex3fv(corners[2].ptr()); - glVertex3fv(corners[1].ptr()); glVertex3fv(corners[3].ptr()); - glVertex3fv(corners[4].ptr()); glVertex3fv(corners[6].ptr()); - glVertex3fv(corners[5].ptr()); glVertex3fv(corners[7].ptr()); - glVertex3fv(corners[0].ptr()); glVertex3fv(corners[1].ptr()); - glVertex3fv(corners[2].ptr()); glVertex3fv(corners[3].ptr()); - glVertex3fv(corners[4].ptr()); glVertex3fv(corners[5].ptr()); - glVertex3fv(corners[6].ptr()); glVertex3fv(corners[7].ptr()); - } - glEnd(); - glDisable(GL_BLEND); - glDepthMask(1); - for(unsigned int i=0; i<8; i++) - corners[i] = grid->getBBCorner(i); - //glEnable(GL_BLEND); - //glDepthMask(0); - - if (!isMoving()) - glColor4f(0.5f, 0.5f, 0.5f, 1.0f); - else - glColor4f(1.0f, 1.0f, 1.0f, 1.0f); - glBegin(GL_LINES); - { - glVertex3fv(corners[0].ptr()); glVertex3fv(corners[4].ptr()); - glVertex3fv(corners[1].ptr()); glVertex3fv(corners[5].ptr()); - glVertex3fv(corners[2].ptr()); glVertex3fv(corners[6].ptr()); - glVertex3fv(corners[3].ptr()); glVertex3fv(corners[7].ptr()); - glVertex3fv(corners[0].ptr()); glVertex3fv(corners[2].ptr()); - glVertex3fv(corners[1].ptr()); glVertex3fv(corners[3].ptr()); - glVertex3fv(corners[4].ptr()); glVertex3fv(corners[6].ptr()); - glVertex3fv(corners[5].ptr()); glVertex3fv(corners[7].ptr()); - glVertex3fv(corners[0].ptr()); glVertex3fv(corners[1].ptr()); - glVertex3fv(corners[2].ptr()); glVertex3fv(corners[3].ptr()); - glVertex3fv(corners[4].ptr()); glVertex3fv(corners[5].ptr()); - glVertex3fv(corners[6].ptr()); glVertex3fv(corners[7].ptr()); - } - glEnd(); - - const float mindist = -(grid->getPMax()-grid->getPMin()).norm()*0.1f; - const float maxdist = (grid->getPMax()-grid->getPMin()).norm()*0.025f; - - if (grid->meshPts.empty()) - { - glBegin(GL_POINTS); - { - for (int z=0, ind=0; zgetNz(); z++) - for (int y=0; ygetNy(); y++) - for (int x=0; xgetNx(); x++, ind++) - { - CudaDistanceGrid::Coord p = grid->coord(x,y,z); - CudaDistanceGrid::Real d = (*grid)[ind]; - if (d < mindist || d > maxdist) continue; - d /= maxdist; - if (d<0) - glColor3d(1+d*0.25, 0, 1+d); - else - glColor3d(0, 1-d*0.25, 1-d); - glVertex3fv(p.ptr()); - } - } - glEnd(); - } - else - { - glColor3d(1, 1 ,1); - glBegin(GL_POINTS); - for (unsigned int i=0; imeshPts.size(); i++) - { - glVertex3fv(grid->meshPts[i].ptr()); - } - glEnd(); - glBegin(GL_LINES); - for (unsigned int i=0; imeshPts.size(); i++) - { - CudaDistanceGrid::Coord p ( grid->meshPts[i].ptr() ); - glColor3d(1, 1 ,1); - CudaDistanceGrid::Coord grad = grid->grad(p); - grad.normalize(); - for (int j = -2; j <= 2; j++) - { - CudaDistanceGrid::Coord p2 = p + grad * (j*maxdist/2); - CudaDistanceGrid::Real d = grid->eval(p2); - //if (rabs(d) > maxdist) continue; - d /= maxdist; - if (d<0) - glColor3d(1+d*0.25, 0, 1+d); - else - glColor3d(0, 1-d*0.25, 1-d); - glVertex3fv(p2.ptr()); - if (j>-2 && j < 2) - glVertex3fv(p2.ptr()); - } - } - glEnd(); - } - if (elems[index].isTransformed) - { - glPopMatrix(); - } -#endif // SOFACUDA_HAVE_SOFA_GL == 1 -} - - - - -} // namespace cuda - -} // namespace gpu - -} // namespace sofa diff --git a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaDistanceGridCollisionModel.h b/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaDistanceGridCollisionModel.h deleted file mode 100644 index 8da7b24b5d6..00000000000 --- a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaDistanceGridCollisionModel.h +++ /dev/null @@ -1,591 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_GPU_CUDA_CUDADISTANCEGRIDCOLLISIONMODEL_H -#define SOFA_GPU_CUDA_CUDADISTANCEGRIDCOLLISIONMODEL_H - -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace sofa -{ - -namespace gpu -{ - -namespace cuda -{ - -using namespace sofa::type; -using namespace sofa::defaulttype; -//using namespace sofa::component::collision; - -class CudaDistanceGrid -{ -public: - typedef float Real; - static Real maxDist() { return (Real)1e10; } - typedef Vec3f Coord; - //typedef Vec4f SamplingPoint; ///< 3D coordinates + radius - typedef CudaVector VecReal; - typedef CudaVector VecCoord; - - CudaDistanceGrid(int nx, int ny, int nz, Coord pmin, Coord pmax); - -protected: - ~CudaDistanceGrid(); - -public: - - /// Load a distance grid - static CudaDistanceGrid* load(const std::string& filename, double scale=1.0, double sampling=0.0, int nx=64, int ny=64, int nz=64, Coord pmin = Coord(), Coord pmax = Coord()); - - /// Load or reuse a distance grid - static CudaDistanceGrid* loadShared(const std::string& filename, double scale=1.0, double sampling=0.0, int nx=64, int ny=64, int nz=64, Coord pmin = Coord(), Coord pmax = Coord()); - - /// Add one reference to this grid. Note that loadShared already does this. - CudaDistanceGrid* addRef(); - - /// Release one reference, deleting this grid if this is the last - bool release(); - - /// Save current grid - bool save(const std::string& filename); - - /// Compute distance field from stored mesh - void calcDistance(); - - /// Compute distance field for a cube of the given half-size. - /// Also create a mesh of points using np points per axis - void calcCubeDistance(Real dim=1, int np=5); - - /// Sample the surface with points approximately separated by the given sampling distance (expressed in voxels if the value is negative) - void sampleSurface(double sampling=-1.0); - - /// Update bbox - void computeBBox(); - - int getNx() const { return nx; } - int getNy() const { return ny; } - int getNz() const { return nz; } - const Coord& getCellWidth() const { return cellWidth; } - const Coord& getInvCellWidth() const { return invCellWidth; } - const VecReal& getDists() const { return dists; } - VecReal& getDists() { return dists; } - - int size() const { return nxnynz; } - - const Coord& getBBMin() const { return bbmin; } - const Coord& getBBMax() const { return bbmax; } - void setBBMin(const Coord& val) { bbmin = val; } - void setBBMax(const Coord& val) { bbmax = val; } - Coord getBBCorner(int i) const { return Coord((i&1)?bbmax[0]:bbmin[0],(i&2)?bbmax[1]:bbmin[1],(i&4)?bbmax[2]:bbmin[2]); } - bool inBBox(const Coord& p, Real margin=0.0f) const - { - for (int c=0; c<3; ++c) - if (p[c] < bbmin[c]-margin || p[c] > bbmax[c]+margin) return false; - return true; - } - - const Coord& getPMin() const { return pmin; } - const Coord& getPMax() const { return pmax; } - Coord getCorner(int i) const { return Coord((i&1)?pmax[0]:pmin[0],(i&2)?pmax[1]:pmin[1],(i&4)?pmax[2]:pmin[2]); } - - bool isCube() const { return cubeDim != 0; } - Real getCubeDim() const { return cubeDim; } - - bool inGrid(const Coord& p) const - { - Coord epsilon = cellWidth*0.1; - for (int c=0; c<3; ++c) - if (p[c] < pmin[c]+epsilon[c] || p[c] > pmax[c]-epsilon[c]) return false; - return true; - } - - Coord clamp(Coord p) const - { - for (int c=0; c<3; ++c) - if (p[c] < pmin[c]) p[c] = pmin[c]; - else if (p[c] > pmax[c]) p[c] = pmax[c]; - return p; - } - - Index ix(const Coord& p) const - { - return helper::rfloor((p[0]-pmin[0])*invCellWidth[0]); - } - - Index iy(const Coord& p) const - { - return helper::rfloor((p[1]-pmin[1])*invCellWidth[1]); - } - - Index iz(const Coord& p) const - { - return helper::rfloor((p[2]-pmin[2])*invCellWidth[2]); - } - - Index index(const Coord& p, Coord& coefs) const - { - coefs[0] = (p[0]-pmin[0])*invCellWidth[0]; - coefs[1] = (p[1]-pmin[1])*invCellWidth[1]; - coefs[2] = (p[2]-pmin[2])*invCellWidth[2]; - int x = helper::rfloor(coefs[0]); - if (x==-1) x=0; else if (x==nx-1) x=nx-2; - coefs[0] -= x; - int y = helper::rfloor(coefs[1]); - if (y==-1) y=0; else if (y==ny-1) y=ny-2; - coefs[1] -= y; - int z = helper::rfloor(coefs[2]); - if (z==-1) z=0; else if (z==nz-1) z=nz-2; - coefs[2] -= z; - return x+nx*(y+ny*(z)); - } - - Index index(const Coord& p) const - { - Coord coefs; - return index(p, coefs); - } - - Index index(int x, int y, int z) - { - return x+nx*(y+ny*(z)); - } - - Coord coord(int x, int y, int z) - { - return pmin+Coord(x*cellWidth[0], y*cellWidth[1], z*cellWidth[2]); - } - - Real operator[](Index index) const { return dists[index]; } - Real& operator[](Index index) { return dists[index]; } - - static Real interp(Real coef, Real a, Real b) - { - return a+coef*(b-a); - } - - Real interp(Index index, const Coord& coefs) const - { - return interp(coefs[2],interp(coefs[1],interp(coefs[0],dists[index ],dists[index+1 ]), - interp(coefs[0],dists[index +nx ],dists[index+1+nx ])), - interp(coefs[1],interp(coefs[0],dists[index +nxny],dists[index+1 +nxny]), - interp(coefs[0],dists[index +nx+nxny],dists[index+1+nx+nxny]))); - } - - Real interp(const Coord& p) const - { - Coord coefs; - int i = index(p, coefs); - return interp(i, coefs); - } - - Coord grad(Index index, const Coord& coefs) const - { - // val = dist[0][0][0] * (1-x) * (1-y) * (1-z) - // + dist[1][0][0] * ( x) * (1-y) * (1-z) - // + dist[0][1][0] * (1-x) * ( y) * (1-z) - // + dist[1][1][0] * ( x) * ( y) * (1-z) - // + dist[0][0][1] * (1-x) * (1-y) * ( z) - // + dist[1][0][1] * ( x) * (1-y) * ( z) - // + dist[0][1][1] * (1-x) * ( y) * ( z) - // + dist[1][1][1] * ( x) * ( y) * ( z) - // dval / dx = (dist[1][0][0]-dist[0][0][0]) * (1-y) * (1-z) - // + (dist[1][1][0]-dist[0][1][0]) * ( y) * (1-z) - // + (dist[1][0][1]-dist[0][0][1]) * (1-y) * ( z) - // + (dist[1][1][1]-dist[0][1][1]) * ( y) * ( z) - const Real dist000 = dists[index ]; - const Real dist100 = dists[index+1 ]; - const Real dist010 = dists[index +nx ]; - const Real dist110 = dists[index+1+nx ]; - const Real dist001 = dists[index +nxny]; - const Real dist101 = dists[index+1 +nxny]; - const Real dist011 = dists[index +nx+nxny]; - const Real dist111 = dists[index+1+nx+nxny]; - return Coord( - interp(coefs[2],interp(coefs[1],dist100-dist000,dist110-dist010),interp(coefs[1],dist101-dist001,dist111-dist011)), //*invCellWidth[0], - interp(coefs[2],interp(coefs[0],dist010-dist000,dist110-dist100),interp(coefs[0],dist011-dist001,dist111-dist101)), //*invCellWidth[1], - interp(coefs[1],interp(coefs[0],dist001-dist000,dist101-dist100),interp(coefs[0],dist011-dist010,dist111-dist110))); //*invCellWidth[2]); - } - - Coord grad(const Coord& p) const - { - Coord coefs; - Index i = index(p, coefs); - return grad(i, coefs); - } - - Real eval(const Coord& x) const - { - Real d; - if (inGrid(x)) - { - d = interp(x); - } - else - { - Coord xclamp = clamp(x); - d = interp(xclamp); - d = helper::rsqrt((x-xclamp).norm2() + d*d); // we underestimate the distance - } - return d; - } - - Real quickeval(const Coord& x) const - { - Real d; - if (inGrid(x)) - { - d = dists[index(x)] - cellWidth[0]; // we underestimate the distance - } - else - { - Coord xclamp = clamp(x); - d = dists[index(xclamp)] - cellWidth[0]; // we underestimate the distance - d = helper::rsqrt((x-xclamp).norm2() + d*d); - } - return d; - } - - Real eval2(const Coord& x) const - { - Real d2; - if (inGrid(x)) - { - Real d = interp(x); - d2 = d*d; - } - else - { - Coord xclamp = clamp(x); - Real d = interp(xclamp); - d2 = ((x-xclamp).norm2() + d*d); // we underestimate the distance - } - return d2; - } - - Real quickeval2(const Coord& x) const - { - Real d2; - if (inGrid(x)) - { - Real d = dists[index(x)] - cellWidth[0]; // we underestimate the distance - d2 = d*d; - } - else - { - Coord xclamp = clamp(x); - Real d = dists[index(xclamp)] - cellWidth[0]; // we underestimate the distance - d2 = ((x-xclamp).norm2() + d*d); - } - return d2; - } - - //CudaVector meshPts; - CudaVector meshPts; - sofa::core::topology::BaseMeshTopology::SeqTriangles meshTriangles; - sofa::core::topology::BaseMeshTopology::SeqQuads meshQuads; - -protected: - int nbRef; - VecReal dists; - const int nx,ny,nz, nxny, nxnynz; - const Coord pmin, pmax; - const Coord cellWidth, invCellWidth; - Coord bbmin, bbmax; ///< bounding box of the object, smaller than the grid - - Real cubeDim; ///< Cube dimension (!=0 if this is actually a cube - //bool updated; - - // Grid shared resources - - struct CudaDistanceGridParams - { - std::string filename; - double scale; - double sampling; - int nx,ny,nz; - Coord pmin,pmax; - bool operator==(const CudaDistanceGridParams& v) const - { - if (!(filename == v.filename)) return false; - if (!(scale == v.scale )) return false; - if (!(sampling == v.sampling)) return false; - if (!(nx == v.nx )) return false; - if (!(ny == v.ny )) return false; - if (!(nz == v.nz )) return false; - if (!(pmin[0] == v.pmin[0] )) return false; - if (!(pmin[1] == v.pmin[1] )) return false; - if (!(pmin[2] == v.pmin[2] )) return false; - if (!(pmax[0] == v.pmax[0] )) return false; - if (!(pmax[1] == v.pmax[1] )) return false; - if (!(pmax[2] == v.pmax[2] )) return false; - return true; - } - bool operator<(const CudaDistanceGridParams& v) const - { - if (filename < v.filename) return false; - if (filename > v.filename) return true; - if (scale < v.scale ) return false; - if (scale > v.scale ) return true; - if (sampling < v.sampling) return false; - if (sampling > v.sampling) return true; - if (nx < v.nx ) return false; - if (nx > v.nx ) return true; - if (ny < v.ny ) return false; - if (ny > v.ny ) return true; - if (nz < v.nz ) return false; - if (nz > v.nz ) return true; - if (pmin[0] < v.pmin[0] ) return false; - if (pmin[0] > v.pmin[0] ) return true; - if (pmin[1] < v.pmin[1] ) return false; - if (pmin[1] > v.pmin[1] ) return true; - if (pmin[2] < v.pmin[2] ) return false; - if (pmin[2] > v.pmin[2] ) return true; - if (pmax[0] < v.pmax[0] ) return false; - if (pmax[0] > v.pmax[0] ) return true; - if (pmax[1] < v.pmax[1] ) return false; - if (pmax[1] > v.pmax[1] ) return true; - if (pmax[2] < v.pmax[2] ) return false; - if (pmax[2] > v.pmax[2] ) return true; - return false; - } - bool operator>(const CudaDistanceGridParams& v) const - { - if (filename > v.filename) return false; - if (filename < v.filename) return true; - if (scale > v.scale ) return false; - if (scale < v.scale ) return true; - if (sampling < v.sampling) return false; - if (sampling > v.sampling) return true; - if (nx > v.nx ) return false; - if (nx < v.nx ) return true; - if (ny > v.ny ) return false; - if (ny < v.ny ) return true; - if (nz > v.nz ) return false; - if (nz < v.nz ) return true; - if (pmin[0] > v.pmin[0] ) return false; - if (pmin[0] < v.pmin[0] ) return true; - if (pmin[1] > v.pmin[1] ) return false; - if (pmin[1] < v.pmin[1] ) return true; - if (pmin[2] > v.pmin[2] ) return false; - if (pmin[2] < v.pmin[2] ) return true; - if (pmax[0] > v.pmax[0] ) return false; - if (pmax[0] < v.pmax[0] ) return true; - if (pmax[1] > v.pmax[1] ) return false; - if (pmax[1] < v.pmax[1] ) return true; - if (pmax[2] > v.pmax[2] ) return false; - if (pmax[2] < v.pmax[2] ) return true; - return false; - } - }; - static std::map& getShared(); - -}; -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// - -class CudaRigidDistanceGridCollisionModel; - -class CudaRigidDistanceGridCollisionElement : public core::TCollisionElementIterator -{ -public: - CudaRigidDistanceGridCollisionElement(CudaRigidDistanceGridCollisionModel* model, Index index); - - explicit CudaRigidDistanceGridCollisionElement(const core::CollisionElementIterator& i); - - CudaDistanceGrid* getGrid(); - - bool isTransformed(); - const Matrix3& getRotation(); - const Vec3& getTranslation(); - - void setGrid(CudaDistanceGrid* surf); - - /// @name Previous state data - /// Used to estimate velocity in case the distance grid itself is dynamic - /// @{ - CudaDistanceGrid* getPrevGrid(); - const Matrix3& getPrevRotation(); - const Vec3& getPrevTranslation(); - double getPrevDt(); - /// @} - - /// Set new grid and transform, keeping the old state to estimate velocity - void setNewState(double dt, CudaDistanceGrid* grid, const Matrix3& rotation, const Vec3& translation); -}; - -class CudaRigidDistanceGridCollisionModel : public core::CollisionModel -{ -public: - SOFA_CLASS(CudaRigidDistanceGridCollisionModel,core::CollisionModel); -protected: - - class ElementData - { - public: - Matrix3 rotation; - Vec3 translation; - CudaDistanceGrid* grid; - - /// @name Previous state data - /// Used to estimate velocity in case the distance grid itself is dynamic - /// @{ - CudaDistanceGrid* prevGrid; ///< Previous grid - Matrix3 prevRotation; ///< Previous rotation - Vec3 prevTranslation; ///< Previous translation - double prevDt; ///< Time difference between previous and current state - /// @} - - bool isTransformed; ///< True if translation/rotation was set - ElementData() : grid(NULL), prevGrid(NULL), prevDt(0.0), isTransformed(false) { rotation.identity(); prevRotation.identity(); } - }; - - std::vector elems; - bool modified; - core::behavior::MechanicalState* rigid; - - void updateGrid(); - - void drawCollisionModel(const core::visual::VisualParams*) override; - -public: - // Input data parameters - sofa::core::objectmodel::DataFileName fileCudaRigidDistanceGrid; - Data< double > scale; ///< scaling factor for input file - Data< double > sampling; ///< if not zero: sample the surface with points approximately separated by the given sampling distance (expressed in voxels if the value is negative) - Data< type::fixed_array > box; ///< Field bounding box defined by xmin,ymin,zmin, xmax,ymax,zmax - Data< int > nx; ///< number of values on X axis - Data< int > ny; ///< number of values on Y axis - Data< int > nz; ///< number of values on Z axis - sofa::core::objectmodel::DataFileName dumpfilename; - - typedef Rigid3Types InDataTypes; - typedef Vec3Types DataTypes; - typedef CudaRigidDistanceGridCollisionElement Element; - - Data< bool > usePoints; ///< use mesh vertices for collision detection - - CudaRigidDistanceGridCollisionModel(); - - ~CudaRigidDistanceGridCollisionModel(); - - core::behavior::MechanicalState* getRigidModel() { return rigid; } - core::behavior::MechanicalState* getMechanicalState() { return rigid; } - - void init() override; - - CudaDistanceGrid* getGrid(Index index=0) - { - return elems[index].grid; - } - bool isTransformed(Index index=0) - { - return elems[index].isTransformed; - } - const Matrix3& getRotation(Index index=0) - { - return elems[index].rotation; - } - const Vec3& getTranslation(Index index=0) - { - return elems[index].translation; - } - - void setGrid(CudaDistanceGrid* surf, Index index=0); - - CudaDistanceGrid* getPrevGrid(Index index=0) - { - return elems[index].prevGrid; - } - const Matrix3& getPrevRotation(Index index=0) - { - return elems[index].prevRotation; - } - const Vec3& getPrevTranslation(Index index=0) - { - return elems[index].prevTranslation; - } - double getPrevDt(Index index=0) - { - return elems[index].prevDt; - } - - /// Set new grid and transform, keeping the old state to estimate velocity - void setNewState(Index index, double dt, CudaDistanceGrid* grid, const Matrix3& rotation, const Vec3& translation); - - /// @} - - /// Set new grid and transform, keeping the old state to estimate velocity - void setNewState(double dt, CudaDistanceGrid* grid, const Matrix3& rotation, const Vec3& translation); - - // -- CollisionModel interface - - void resize(Size size) override; - - /// Create or update the bounding volume hierarchy. - void computeBoundingTree(int maxDepth=0) override; - - void draw(const core::visual::VisualParams*, Index index) override; -}; - -inline CudaRigidDistanceGridCollisionElement::CudaRigidDistanceGridCollisionElement(CudaRigidDistanceGridCollisionModel* model, Index index) - : core::TCollisionElementIterator(model, index) -{} - -inline CudaRigidDistanceGridCollisionElement::CudaRigidDistanceGridCollisionElement(const core::CollisionElementIterator& i) - : core::TCollisionElementIterator(static_cast(i.getCollisionModel()), i.getIndex()) -{ -} - -inline CudaDistanceGrid* CudaRigidDistanceGridCollisionElement::getGrid() { return model->getGrid(index); } -inline void CudaRigidDistanceGridCollisionElement::setGrid(CudaDistanceGrid* surf) { return model->setGrid(surf, index); } - -inline bool CudaRigidDistanceGridCollisionElement::isTransformed() { return model->isTransformed(index); } -inline const Matrix3& CudaRigidDistanceGridCollisionElement::getRotation() { return model->getRotation(index); } -inline const Vec3& CudaRigidDistanceGridCollisionElement::getTranslation() { return model->getTranslation(index); } - -inline CudaDistanceGrid* CudaRigidDistanceGridCollisionElement::getPrevGrid() { return model->getPrevGrid(index); } -inline const Matrix3& CudaRigidDistanceGridCollisionElement::getPrevRotation() { return model->getPrevRotation(index); } -inline const Vec3& CudaRigidDistanceGridCollisionElement::getPrevTranslation() { return model->getPrevTranslation(index); } -inline double CudaRigidDistanceGridCollisionElement::getPrevDt() { return model->getPrevDt(index); } - -inline void CudaRigidDistanceGridCollisionElement::setNewState(double dt, CudaDistanceGrid* grid, const Matrix3& rotation, const Vec3& translation) -{ - return model->setNewState(dt, grid, rotation, translation); -} - -} // namespace cuda - -} // namespace gpu - -} // namespace sofa - -#endif diff --git a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaDistanceGridContactMapper.cu b/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaDistanceGridContactMapper.cu deleted file mode 100644 index 28965400760..00000000000 --- a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaDistanceGridContactMapper.cu +++ /dev/null @@ -1,71 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#include -#include -#include "cuda.h" - -extern "C" -{ - void RigidContactMapperCuda3f_setPoints2(unsigned int size, unsigned int nbTests, unsigned int maxPoints, const void* tests, const void* contacts, void* map); -} - -struct /*__align__(16)*/ GPUContact -{ - int p1; - float3 p2; - float distance; - float3 normal; -}; - -struct /*__align__(8)*/ GPUTestEntry -{ - int firstIndex; - int curSize; - int maxSize; - int newIndex; - int elem1,elem2; -}; - -__shared__ GPUTestEntry curTestEntry; - -__global__ void RigidContactMapperCuda3f_setPoints2_kernel(const GPUTestEntry* tests, const GPUContact* contacts, float3* map) -{ - if (threadIdx.x == 0) - curTestEntry = tests[blockIdx.x]; - - __syncthreads(); - - GPUContact c = contacts[curTestEntry.firstIndex + threadIdx.x]; - if (threadIdx.x < curTestEntry.curSize) - { - map[curTestEntry.newIndex + threadIdx.x] = c.p2; - } -} - -void RigidContactMapperCuda3f_setPoints2(unsigned int size, unsigned int nbTests, unsigned int maxPoints, const void* tests, const void* contacts, void* map) -{ - // round up to 16 - //maxPoints = (maxPoints+15)&-16; - dim3 threads(maxPoints,1); - dim3 grid(nbTests,1); - {RigidContactMapperCuda3f_setPoints2_kernel<<< grid, threads >>>((const GPUTestEntry*)tests, (GPUContact*)contacts, (float3*)map); mycudaDebugError("RigidContactMapperCuda3f_setPoints2_kernel");} -} diff --git a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaDistanceGridContactMapper.h b/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaDistanceGridContactMapper.h deleted file mode 100644 index e98718ceee4..00000000000 --- a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/CudaDistanceGridContactMapper.h +++ /dev/null @@ -1,102 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#pragma once -#include -#include -#include - -namespace sofa::gpu::cuda -{ -extern "C" -{ - void RigidContactMapperCuda3f_setPoints2(unsigned int size, unsigned int nbTests, unsigned int maxPoints, const void* tests, const void* contacts, void* map); -} -} - -/// Mapper for CudaRigidDistanceGridCollisionModel -template -class sofa::component::collision::response::mapper::ContactMapper : - public sofa::component::collision::response::mapper::RigidContactMapper -{ -public: - typedef typename DataTypes::Real Real; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef RigidContactMapper Inherit; - typedef typename Inherit::MMechanicalState MMechanicalState; - typedef typename Inherit::MCollisionModel MCollisionModel; - - int addPoint(const Coord& P, int index, Real& r) - { - int i = this->Inherit::addPoint(P, index, r); - if (!this->mapping) - { - MCollisionModel* model = this->model; - MMechanicalState* outmodel = this->outmodel.get(); - Data* d_x = outmodel->write(sofa::core::vec_id::write_access::position); - VecDeriv& vx = *d_x->beginEdit(); - Data* d_v = outmodel->write(sofa::core::vec_id::write_access::velocity); - VecCoord& vv = *d_v->beginEdit(); - - typename DataTypes::Coord& x = vx[i]; - typename DataTypes::Deriv& v = vv[i]; - if (model->isTransformed(index)) - { - x = model->getTranslation(index) + model->getRotation(index) * P; - } - else - { - x = P; - } - v = typename DataTypes::Deriv(); - - d_x->endEdit(); - d_v->endEdit(); - } - return i; - } - - void setPoints2(sofacuda::GPUDetectionOutputVector* outputs) - { - int n = outputs->size(); - int nt = outputs->nbTests(); - int maxp = 0; - for (int i=0; irtest(i).curSize > maxp) maxp = outputs->rtest(i).curSize; - if (this->outmodel) - this->outmodel->resize(n); - if (this->mapping) - { - this->mapping->d_points.beginEdit()->fastResize(n); - this->mapping->m_rotatedPoints.fastResize(n); - gpu::cuda::RigidContactMapperCuda3f_setPoints2(n, nt, maxp, outputs->tests.deviceRead(), outputs->results.deviceRead(), this->mapping->d_points.beginEdit()->deviceWrite()); - } - else - { - Data* d_x = this->outmodel->write(sofa::core::vec_id::write_access::position); - VecCoord& vx = *d_x->beginEdit(); - gpu::cuda::RigidContactMapperCuda3f_setPoints2(n, nt, maxp, outputs->tests.deviceRead(), outputs->results.deviceRead(), vx.deviceWrite()); - d_x->endEdit(); - } - } -}; diff --git a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/config.h.in b/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/config.h.in deleted file mode 100644 index ed81edd83ac..00000000000 --- a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/config.h.in +++ /dev/null @@ -1,37 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#pragma once - -#include - -#ifdef SOFA_BUILD_SOFADISTANCEGRID_CUDA -# define SOFA_TARGET @PROJECT_NAME@ -# define SOFA_SOFADISTANCEGRID_CUDA_API SOFA_EXPORT_DYNAMIC_LIBRARY -#else -# define SOFA_SOFADISTANCEGRID_CUDA_API SOFA_IMPORT_DYNAMIC_LIBRARY -#endif - -namespace sofadistancegrid::cuda -{ - constexpr const char* MODULE_NAME = "@PROJECT_NAME@"; - constexpr const char* MODULE_VERSION = "@PROJECT_VERSION@"; -} // namespace sofadistancegrid::cuda diff --git a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/init.cpp b/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/init.cpp deleted file mode 100644 index fd24b4bdf2a..00000000000 --- a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/init.cpp +++ /dev/null @@ -1,80 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#include -#include -#include - -#include -#include - -namespace sofa::gpu::cuda -{ - extern void registerCudaCollisionDetection(sofa::core::ObjectFactory* factory); - extern void registerCudaRigidDistanceGridCollisionModel(sofa::core::ObjectFactory* factory); -} - -namespace sofadistancegrid::cuda -{ - -extern "C" { - SOFA_EXPORT_DYNAMIC_LIBRARY void initExternalModule(); - SOFA_EXPORT_DYNAMIC_LIBRARY const char* getModuleName(); - SOFA_EXPORT_DYNAMIC_LIBRARY const char* getModuleVersion(); - SOFA_SOFADISTANCEGRID_API void registerObjects(sofa::core::ObjectFactory* factory); -} - -void initExternalModule() -{ - init(); -} - -const char* getModuleName() -{ - return MODULE_NAME; -} - -const char* getModuleVersion() -{ - return MODULE_VERSION; -} - -void init() -{ - static bool first = true; - if (first) - { - // make sure that this plugin is registered into the PluginManager - sofa::helper::system::PluginManager::getInstance().registerPlugin(MODULE_NAME); - - sofadistancegrid::initSofaDistanceGrid(); - sofa::gpu::cuda::init(); - first = false; - } -} - -void registerObjects(sofa::core::ObjectFactory* factory) -{ - sofa::gpu::cuda::registerCudaCollisionDetection(factory); - sofa::gpu::cuda::registerCudaRigidDistanceGridCollisionModel(factory); -} - -} // namespace volumetricrendering::cuda diff --git a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/init.h b/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/init.h deleted file mode 100644 index df76bdbe8e5..00000000000 --- a/applications/plugins/SofaDistanceGrid/extensions/CUDA/src/SofaDistanceGrid/CUDA/init.h +++ /dev/null @@ -1,29 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#pragma once - -#include - -namespace sofadistancegrid::cuda -{ -SOFA_SOFADISTANCEGRID_CUDA_API void init(); -} // namespace sofadistancegrid::cuda diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/CMakeLists.txt b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/CMakeLists.txt deleted file mode 100644 index d0f6f280175..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/CMakeLists.txt +++ /dev/null @@ -1,59 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(miniFlowVR) - -set(HEADER_FILES - include/ftl/cmdline.h - include/ftl/crc.h - include/ftl/fixed_array.h - include/ftl/mat.h - include/ftl/quat.h - include/ftl/rmath.h - include/ftl/type.h - include/ftl/vec.h - include/flowvr/render/bbox.h - include/flowvr/render/mesh.h - include/flowvr/render/mesh.inl - include/flowvr/render/noise.h -) - -set(SOURCE_FILES - src/ftlm/cmdline.cpp - src/ftlm/crc.cpp - src/ftlm/mat.cpp - src/ftlm/quat.cpp - src/ftlm/type.cpp - src/librender/bbox.cpp - src/librender/mesh.cpp - src/librender/mesh_dist.cpp - src/librender/mesh_io_mesh.cpp - src/librender/mesh_io_obj.cpp - src/librender/mesh_io_off.cpp - src/librender/mesh_io_stl.cpp - src/librender/mesh_io_vtk.cpp - src/librender/mesh_io_lwo.cpp -) - -add_library(${PROJECT_NAME} STATIC ${HEADER_FILES} ${SOURCE_FILES}) -target_include_directories(${PROJECT_NAME} PUBLIC "$") -target_include_directories(${PROJECT_NAME} PUBLIC "$") - -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) # static_assert needs at least C++11 (not by default with clang5) - -# The code must be relocatable if we want to link a shared library against it -if("x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xGNU" OR "x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xClang") - target_compile_options(${PROJECT_NAME} PRIVATE "-fPIC") -endif() - -if(WIN32) - # remove warnings about deprecation (CRT,etc) - target_compile_options(${PROJECT_NAME} PRIVATE "/wd4996") -endif() - -include(SofaMacros) -sofa_create_package_with_targets( - PACKAGE_NAME MiniFlowVR - PACKAGE_VERSION 0.1 - TARGETS ${PROJECT_NAME} - INCLUDE_SOURCE_DIR "include" - INCLUDE_INSTALL_DIR "extlibs/MiniFlowVR" - ) diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/MiniFlowVRConfig.cmake.in b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/MiniFlowVRConfig.cmake.in deleted file mode 100644 index 7f5ef1db2ef..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/MiniFlowVRConfig.cmake.in +++ /dev/null @@ -1,9 +0,0 @@ -# CMake package configuration file for the miniFlowVR library - -@PACKAGE_INIT@ - -if(NOT TARGET miniFlowVR) - include("${CMAKE_CURRENT_LIST_DIR}/MiniFlowVRTargets.cmake") -endif() - -check_required_components(miniFlowVR) diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/README.txt b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/README.txt deleted file mode 100644 index fe0d26848a1..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/README.txt +++ /dev/null @@ -1,2 +0,0 @@ -This folder contains a small subset of tools, mainly mesh manipulations, -available in FlowVR. See http://flowvr.sf.net/ for the complete package. diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/flowvr/render/bbox.h b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/flowvr/render/bbox.h deleted file mode 100644 index 1886ab3fa4e..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/flowvr/render/bbox.h +++ /dev/null @@ -1,112 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR Render * -* Parallel Rendering Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 2005 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING-LIB file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: ./include/flowvr/render/bbox.h * -* * -* Contacts: * -* * -******************************************************************/ -#ifndef FLOWVR_RENDER_BBOX_H -#define FLOWVR_RENDER_BBOX_H - -#include -#include - -#include - -namespace flowvr -{ - -namespace render -{ - -class BBox -{ - public: - ftl::Vec3f a,b; - - BBox(); - BBox(const ftl::Vec3f& _a,const ftl::Vec3f& _b); - - bool isEmpty() const; - - void clear(); - - BBox& operator+=(const BBox& p); - - BBox& operator+=(const ftl::Vec3f& p); - - BBox operator+(const BBox& p); - - void apply(const BBox& p,const ftl::Mat4x4f& m); - - BBox operator*(const ftl::Mat4x4f& m); - - float size() const - { - if (isEmpty()) return 0; - ftl::Vec3f v=b-a; - return v.norm(); - } - - bool in(const ftl::Vec3f& p) const - { - return (p.x()>=a.x() && p.x()<=b.x() && p.y()>=a.y() && p.y()<=b.y() && p.z()>=a.z() && p.z()<=b.z()); - } - - bool in(const BBox& bb) const - { - return (bb.b.x()>=a.x() && bb.a.x()<=b.x() && bb.b.y()>=a.y() && bb.a.y()<=b.y() && bb.b.z()>=a.z() && bb.a.z()<=b.z()); - } - - /// Test if bbox intersect with half-space defined as eq.P >= 0 - bool inHalfspace(const ftl::Vec4f& eq) const; - - /// Test if bbox intersect with frustum defined by the given matrix - /// (interpreted as an OpenGL projection matrix) - /// if exact is false then the result is overestimated - /// if exact is true then a 3x3 matrix inversion is required - bool inFrustum(const ftl::Mat4x4f& frustum, bool exact=false) const; - - bool operator==(const BBox& p) const - { - return a==p.a && b==p.b; - } - - bool operator!=(const BBox& p) const - { - return a!=p.a || b!=p.b; - } - - std::string toString() const; - void fromString(const std::string& text); - -}; - -} // namespace render - -} // namespace flowvr - -// iostream -std::ostream& operator<<(std::ostream& o, const flowvr::render::BBox& b); -std::istream& operator>>(std::istream& in, flowvr::render::BBox& b); - -#endif diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/flowvr/render/mesh.h b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/flowvr/render/mesh.h deleted file mode 100644 index 4e1a6f9463d..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/flowvr/render/mesh.h +++ /dev/null @@ -1,426 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR Render * -* Parallel Rendering Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 2005 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING-LIB file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: ./include/flowvr/render/mesh.h * -* * -* Contacts: * -* * -******************************************************************/ -#ifndef FLOWVR_RENDER_MESH_H -#define FLOWVR_RENDER_MESH_H - -#define MINI_FLOWVR - -#include -#include -#include "bbox.h" -#ifndef MINI_FLOWVR -#include "chunkwriter.h" -#endif - -#include -#include -#include - -namespace flowvr -{ - -namespace render -{ - -using namespace ftl; - -/// Mesh container helper class -class Mesh -{ -public: - enum { MESH_MAGIC = 0x003E583E, MESH_MAGIC_SWAP = 0x3E583E00 }; - - enum Attrib { - MESH_POINTS_POSITION=1<<0, - MESH_POINTS_TEXCOORD=1<<1, - MESH_POINTS_NORMAL =1<<2, - MESH_POINTS_COLOR =1<<3, - MESH_POINTS_VALUE =1<<4, - MESH_POINTS_GROUP =1<<5, - MESH_FACES =1<<6, - MESH_DISTMAP =1<<7, - MESH_VOXEL =1<<8, - MESH_MATERIALS =1<<9, - }; - - int attrib; - - bool getAttrib(Attrib a) const { return (attrib&(a))!=0; } - void setAttrib(Attrib a, bool b) { if(b) attrib|=(a); else attrib&=~(a); } - - struct Vertex //__attribute__((packed)) - { - Vec3f p; - Vec2f t; - Vec3f n; - Vec4b c; - float v; - void operator+=(const Vertex& b) - { - p+=b.p; - t+=b.t; - n+=b.n; - c+=b.c; - v+=b.v; - } - void mean(int nb) - { - p/=(float)nb; - t/=(float)nb; - n.normalize(); - c/=nb; - v/=nb; - } - void lerp(const Vertex& v1, float f1, const Vertex& v2, float f2) - { - p=v1.p*f1+v2.p*f2; - t=v1.t*f1+v2.t*f2; - n=v1.n*f1+v2.n*f2; n.normalize(); - for (int i=0;i<4;++i) c[i] = (unsigned char)((float)v1.c[i]*f1+(float)v2.c[i]*f2); - v=v1.v*f1+v2.v*f2; - } - void lerp(const Vertex& v1, float f1, const Vertex& v2, float f2, const Vertex& v3, float f3) - { - p=v1.p*f1+v2.p*f2+v3.p*f3; - t=v1.t*f1+v2.t*f2+v3.t*f3; - n=v1.n*f1+v2.n*f2+v3.n*f3; n.normalize(); - for (int i=0;i<4;++i) c[i] = (unsigned char)((float)v1.c[i]*f1+(float)v2.c[i]*f2+(float)v3.c[i]*f3); - v=v1.v*f1+v2.v*f2+v3.v*f3; - } - void lerp(const Vertex& v1, float f1, const Vertex& v2, float f2, const Vertex& v3, float f3, const Vertex& v4, float f4) - { - p=v1.p*f1+v2.p*f2+v3.p*f3+v4.p*f4; - t=v1.t*f1+v2.t*f2+v3.t*f3+v4.t*f4; - n=v1.n*f1+v2.n*f2+v3.n*f3+v4.n*f4; n.normalize(); - for (int i=0;i<4;++i) c[i] = (unsigned char)((float)v1.c[i]*f1+(float)v2.c[i]*f2+(float)v3.c[i]*f3+(float)v4.c[i]*f4); - v=v1.v*f1+v2.v*f2+v3.v*f3+v4.v*f4; - } - }; - - std::vector points_p; - - Vec3f getPP(int i) const { if ((unsigned)i=points_p.size()) points_p.resize(i+1); return points_p[i]; } - - Vec2f getPT(int i) const { if ((unsigned)i=points_t.size()) points_t.resize(i+1); return points_t[i]; } - - Vec3f getPN(int i) const { if ((unsigned)i=points_n.size()) points_n.resize(i+1); return points_n[i]; } - - Vec4b getPC(int i) const { if ((unsigned)i=points_c.size()) points_c.resize(i+1); return points_c[i]; } - - float getPV(int i) const { if ((unsigned)i=points_v.size()) points_v.resize(i+1); return points_v[i]; } - - Vertex getP(int i) const { - Vertex v; - v.p = getPP(i); - v.t = getPT(i); - v.n = getPN(i); - v.c = getPC(i); - v.v = getPV(i); - return v; - } - - int addP(const Vertex& v, int g=-1) { - int i = nbp(); - PP(i) = v.p; - PN(i) = v.n; - if (getAttrib(MESH_POINTS_TEXCOORD)) PT(i) = v.t; - if (getAttrib(MESH_POINTS_COLOR )) PC(i) = v.c; - if (getAttrib(MESH_POINTS_VALUE )) PV(i) = v.v; - if (getAttrib(MESH_POINTS_GROUP )) - { - if (g==-1) g=nbg(); - PG(i) = g; - if (g>=nbg()) - GP0(g) = i; - } - return i; - } - - int getPG(int i) const { if ((unsigned)i=points_g.size()) points_g.resize(i+1); return points_g[i]; } - - int getGP0(int i) const { if ((unsigned)i=groups_p0.size()) groups_p0.resize(i+1); return groups_p0[i]; } - - Vec3i getFP(int i) const { if ((unsigned)i=faces_p.size()) faces_p.resize(i+1); return faces_p[i]; } - - Vec3f getFN(int i) const { if ((unsigned)i=faces_n.size()) faces_n.resize(i+1); return faces_n[i]; } - - Vec3f getFU(int i) const { if ((unsigned)i=faces_u.size()) faces_u.resize(i+1); return faces_u[i]; } - - Vec3f getFV(int i) const { if ((unsigned)i=faces_v.size()) faces_v.resize(i+1); return faces_v[i]; } - - int addF(Vec3i f) - { - int i = nbf(); - FP(i) = f; - if (!edges.empty()) - { - addEdgeFace(f[0],f[1],i); - addEdgeFace(f[1],f[2],i); - addEdgeFace(f[2],f[0],i); - } - return i; - } - - std::vector points_t; - std::vector points_n; - std::vector points_c; - std::vector points_v; - - std::vector points_g; - std::vector groups_p0; - -/* - struct Face //__attribute__((packed)) - { - int p[3]; - Vec3f n; - Vec3f u; // Vector to compute u barycentric coordinate - Vec3f v; // Vector to compute v barycentric coordinate - }; -*/ - std::vector faces_p; - std::vector faces_n; - std::vector faces_u; - std::vector faces_v; - - struct Edge - { - int f1,f2; - Edge() : f1(-1), f2(-1) {} - }; - - std::vector< std::map< int,Edge > > edges; - - struct DistMap - { - int nx; - int ny; - int nz; - float maxDist; - Mat4x4f mat; - float data[1]; - int nval() const { return nx*ny*nz; } - int size() const { return sizeof(DistMap)+(nval()-1)*sizeof(float); } - }; - - DistMap* distmap; - - struct Voxel - { - int nx; - int ny; - int nz; - Mat4x4f mat; - unsigned char data[1]; ///< bit-field containing the voxels, byte-aligned at each line - int size() const { return (sizeof(Voxel)-sizeof(unsigned char))+((nx+7)/8)*ny*nz; } - }; - - Voxel* voxel; - - struct Material - { - std::string mtllib; ///< Name of file containing material - std::string matname; ///< Name of material - Vec3f ambient, diffuse, specular, emmisive; - float alpha; - float shininess; - float optical_density; - std::string map_ambient, map_diffuse, map_specular, map_emmisive, map_alpha; - std::string map_bump; - - Material() - : alpha(1), shininess(0), optical_density(1) - { - } - }; - - struct MaterialGroup - { - std::string gname; ///< Name of group of faces using this material - std::string matname; ///< Name of material - Material* mat; ///< Material itself - int f0, nbf; ///< List of faces in this material group - MaterialGroup() - : mat(NULL), f0(0), nbf(0) - { - } - }; - - std::map mat_map; - - Material* getM(const std::string& name) - { - std::map::iterator it = mat_map.find(name); - if (it == mat_map.end()) return NULL; - else return it->second; - } - - std::vector mat_groups; - - BBox bb; - - int nbp() const { return (int)points_p.size(); } - int nbf() const { return (int)faces_p.size(); } - int nbg() const { return (int)groups_p0.size(); } - - int nbmatg() const { return (int)mat_groups.size(); } - - Mesh(); - - ~Mesh(); - - void operator=(const Mesh& mesh); - - void clear(); - void init(int nbp, int nbf, int attribs=MESH_POINTS_POSITION|MESH_FACES); - - bool load(const char* filename); - bool loadMesh(const char* filename); - bool loadObj(const char* filename, const char* filter=NULL); - bool loadOff(const char* filename); - bool loadVtk(const char* filename); - bool loadLwo(const char* filename); - bool save(const char* filename) const; - bool saveMesh(const char* filename) const; - bool saveObj(const char* filename) const; - bool saveOff(const char* filename) const; - bool saveStl(const char* filename) const; - bool saveVtk(const char* filename, bool binary = true) const; - bool saveLwo(const char* filename) const; - bool saveObj(FILE* f, int &v0, int &vn0, int &vt0) const; - bool saveObj(FILE* f, int &v0) const - { - int vn0=1; - int vt0=1; - return saveObj(f,v0,vn0,vt0); - } - bool saveObj(FILE* f) const - { - int v0=1; - return saveObj(f,v0); - } - -#ifndef MINI_FLOWVR - /// Output the mesh - void writeMesh(ChunkRenderWriter* scene, ID idIB, ID idVB, int gen=-1) const; - void writeParams(ChunkRenderWriter* scene, ID idP, ID idIB, ID idVB) const; -#endif - - /// Compute face normals and points normals (if not already specified or if force is true) - void calcNormals(bool force=false); - - /// Compute mesh bounding box - BBox calcBBox(); - - /// Compute bounding box of a submesh - BBox calcBBox(int f0, int nbf); - - /// Compute bounding box of a material group - BBox calcBBox(int matg) - { - return calcBBox(mat_groups[matg].f0, mat_groups[matg].nbf); - } - - /// Compute edges data - void calcEdges(); - - /// Flip all faces - void flipAll(); - - /// Flip faces not correctly oriented - void calcFlip(); - - /// Check if the mesh is closed - bool isClosed(); - - /// Close the mesh, creating intermediate vertices no further apart than given dist. - /// Note: the mesh must be correctly flipped - void closeDist(float dist); - - /// Close the mesh. Note: the mesh must be correctly flipped - void close(); - - /// Brute-force distance computation. Note: the mesh must be closed - float calcDist(Vec3f pos) const; - - template - void calcExtDistMap(Mat4x4f mat, Real* dest, int nx, int ny, int nz, float maxDist, float fact = 1); - - void calcDistMap(Mat4x4f mat, int nx, int ny, int nz, float maxDist); - - void calcDistMap(int nx, int ny, int nz, float maxDist); - - void calcDistMap(int n, float maxDist); - - void translate(Vec3f d); - - void dilate(float dist); - - /// Optimize mesh by merging identical groups and reordering faces - void optimize(); - - /// Merge vertices closer than the given distance - void mergeVertices(float dist); - - /// Return true if some materials requires tangent vectors - bool needTangents() const; - - /// Compute the tangent and co-tangent at each point - void calcTangents(std::vector& tangent1, std::vector& tangent2) const; - - /// Compute the tangent at each point, plus a fourth coordinate indicating the direction of the co-tangent. - /// See http://www.terathon.com/code/tangent.php - void calcTangents(std::vector& tangent) const; - - int getEdgeFace(int p0, int p1); - -protected: - - void clearEdges(); - - bool addEdgeFace(int p0, int p1, int f); - - void closeLoop(const std::vector& loop, float dist); - -}; - -} // namespace render - -} // namespace flowvr - -#endif diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/flowvr/render/mesh.inl b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/flowvr/render/mesh.inl deleted file mode 100644 index 276a1aa243e..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/flowvr/render/mesh.inl +++ /dev/null @@ -1,59 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR Render * -* Parallel Rendering Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 2005 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING-LIB file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: ./include/flowvr/render/mesh.h * -* * -* Contacts: * -* * -******************************************************************/ -#ifndef FLOWVR_RENDER_MESH_INL -#define FLOWVR_RENDER_MESH_INL - -namespace flowvr -{ - -namespace render -{ - -template -void Mesh::calcExtDistMap(Mat4x4f mat, Real* dest, int nx, int ny, int nz, float /*maxDist*/, float fact) -{ - close(); // make sure the mesh is closed - calcNormals(true); - calcEdges(); - for (int z=0;z -#include -#include - -inline int rfloor(double r) -{ - static const double FLOATTOINTCONST=(1.5*(1LL<<(52-16))); - union { double d; - int i; - } temp; - temp.d = r+FLOATTOINTCONST; - return ((temp.i)>>16); -} - -inline int rnear(double r) -{ - static const double FLOATTOINTCONST_0_5=(1.5*(1LL<<(52-16)))+0.5; - union { double d; - int i; - } temp; - temp.d = r+FLOATTOINTCONST_0_5; - return ((temp.i)>>16); -} - -inline int rceil(double r) -{ - return -rfloor(-r); -} - -inline float rabs(float r) -{ - return (r>=0)?r:-r; -} - -// clamp x to be between a and b -inline float rclamp(float x, float a, float b) -{ - return (x < a ? a : (x > b ? b : x)); -} - -inline float rrand() -{ - return (rand() / (float) RAND_MAX); -} - -// a nice piecewise-cubic spline function, defined between 0.0<=x<=2.0 -// approximates a windowed sinc function - negative lobes (sharpens slightly) -// using a = -0.75 = constraint parameter -0.5<=a<=-1.0 recommended -inline float cubic(float x) -{ - const float a = -0.75; - double w; - - if ((x >= 0.0) && (x < 1.0)) { - // Over [0,1) interval - // (a+2)x^3 - (a+3)x^2 + 1 - w = ((a + 2.0) * x - (a + 3.0)) * x * x + 1.0; - } else if ((x >= 1.0) && (x <= 2.0)) { - // Over [1,2] interval - // ax^3 - 5ax^2 + 8ax - 4a - w = ((a * x - 5.0 * a) * x + 8.0 * a) * x - 4.0 * a; - } else { - w = 0.0; - } - return (float)w; -} - -// 1-dimensional cubic interpolation -// not very fast, but easy to understand -// 0<=x<=1 -inline float cubicFilter4f(float x, float vm1, float v, float vp1, float vp2) -{ - return vm1 * cubic(1.0f+x) + - v * cubic(x) + - vp1 * cubic(1.0f-x) + - vp2 * cubic(2.0f-x); -} - -inline float cubicFilter4fv(float x, float *v) -{ - return v[0] * cubic(1.0f+x) + - v[1] * cubic(x) + - v[2] * cubic(1.0f-x) + - v[3] * cubic(2.0f-x); -} - -/* - 1D cubic interpolator (this is just a faster version of the above) - assumes x > 0.0 - constraint parameter = -1 -*/ -inline float cubicFilterFast4fv(float x, float *i) -{ - float x1, x2, x3; - float h1, h2, h3, h4; - float result; - - x1 = x + 1.0f; - x2 = x1 * x1; - x3 = x2 * x1; - h4 = -x3 + 5 * x2 - 8 * x1 + 4; - result = h4 * i[0]; - - x1 = x; - x2 = x1 * x1; - x3 = x2 * x1; - h1 = x3 - 2 * x2 + 1; - result += h1 * i[1]; - - x1 = 1.0f - x; - x2 = x1 * x1; - x3 = x2 * x1; - h2 = x3 - 2 * x2 + 1; - result += h2 * i[2]; - - x1 = 2.0f - x; - x2 = x1 * x1; - x3 = x2 * x1; - h3 = -x3 + 5 * x2 - 8 * x1 + 4; - result += h3 * i[3]; - - return result; -} - -class Noise2D -{ -public: - float* rdata; - int rsize; - - Noise2D(int size=64) - { - rsize = 1; - while (rsize < size) rsize<<=1; - rdata = new float[rsize*rsize]; - for (int y=0;y -#include -#include -#include -#include - -namespace ftl -{ - -/** Base Class for describing an option. Should not be used directly. - */ -class BaseOption -{ -public: - enum ArgType { - NO_ARG=0, - REQ_ARG=1, - OPT_ARG=2 }; - - const char* longname; - char shortname; - const char* description; - ArgType arg; bool hasdefault; - - /// Counts the number of times this option was given. - int count; - - BaseOption(const char* _longname, char _shortname, const char* _description, ArgType _arg); - virtual ~BaseOption(); - virtual bool set() { ++count; return true; } - virtual bool set(const char* /* argval */) { ++count; return true; } - virtual std::string help() - { - if (description!=NULL) return std::string(description); - else return std::string(); - } -}; - -/** Option of type T (int, string, ...). - */ -template -class Option : public BaseOption -{ -public: - /** Declaration of an option. - * @param _longname Long name version (--) if not NULL. - * @param _shortname Short name version (single character). - * @param _description Text description of the option. - * @param _val Pointer to a space where to store the option value. - * @param optional Indicates whether the argument is required. - */ - Option(const char* _longname, char _shortname, const char* _description, T* _val, bool optional=false) - : BaseOption(_longname, _shortname, _description, optional?BaseOption::OPT_ARG:BaseOption::REQ_ARG) - { - val = _val; - hasdefault = true; - } - - /** Declaration of an option. - * @param _longname Long name version (--) if not NULL. - * @param _shortname Short name version (single character). - * @param _description Text description of the option. - * @param optional Indicates whether the argument is required. - */ - Option(const char* _longname, char _shortname, const char* _description, bool optional=false) - : BaseOption(_longname, _shortname, _description, optional?BaseOption::OPT_ARG:BaseOption::REQ_ARG) - { - val = new T(); - hasdefault = false; - } - - /** Declaration of an option. - * @param _longname Long name version (--) if not NULL. - * @param _shortname Short name version (single character). - * @param _description Text description of the option. - * @param defaultval Default value if no argument is given. - * @param optional Indicates whether the argument is required. - */ - Option(const char* _longname, char _shortname, const char* _description, const T& defaultval, bool optional=true) - : BaseOption(_longname, _shortname, _description, optional?BaseOption::OPT_ARG:BaseOption::REQ_ARG) - { - val = new T(defaultval); - hasdefault = true; - } - - /// Retrieve the value of the option. - operator T&() { return *val; } - operator const T&() const { return *val; } - - T& value() { return *val; } - const T& value() const { return *val; } - - /// Set a default value. - void operator=(const T& v) - { - *val = v; - hasdefault = true; - } - - std::string type() const; - std::string defaultval() const; - - /// Set the value of the option from a text. - virtual bool set(const char* argval) - { - if (! ftl::Type::assignString(*val, std::string(argval))) - return false; - else - return BaseOption::set(argval); - } - - /// Build the help line concerning this option. - virtual std::string help() - { - std::ostringstream ss; - ss << " "; - if (shortname!='\0') - { - ss << '-' << shortname; - if (arg == BaseOption::OPT_ARG) - ss << " [" << type() << ']'; - else if (arg == BaseOption::REQ_ARG) - ss << " " << type(); - - if (longname!=NULL && longname[0]!='\0') - ss << " or "; - } - if (longname!=NULL && longname[0]!='\0') - { - ss << "--" << longname; - if (arg == BaseOption::OPT_ARG) - ss << " [" << type() << ']'; - else if (arg == BaseOption::REQ_ARG) - ss << " " << type(); - } - - ss << "\n "; - if (description!=NULL) - ss << description; - if (arg != BaseOption::NO_ARG && hasdefault) - ss << " (default "< -inline std::string Option::type() const -{ - return ftl::Type::name(ftl::Type::get(*val)); -} - -template <> -inline std::string Option::type() const -{ - return ftl::Type::name(ftl::Type::String0); -} - -template -inline std::string Option::defaultval() const -{ - std::string res; - ftl::Type::Assign::do_assign(res,ftl::Type::get(*val),(const void*)val); - return res; -} - -template <> -inline std::string Option::defaultval() const -{ - return *val; -} - -/** Specialization of Option for booleans (flags). - */ -template<> -class Option : public BaseOption -{ -public: - /** Declaration of an option. - * @param _longname Long name version (--) if not NULL. - * @param _shortname Short name version (single character). - * @param _description Text description of the option. - * @param _val Pointer to a space where to store the option value. - */ - Option(const char* _longname, char _shortname, const char* _description, bool* _val) - : BaseOption(_longname, _shortname, _description, BaseOption::NO_ARG) - { - val = _val; - } - - /** Declaration of an option. - * @param _longname Long name version (--) if not NULL. - * @param _shortname Short name version (single character). - * @param _description Text description of the option. - */ - Option(const char* _longname, char _shortname, const char* _description) - : BaseOption(_longname, _shortname, _description, BaseOption::NO_ARG) - { - val = new bool(false); - } - - /// Retrieve the value of the option ( - operator bool&() { return *val; } - operator const bool&() const { return *val; } - - bool& value() { return *val; } - const bool& value() const { return *val; } - - void operator=(const bool& v) - { - *val = v; - } - - virtual bool set() - { - *val = true; - return BaseOption::set(); - } - - virtual std::string help() - { - std::ostringstream ss; - ss << " "; - if (shortname!='\0') - { - ss << '-' << shortname; - if (longname!=NULL && longname[0]!='\0') - ss << " or "; - } - if (longname!=NULL && longname[0]!='\0') - { - ss << "--" << longname; - } - - ss << "\n "; - if (description!=NULL) - ss << description; - return ss.str(); - } - -protected: - bool* val; -}; - -/// A Flag option is a boolean option. -typedef Option FlagOption; - -/// Helper class to parse command line arguments -class CmdLine -{ -public: - - CmdLine(const char* _description=NULL); - - /// Adds a flag option. - void opt(const char* longname, char shortname, const char* description) - { - new FlagOption(longname, shortname, description); - } - - /// Adds a typed option. - template - void opt(const char* longname, char shortname, const char* description, T* val) - { - new Option(longname, shortname, description, val); - } - - /// Adds a typed option with specifying if an argument is required. - template - void opt(const char* longname, char shortname, const char* description, T* val, bool optional) - { - new Option(longname, shortname, description, val, optional); - } - - /** Parse the arguments. - * @param error Pointer to a boolean for retrieving if the command line has errors. - * @return True if the program should continue, false if the program should end - * (either because the option --help was used or because an error occurred). - */ - bool parse(int argc, char** argv, bool* error=NULL); - - /// Remaining arguments. - std::vector args; - - /// Create usage help message. - std::string help(); - - /// Return an option given its longname (NULL if does not exists) - BaseOption* getOpt(const std::string longname_); - - /// Set or change the cmdline description using the string given in parameter - void setDesc(const std::string desc); - - /// List of available options (static for a program). - static std::vector& opts(); - - protected: - const char* description; -}; - -} // namespace ftl - -#endif - diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/crc.h b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/crc.h deleted file mode 100644 index 3fa532ce569..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/crc.h +++ /dev/null @@ -1,37 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR * -* Template Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 20054 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: include/ftl/vec.h * -* * -* Contacts: 20/09/2005 Clement Menier * -* * -******************************************************************/ -#ifndef FTL_CRC_H -#define FTL_CRC_H - -namespace ftl -{ - -extern unsigned int crc32MsbToLsb(const void *buf, unsigned int size); - -} // namespace ftl - -#endif diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/fixed_array.h b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/fixed_array.h deleted file mode 100644 index 792c8950cc9..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/fixed_array.h +++ /dev/null @@ -1,174 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR * -* Template Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 20054 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: include/ftl/fixed_array.h * -* * -* Contacts: 20/09/2005 Clement Menier * -* * -******************************************************************/ -/* The following code declares class array, - * an STL container (as wrapper) for arrays of constant size. - * - * See - * http://www.josuttis.com/cppcode - * for details and the latest version. - * - * (C) Copyright Nicolai M. Josuttis 2001. - * Permission to copy, use, modify, sell and distribute this software - * is granted provided this copyright notice appears in all copies. - * This software is provided "as is" without express or implied - * warranty, and with no claim as to its suitability for any purpose. - * - * 29 Jun 2005 - remove boost includes and reverse iterators. (Jeremie Allard) - * 23 Aug 2002 - fix for Non-MSVC compilers combined with MSVC libraries. - * 05 Aug 2001 - minor update (Nico Josuttis) - * 20 Jan 2001 - STLport fix (Beman Dawes) - * 29 Sep 2000 - Initial Revision (Nico Josuttis) - */ - -// See http://www.boost.org/libs/array for Documentation. - -#ifndef FTL_FIXED_ARRAY_H -#define FTL_FIXED_ARRAY_H - -#include -#include -#include -#include - -namespace ftl -{ - -template -class fixed_array { -public: - T elems[N]; // fixed-size array of elements of type T - -public: - // type definitions - typedef T value_type; - typedef T* iterator; - typedef const T* const_iterator; - typedef T& reference; - typedef const T& const_reference; - typedef std::size_t size_type; - typedef std::ptrdiff_t difference_type; - - // iterator support - iterator begin() { return elems; } - const_iterator begin() const { return elems; } - iterator end() { return elems+N; } - const_iterator end() const { return elems+N; } - - // operator[] - reference operator[](size_type i) { return elems[i]; } - const_reference operator[](size_type i) const { return elems[i]; } - - // at() with range check - reference at(size_type i) { rangecheck(i); return elems[i]; } - const_reference at(size_type i) const { rangecheck(i); return elems[i]; } - - // front() and back() - reference front() { return elems[0]; } - const_reference front() const { return elems[0]; } - reference back() { return elems[N-1]; } - const_reference back() const { return elems[N-1]; } - - // size is constant - static size_type size() { return N; } - static bool empty() { return false; } - static size_type max_size() { return N; } - enum { static_size = N }; - - // swap (note: linear complexity) - void swap (fixed_array& y) - { - std::swap_ranges(begin(),end(),y.begin()); - } - - // direct access to data - const T* data() const { return elems; } - - // assignment with type conversion - template - fixed_array& operator= (const fixed_array& rhs) - { - std::copy(rhs.begin(),rhs.end(), begin()); - return *this; - } - - // assign one value to all elements - void assign (const T& value) - { - std::fill_n(begin(),size(),value); - } - -private: - - // check range (may be private because it is static) - static void rangecheck (size_type i) { - if (i >= size()) { throw std::range_error("fixed_array"); } - } - -}; - -// comparisons -template -bool operator== (const fixed_array& x, const fixed_array& y) -{ - return std::equal(x.begin(), x.end(), y.begin()); -} -template -bool operator< (const fixed_array& x, const fixed_array& y) -{ - return std::lexicographical_compare(x.begin(),x.end(),y.begin(),y.end()); -} -template -bool operator!= (const fixed_array& x, const fixed_array& y) -{ - return !(x==y); -} -template -bool operator> (const fixed_array& x, const fixed_array& y) -{ - return y -bool operator<= (const fixed_array& x, const fixed_array& y) -{ - return !(y -bool operator>= (const fixed_array& x, const fixed_array& y) -{ - return !(x -inline void swap (fixed_array& x, fixed_array& y) -{ - x.swap(y); -} - -} // namespace ftl - -#endif diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/mat.h b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/mat.h deleted file mode 100644 index d0b0edb6b8a..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/mat.h +++ /dev/null @@ -1,704 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR * -* Template Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 20054 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: include/ftl/mat.h * -* * -* Contacts: 20/09/2005 Clement Menier * -* * -******************************************************************/ -#ifndef FTL_MAT_H -#define FTL_MAT_H - -#include "vec.h" - -namespace ftl -{ - -template -class Mat : public fixed_array,L> -{ - public: - - static Type::Type getType() { return (Type::Type)Type::matrix(Type::get(real()),C,L); } - - enum { N = L*C }; - - typedef Vec Line; - typedef Vec Col; - - Mat() - { - } - - /// Specific constructor with a single line. - Mat(Line r1) - { - static_assert(L == 1, ""); - this->elems[0]=r1; - } - - /// Specific constructor with 2 lines. - Mat(Line r1, Line r2) - { - static_assert(L == 2, ""); - this->elems[0]=r1; - this->elems[1]=r2; - } - - /// Specific constructor with 3 lines. - Mat(Line r1, Line r2, Line r3) - { - static_assert(L == 3, ""); - this->elems[0]=r1; - this->elems[1]=r2; - this->elems[2]=r3; - } - - /// Specific constructor with 4 lines. - Mat(Line r1, Line r2, Line r3, Line r4) - { - static_assert(L == 4, ""); - this->elems[0]=r1; - this->elems[1]=r2; - this->elems[2]=r3; - this->elems[3]=r4; - } - - /// Constructor from an array of elements (stored per line). - template - explicit Mat(const real2* p) - { - std::copy(p, p+N, this->begin()->begin()); - } - - /// Assignment from an array of elements (stored per line). - void operator=(const real* p) - { - std::copy(p, p+N, this->begin()->begin()); - } - - /// Assignment from a matrix of different size. - template void operator=(const Mat& m) - { - std::copy(m.begin(), m.begin()+(L>L2?L2:L), this->begin()); - } - - /// Sets each element to 0. - void clear() - { - for (int i=0;ielems[i].clear(); - } - - /// Sets each element to r. - void fill(real r) - { - for (int i=0;ielems[i].fill(r); - } - - /// Read-only access to line i. - const Line& line(int i) const - { - return this->elems[i]; - } - - /// Copy of column j. - Col col(int j) const - { - Col c; - for (int i=0;ielems[i][j]; - return c; - } - - /// Write access to line i. - Line& operator[](int i) - { - return this->elems[i]; - } - - /// Read-only access to line i. - const Line& operator[](int i) const - { - return this->elems[i]; - } - - /// Write access to line i. - Line& operator()(int i) - { - return this->elems[i]; - } - - /// Read-only access to line i. - const Line& operator()(int i) const - { - return this->elems[i]; - } - - /// Write access to element (i,j). - real& operator()(int i, int j) - { - return this->elems[i][j]; - } - - /// Read-only access to element (i,j). - const real& operator()(int i, int j) const - { - return this->elems[i][j]; - } - - /// Cast into a standard C array of lines (read-only). - const Line* lptr() const - { - return this->elems; - } - - /// Cast into a standard C array of lines. - Line* lptr() - { - return this->elems; - } - - /// Cast into a standard C array of elements (stored per line) (read-only). - const real* ptr() const - { - return this->elems[0].ptr(); - } - - /// Cast into a standard C array of elements (stored per line). - real* ptr() - { - return this->elems[0].ptr(); - } - - /// Special access to first line. - Line& x() { static_assert(L >= 1, ""); return this->elems[0]; } - /// Special access to second line. - Line& y() { static_assert(L >= 2, ""); return this->elems[1]; } - /// Special access to third line. - Line& z() { static_assert(L >= 3, ""); return this->elems[2]; } - /// Special access to fourth line. - Line& w() { static_assert(L >= 4, ""); return this->elems[3]; } - - /// Special access to first line (read-only). - const Line& x() const { static_assert(L >= 1, ""); return this->elems[0]; } - /// Special access to second line (read-only). - const Line& y() const { static_assert(L >= 2, ""); return this->elems[1]; } - /// Special access to third line (read-only). - const Line& z() const { static_assert(L >= 3, ""); return this->elems[2]; } - /// Special access to fourth line (read-only). - const Line& w() const { static_assert(L >= 4, ""); return this->elems[3]; } - - /// Set matrix to identity. - void identity() - { - static_assert(L == C, ""); - clear(); - for (int i=0;ielems[i][i]=1; - } - - /// Set matrix as the transpose of m. - void transpose(const Mat &m) - { - for (int i=0;ielems[i][j]=m[j][i]; - } - - /// Transpose current matrix. - void transpose() - { static_assert(L == C, ""); - for (int i=0;ielems[i][j]; - this->elems[i][j] = this->elems[j][i]; - this->elems[j][i] = t; - } - } - - // LINEAR ALGEBRA - - /// Matrix multiplication operator. - template - Mat operator*(const Mat& m) const - { - Mat r; - for(int i=0;i operator+(const Mat& m) const - { - Mat r; - for(int i = 0; i < L; i++) - r[i] = (*this)[i] + m[i]; - return r; - } - - /// Matrix subtraction operator. - Mat operator-(const Mat& m) const - { - Mat r; - for(int i = 0; i < L; i++) - r[i] = (*this)[i] - m[i]; - return r; - } - - /// Multiplication operator Matrix * Column. - Col operator*(const Line& v) const - { - Col r; - for(int i=0;i operator*(real f) const - { - Mat r; - for(int i=0;ielems[i]*=r; - } - - /// Addition assignment operator. - void operator +=(const Mat& m) - { - for(int i=0;ielems[i]+=m[i]; - } - - /// Determinant of the matrix. - /// @ATTENTION: determinant currently only implemented for 3x3 matrices. - real determinant() const - { - static_assert(L == 3, ""); - static_assert(C == 3, ""); - return (*this)(0,0)*(*this)(1,1)*(*this)(2,2) - + (*this)(1,0)*(*this)(2,1)*(*this)(0,2) - + (*this)(2,0)*(*this)(0,1)*(*this)(1,2) - - (*this)(0,0)*(*this)(2,1)*(*this)(1,2) - - (*this)(1,0)*(*this)(0,1)*(*this)(2,2) - - (*this)(2,0)*(*this)(1,1)*(*this)(0,2); - } - - /// Invert matrices and stores the result in m. - bool invert(const Mat& m) - { - return invertMatrix(*this,m); - } - -}; - -/// Matrix inversion (general case). -template -extern inline bool invertMatrix(Mat& dest, const Mat& from) -{ - int i, j, k; - Vec r, c, row, col; - - Mat m1 = from; - Mat m2; - m2.identity(); - - for ( k = 0; k < N; k++ ) - { - // Choosing the pivot - real pivot = 0; - for (i = 0; i < N; i++) - { - if (row[i]) - continue; - for (j = 0; j < N; j++) - { - if (col[j]) - continue; - real t = m1[i][j]; if (t<0) t=-t; - if ( t > pivot) - { - pivot = t; - r[k] = i; - c[k] = j; - } - } - } - - if (pivot <= 1e-10) - { - return false; - } - - row[r[k]] = col[c[k]] = 1; - pivot = m1[r[k]][c[k]]; - - // Normalization - m1[r[k]] /= pivot; m1[r[k]][c[k]] = 1; - m2[r[k]] /= pivot; - - // Reduction - for (i = 0; i < N; i++) - { - if (i != r[k]) - { - real f = m1[i][c[k]]; - m1[i] -= m1[r[k]]*f; m1[i][c[k]] = 0; - m2[i] -= m2[r[k]]*f; - } - } - } - - for (i = 0; i < N; i++) - for (j = 0; j < N; j++) - if (c[j] == i) - row[i] = r[j]; - - for ( i = 0; i < N; i++ ) - dest[i] = m2[row[i]]; - - return true; -} - -/// Matrix inversion (special case 3x3). -template -extern inline bool invertMatrix(Mat<3,3,real>& dest, const Mat<3,3,real>& from) -{ - real det=from.determinant(); - - if ( -1e-10<=det && det<=1e-10) - return false; - - dest(0,0)= (from(1,1)*from(2,2) - from(2,1)*from(1,2))/det; - dest(1,0)= (from(1,2)*from(2,0) - from(2,2)*from(1,0))/det; - dest(2,0)= (from(1,0)*from(2,1) - from(2,0)*from(1,1))/det; - dest(0,1)= (from(2,1)*from(0,2) - from(0,1)*from(2,2))/det; - dest(1,1)= (from(2,2)*from(0,0) - from(0,2)*from(2,0))/det; - dest(2,1)= (from(2,0)*from(0,1) - from(0,0)*from(2,1))/det; - dest(0,2)= (from(0,1)*from(1,2) - from(1,1)*from(0,2))/det; - dest(1,2)= (from(0,2)*from(1,0) - from(1,2)*from(0,0))/det; - dest(2,2)= (from(0,0)*from(1,1) - from(1,0)*from(0,1))/det; - - return true; -} - -/// Matrix inversion (special case 2x2). -template -extern inline bool invertMatrix(Mat<2,2,real>& dest, const Mat<2,2,real>& from) -{ - real det=from(0,0)*from(1,1)-from(0,1)*from(1,0); - - if ( -1e-10<=det && det<=1e-10) - return false; - - dest(0,0)= from(1,1)/det; - dest(0,1)= -from(0,1)/det; - dest(1,0)= -from(1,0)/det; - dest(1,1)= from(0,0)/det; - - return true; -} - -// Special operations for transformation matrices - -template -extern inline Vec<3,real> transform(const Mat<3,4,real2>& mat, const Vec<3,real>& p) -{ - return Vec<3,real>( - mat[0][0]*p[0] + mat[0][1]*p[1] + mat[0][2]*p[2] + mat[0][3], - mat[1][0]*p[0] + mat[1][1]*p[1] + mat[1][2]*p[2] + mat[1][3], - mat[2][0]*p[0] + mat[2][1]*p[1] + mat[2][2]*p[2] + mat[2][3] - ); -} - -template -extern inline Vec<3,real> transform(const Mat<4,4,real>& mat, const Vec<3,real>& p) -{ - return Vec<3,real>( - mat[0][0]*p[0] + mat[0][1]*p[1] + mat[0][2]*p[2] + mat[0][3], - mat[1][0]*p[0] + mat[1][1]*p[1] + mat[1][2]*p[2] + mat[1][3], - mat[2][0]*p[0] + mat[2][1]*p[1] + mat[2][2]*p[2] + mat[2][3] - ); -} - -template -extern inline Mat<3,4,real> transform(const Mat<3,4,real>& m1, const Mat<3,4,real>& m2) -{ - Mat<3,4,real> mat; - for(int i = 0; i < 3; i++) { - for(int j = 0; j < 4; j++) { - mat[i][j] = m1[i][0]*m2[0][j] + m1[i][1]*m2[1][j] + m1[i][2]*m2[2][j]; - } - mat[i][3] += m1[i][3]; - } - return mat; -} - -template -extern inline Vec<3,real> transformv(const Mat<3,4,real>& mat, const Vec<3,real>& p) -{ - return Vec<3,real>( - mat[0][0]*p[0] + mat[0][1]*p[1] + mat[0][2]*p[2], - mat[1][0]*p[0] + mat[1][1]*p[1] + mat[1][2]*p[2], - mat[2][0]*p[0] + mat[2][1]*p[1] + mat[2][2]*p[2] - ); -} - -template -extern inline Vec<3,real> transformv(const Mat<4,4,real>& mat, const Vec<3,real>& p) -{ - return Vec<3,real>( - mat[0][0]*p[0] + mat[0][1]*p[1] + mat[0][2]*p[2], - mat[1][0]*p[0] + mat[1][1]*p[1] + mat[1][2]*p[2], - mat[2][0]*p[0] + mat[2][1]*p[1] + mat[2][2]*p[2] - ); -} - -typedef Mat<2,2,unsigned char> Mat2x2b; -typedef Mat<2,2,int> Mat2x2i; -typedef Mat<2,2,float> Mat2x2f; -typedef Mat<2,2,double> Mat2x2d; - -typedef Mat<2,3,unsigned char> Mat2x3b; -typedef Mat<2,3,int> Mat2x3i; -typedef Mat<2,3,float> Mat2x3f; -typedef Mat<2,3,double> Mat2x3d; - -typedef Mat<2,4,unsigned char> Mat2x4b; -typedef Mat<2,4,int> Mat2x4i; -typedef Mat<2,4,float> Mat2x4f; -typedef Mat<2,4,double> Mat2x4d; - -typedef Mat<3,2,unsigned char> Mat3x2b; -typedef Mat<3,2,int> Mat3x2i; -typedef Mat<3,2,float> Mat3x2f; -typedef Mat<3,2,double> Mat3x2d; - -typedef Mat<3,3,unsigned char> Mat3x3b; -typedef Mat<3,3,int> Mat3x3i; -typedef Mat<3,3,float> Mat3x3f; -typedef Mat<3,3,double> Mat3x3d; - -typedef Mat<3,4,unsigned char> Mat3x4b; -typedef Mat<3,4,int> Mat3x4i; -typedef Mat<3,4,float> Mat3x4f; -typedef Mat<3,4,double> Mat3x4d; - -typedef Mat<4,2,unsigned char> Mat4x2b; -typedef Mat<4,2,int> Mat4x2i; -typedef Mat<4,2,float> Mat4x2f; -typedef Mat<4,2,double> Mat4x2d; - -typedef Mat<4,3,unsigned char> Mat4x3b; -typedef Mat<4,3,int> Mat4x3i; -typedef Mat<4,3,float> Mat4x3f; -typedef Mat<4,3,double> Mat4x3d; - -typedef Mat<4,4,unsigned char> Mat4x4b; -typedef Mat<4,4,int> Mat4x4i; -typedef Mat<4,4,float> Mat4x4f; -typedef Mat<4,4,double> Mat4x4d; - -// Creation of transformation matrices - -Mat4x4f matrixTranslation(const Vec3f& pos); - -Mat4x4f matrixScale(const Vec3f& scale); -Mat4x4f matrixScale(const float scale); - -class Quat; -Mat4x4f matrixRotation(const Quat& rot); -Mat4x4f matrixRotation(float ang, const Vec3f& axis); - -Mat4x4f matrixTransform(const Vec3f& pos, const Quat& rot, const Vec3f& scale); -Mat4x4f matrixTransform(const Vec3f& pos, const Quat& rot, float scale=1.0f); -Mat4x4f matrixTransform(const Vec3f& pos, float ang, const Vec3f& axis, const Vec3f& scale); -Mat4x4f matrixTransform(const Vec3f& pos, float ang, const Vec3f& axis, float scale=1.0f); -Mat4x4f matrixTransform(const Vec3f& pos, const Vec3f& scale); -Mat4x4f matrixTransform(const Vec3f& pos, float scale); -Mat4x4f matrixTransform(const Vec3f& pos); - -// Assignment from typed data - -namespace Type -{ - -//template -//inline Type get(const Mat& m) -//{ -// return (Type)matrix(get(m[0][0]),C,L); -//} - -template -class Assign< Mat > -{ -public: -static bool do_assign(Mat& dest, int type, const void* data) -{ - if (isString(type)) - { - const char* src = (const char*) data; - const char* end = src + size(type); - bool res = true; - int x = 0; - int y = 0; - while (y < L && src < end) - { - if (*src==' ' || *src==',') ++src; - else - { - const char* s0 = src; - if (x==0 && *s0 == '{') - { - ++s0; - if (y==0 && s00 && s0[s-1]=='}') - { - eol = true; - --s; - } - if (s>0) - { - res &= assign(dest[y][x], buildString(s), s0); - ++x; - } - if (eol || x == C) - { - x = 0; - ++y; - } - } - } - return res; - } - else if (isMatrix(type)) - { - int eSize = elemSize(type); - int eType = toSingle(type); - int c = nx(type); int r=0; - int l = ny(type); - if (c>C) { r=c-C; c=C; } - if (l>L) l=L; - bool res = true; - for (int y=0;y -std::ostream& operator<<(std::ostream& o, const ftl::Mat& m) -{ - o << '<' << m[0]; - for (int i=1; i'; - return o; -} - -template -std::istream& operator>>(std::istream& in, ftl::Mat& m) -{ - int c; - c = in.peek(); - while (c==' ' || c=='\n' || c=='<') - { - in.get(); - c = in.peek(); - } - in >> m[0]; - for (int i=1; i> m[i]; - } - c = in.peek(); - while (c==' ' || c=='\n' || c=='>') - { - in.get(); - c = in.peek(); - } - return in; -} - -/// Scalar matrix multiplication operator. -template -ftl::Mat operator*(real r, const ftl::Mat& m) -{ - return m*r; -} - -#endif diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/quat.h b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/quat.h deleted file mode 100644 index 24ccea1b7af..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/quat.h +++ /dev/null @@ -1,107 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR * -* Template Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 20054 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: include/ftl/quat.h * -* * -* Contacts: 20/09/2005 Clement Menier * -* * -******************************************************************/ -#ifndef FTL_QUAT_H -#define FTL_QUAT_H - -#include "vec.h" -#include "mat.h" - -namespace ftl -{ - -// Basic angle-related definitions - -static const double Pi = 3.1415926535897932384626433832795029; - -extern inline double RAD2DEG(double a) { return a*180/Pi; } -extern inline double DEG2RAD(double a) { return a*Pi/180; } - -/// Quaternion -class Quat -{ - public: - - float w,x,y,z; - - Quat() : w(1),x(0),y(0),z(0) {} - Quat(float _w,float _x,float _y,float _z) : w(_w),x(_x),y(_y),z(_z) {} - Quat(float _w,const Vec3f &_v) : w(_w), x(_v[0]),y(_v[1]),z(_v[2]) {} - - Quat & operator+=(const Quat &q); - Quat & operator-=(const Quat &q); - Quat & operator*=(const Quat &q); - //Quat & operator/=(Quat &q); - Quat & operator*=(float f); - Quat & operator/=(float f); - - Quat operator+(const Quat &q) const; - Quat operator-(const Quat &q) const; - Quat operator*(const Quat &q) const; - //Quat operator/(Quat &q); - Quat operator*(float f) const; - Quat operator/(float f) const; - - friend Quat operator*(float f,const Quat &q); - - Quat operator-() const; - Quat operator~() const; - - void fromAngAxis(float ang,Vec3f axis); - void toAngAxis(float *ang,Vec3f *axis) const; - - void fromDegreeAngAxis(float ang,Vec3f axis); - void toDegreeAngAxis(float *ang,Vec3f *axis) const; - - void fromMatrix(const Mat3x3f &m); - void toMatrix(Mat3x3f *m) const; - - float length() const; - void clear(); - void normalize(); - bool isIdentity() const; - -}; - -// Assignment from typed data - -namespace Type -{ - -// template<> extern inline Type get(const Quat&) { return Vec4f; } - -template <> -bool assign(Quat& dest, int type, const void* data); - -} // namespace Type - -} // namespace ftl - -std::ostream& operator<<(std::ostream& o, const ftl::Quat& q); - -std::istream& operator>>(std::istream& in, ftl::Quat& q); - -#endif diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/rmath.h b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/rmath.h deleted file mode 100644 index f72c2dd5332..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/rmath.h +++ /dev/null @@ -1,115 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR * -* Template Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 20054 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: include/ftl/vec.h * -* * -* Contacts: 06/22/2005 Jeremie Allard * -* * -******************************************************************/ -#ifndef FTL_RMATH_H -#define FTL_RMATH_H - -#include - -namespace ftl -{ - -/* -// Most portable version, but also the slowest -inline int rfloor(double r) -{ - return ((int)(r+1000000))-1000000; -} - -inline int rnear(double r) -{ - return ((int)(r+1000000.5))-1000000; -} -*/ - -/* -// Does not work on gcc 4.x due to more aggressive optimizations -inline int rfloor(double r) -{ - static const double FLOATTOINTCONST=(1.5*(1LL<<(52-16))); - r+=FLOATTOINTCONST; - return ((((int*)&r)[0])>>16); -} - -inline int rnear(double r) -{ - static const double FLOATTOINTCONST_0_5=(1.5*(1LL<<(52-16)))+0.5; - r+=FLOATTOINTCONST_0_5; - return ((((int*)&r)[0])>>16); -} -*/ - -// Works on gcc 3.x and 4.x -inline int rfloor(double r) -{ - static const double FLOATTOINTCONST=(1.5*(1LL<<(52-16))); - union { double d; int i; } temp; - temp.d = r+FLOATTOINTCONST; - return ((temp.i)>>16); -} - -inline int rnear(double r) -{ - static const double FLOATTOINTCONST_0_5=(1.5*(1LL<<(52-16)))+0.5; - union { double d; int i; } temp; - temp.d = r+FLOATTOINTCONST_0_5; - return ((temp.i)>>16); -} - - - -inline int rceil(double r) -{ - return -rfloor(-r); -} - -template -inline real rabs(real r) -{ - return (r>=0)?r:-r; -} - -template -inline real rmin(real r, real s) -{ - return (r<=s)?r:s; -} - -template -inline real rmax(real r, real s) -{ - return (r>s)?r:s; -} - -template -inline T rlerp(const T& a, const T& b, float f) -{ - return a+(b-a)*f; -} - -} // namespace ftl - -#endif diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/type.h b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/type.h deleted file mode 100644 index e9bca203297..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/type.h +++ /dev/null @@ -1,656 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR * -* Template Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 20054 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: include/ftl/type.h * -* * -* Contacts: 20/09/2005 Clement Menier * -* * -******************************************************************/ -#ifndef FTL_TYPE_H -#define FTL_TYPE_H - -#include -#include - -namespace ftl -{ - -namespace Type -{ - -/// size: 14 bits (total size) -/// elem: 4 bits (size of each element) -/// nx: 5 bits (number of columns-1) -/// ny: 5 bits (number of rows-1) -/// type: 2 bits -/// 0 = Special types (empty or delete if elem==0, string if elem==1, enum if elem==4, ID if elem==8) -/// 1 = RESERVED -/// 2 = Numerical types -/// 3 = Floating point types -/// endian: 2 bits -/// 00 = Don't care about endianness (empty) -/// 01 = Endianness OK -/// 10 = Endianness reversed -/// 11 = Special types (delete) - -/// The current bit allocation is: -/// bit 0 - 1 : type -/// bit 2 - 6 : nx -/// bit 7 : endian 1 -/// bit 8 - 12 : ny -/// bit 13 - 16 : elem -/// bit 17 - 30 : size -/// bit 31 : endian 2 - -enum -{ - - MASK_TYPE = 0x00000003, SHIFT_TYPE = 0, - MASK_NX = 0x0000007C, SHIFT_NX = 2, - MASK_NY = 0x00001F00, SHIFT_NY = 8, - MASK_ELEM = 0x0001E000, SHIFT_ELEM = 13, - MASK_SIZE = 0x7FFE0000, SHIFT_SIZE = 17, - MASK_ENDIAN = 0x80000080, - ENDIAN_OK = 0x00000080, - ENDIAN_SWAP = 0x80000000, - TYPE_SPECIAL = 0, - TYPE_RESERVED = 1 << SHIFT_TYPE, - TYPE_NUM = 2 << SHIFT_TYPE, - TYPE_REAL = 3 << SHIFT_TYPE, - -}; - -// This must be a macro to be able to initialize enum Type - -#define FLOWVR_TYPE2(type, elembits, nx, ny) \ - ( ENDIAN_OK | (type) | (((elembits)/8) << SHIFT_ELEM) \ - | (((nx)-1) << SHIFT_NX) | (((ny)-1) << SHIFT_NY) \ - | ((((elembits)*(nx)*(ny)+7)/8) << SHIFT_SIZE) ) - -#define FLOWVR_TYPE1(type, elembits, nx) FLOWVR_TYPE2(type, elembits, nx, 1) -#define FLOWVR_TYPE(type, elembits) FLOWVR_TYPE2(type, elembits, 1, 1) - -extern inline int build(int type, int elembits, int nx=1, int ny=1) -{ - return FLOWVR_TYPE2(type, elembits, nx, ny); -} - -extern inline int buildString(int len) -{ - return ENDIAN_OK | TYPE_SPECIAL | (1 << SHIFT_ELEM) - | ((len) << SHIFT_SIZE); -} - -extern inline int endianOk(int t) -{ - return (t & MASK_ENDIAN) != ENDIAN_SWAP; -} - -extern inline int type(int t) -{ - return (t & MASK_TYPE); -} - -extern inline int elemSize(int t) -{ - return (t & MASK_ELEM) >> SHIFT_ELEM; -} - -extern inline int size(int t) -{ - return (t & MASK_SIZE) >> SHIFT_SIZE; -} - -extern inline int nx(int t) -{ - return ((t & MASK_NX) >> SHIFT_NX)+1; -} - -extern inline int ny(int t) -{ - return ((t & MASK_NY) >> SHIFT_NY)+1; -} - -extern inline bool isSpecial(int t) -{ - return type(t) == TYPE_SPECIAL; -} - -extern inline bool isNum(int t) -{ - return type(t) == TYPE_NUM; -} - -extern inline bool isReal(int t) -{ - return type(t) == TYPE_REAL; -} - -extern inline bool isString(int t) -{ - return (t & (MASK_TYPE|MASK_ELEM)) == (TYPE_SPECIAL|(1<1 && ny(t)==1; -} - -extern inline bool isMatrix(int t) -{ - return ny(t)>1; -} - -extern inline int toSingle(int t) -{ - int e = elemSize(t); - if (e==0) e=1; - return ( t & ~(MASK_NX|MASK_NY|MASK_SIZE) ) | (e << SHIFT_SIZE); -} - -extern inline int toVector(int t) -{ - return ( t & ~(MASK_NY|MASK_SIZE) ) | ((size(t)/ny(t)) << SHIFT_SIZE); -} - -extern inline int vector(int elemtype, int nx) -{ - return (elemtype &~ (MASK_NX|MASK_NY|MASK_SIZE)) | ((nx-1)< -inline Type get(const T&) { return T::getType(); } - -template<> inline Type get(const bool&) { return Bool; } -template<> inline Type get(const char&) { return Byte; } -template<> inline Type get(const unsigned char&) { return Byte; } -template<> inline Type get(const short&) { return Short; } -template<> inline Type get(const unsigned short&) { return Short; } -template<> inline Type get(const int&) { return Int; } -template<> inline Type get(const unsigned int&) { return Int; } -template<> inline Type get(const long long&) { return QWord; } -template<> inline Type get(const unsigned long long&) { return QWord; } -template<> inline Type get(const float&) { return Float; } -template<> inline Type get(const double&) { return Double; } -template<> inline Type get(const std::string& s) { return (Type)buildString((int)s.length()); } - -std::string name(int t); -int fromName(const std::string& name); - -/// Set a variable to Null (0/empty) value -/// Default implementation: use operator=(0) -template -bool assignNull(T& dest) -{ - dest = 0; - return true; -} - -/// Assign a variable from a string -/// Default implementation: use operator= -template -bool assignString(T& dest, const std::string& data) -{ - dest = data; - return true; -} - -/// Assign a variable from an enum valye -/// Default implementation: use operator= -template -bool assignEnum(T& dest, int data) -{ - dest = data; - return true; -} - -/// Assign a variable from an ID -/// Default implementation: use operator= -template -bool assignID(T& dest, long long data) -{ - dest = data; - return true; -} - -/// Assign a variable from a typed data. -/// Return false if failed. -/// Default implementation: use constructor from all single-valued types -template -class Assign -{ -public: - static bool do_assign(T& dest, int type, const void* data) - { - if (isSpecial(type)) - { - if (isString(type)) - { - std::string str((const char*)data,size(type)); - return assignString(dest,str); - } - switch (type) - { - case Null: return assignNull(dest); - case Enum: return assignEnum((int &) dest,*(const int*)data); - case ID: return assignID((long long &) dest,*(const long long*)data); - } - } - else - { - if (!isSingle(type)) type = toSingle(type); - switch (type) - { - case Bool: dest = (T)(bool)( *(const unsigned char*)data != 0 ); - return true; - case Byte: dest = (T)*(const unsigned char*)data; - return true; - case Short: dest = (T)*(const short*)data; - return true; - case Int: dest = (T)*(const int*)data; - return true; - case QWord: dest = (T)*(const long long*)data; - return true; - case Float: dest = (T)*(const float*)data; - return true; - case Double: dest = (T)*(const double*)data; - return true; - case LongDouble: dest = (T)*(const long double*)data; - return true; - } - } - // Failure - // In this implementation: only Type::Delete is not supported - return false; - } -}; - -template -bool assign(T& dest, int type, const void* data) -{ - return Assign::do_assign(dest,type,data); -} - -// Specializations - -template<> bool assignString(bool& dest, const std::string& data); -template<> bool assignString(char& dest, const std::string& data); -template<> bool assignString(unsigned char& dest, const std::string& data); -template<> bool assignString(short& dest, const std::string& data); -template<> bool assignString(unsigned short& dest, const std::string& data); -template<> bool assignString(int& dest, const std::string& data); -template<> bool assignString(unsigned int& dest, const std::string& data); -template<> bool assignString(long long& dest, const std::string& data); -template<> bool assignString(unsigned long long& dest, const std::string& data); -template<> bool assignString(float& dest, const std::string& data); -template<> bool assignString(double& dest, const std::string& data); -template<> bool assignString(long double& dest, const std::string& data); - -template<> -class Assign -{ -public: - static bool do_assign(std::string& dest, int type, const void* data); -}; - -} // namespace Type - -/// 16 bits word byteswap operation -extern inline unsigned short wswap(unsigned short w) -{ - return (w>>8) | (w<<8); -} - -/// 32 bits dword byteswap operation -extern inline unsigned int dswap(unsigned int d) -{ - return (d>>24) | ((d>>8)&0x0000ff00) - | ((d<<8)&0x00ff0000) | (d<<24); -} - -/// 64 bits qword byteswap operation -extern inline unsigned long long qswap(unsigned long long q) -{ - // TODO: better swap... - return (unsigned long long)(dswap((unsigned int)(q>>32))) - | (((unsigned long long)(dswap((unsigned int)q)))<<32); -} - -/// Array of values of a specified type. -/// -/// Note: For vector or matrix types, a value contains several elements. -class TypedArray -{ -protected: - int valueType; - unsigned int dataSize; - union - { - unsigned char sdata[16]; ///< for small data - struct - { - unsigned char* ptr; ///< for larger data - int* refs; ///< Number of references to the allocated data - }; - }; -public: - - TypedArray() - : valueType(Type::Null), dataSize(0), ptr(NULL), refs(NULL) - { - } - - TypedArray(int type, const void* data, unsigned int size) - : valueType(type) - { - if (data == NULL || size<=0) - { - ptr = NULL; - refs = NULL; - dataSize = 0; - } - else if (size <= sizeof(sdata)) - { - dataSize = size; - memcpy(sdata,data,size); - } - else - { - ptr = new unsigned char[size]; - memcpy(ptr,data,size); - dataSize=size; - refs = new int; - *refs = 1; - } - if (dataSize>0 && !Type::endianOk(valueType)) - { - /// Must swap endianness - valueType = dswap(valueType); - switch (Type::elemSize(valueType)) - { - case 2: - { - unsigned short* p = (unsigned short*)this->data(); - for (unsigned int i=0;idata(); - for (unsigned int i=0;idata(); - for (unsigned int i=0;i sizeof(sdata)) - { - if (--(*refs)==0) - { - delete[] ptr; - delete refs; - } - } - } - - void operator=(const TypedArray& from) - { - if (dataSize > sizeof(sdata)) - { - if (--(*refs)==0) - { - delete[] ptr; - delete refs; - } - } - valueType=from.valueType; - dataSize=from.dataSize; - if (dataSize == 0) - { - ptr = NULL; - refs = NULL; - } - else if (dataSize <= sizeof(sdata)) - { - memcpy(sdata,from.sdata,dataSize); - } - else - { - ptr = from.ptr; - refs = from.refs; - ++(*refs); - } - } - - const unsigned char* data() const - { - return (dataSize<=sizeof(sdata))?sdata:ptr; - } - - unsigned int size() const - { - return dataSize; - } - - unsigned int n() const - { - unsigned int v = valueSize(); - if (v==0) return 0; - return size() / v; - } - - bool empty() const - { - return n()==0; - } - - int type() const - { - return valueType; - } - - bool isSpecial() const - { - return Type::isSpecial(valueType); - } - - bool isNum() const - { - return Type::isNum(valueType); - } - - bool isReal() const - { - return Type::isReal(valueType); - } - - bool isString() const - { - return Type::isString(valueType); - } - - bool isSingle() const - { - return Type::isSingle(valueType); - } - - bool isVector() const - { - return Type::isVector(valueType); - } - - bool isMatrix() const - { - return Type::isMatrix(valueType); - } - - unsigned int valueSize() const - { - return Type::size(valueType); - } - - unsigned int elemSize() const - { - return Type::elemSize(valueType); - } - - int elemType() const - { - if (isSingle()) return valueType; - else return Type::toSingle(valueType); - } - - unsigned int nx() const - { - return Type::nx(valueType); - } - - unsigned int ny() const - { - return Type::ny(valueType); - } - - template - bool assign(T& dest, int n=0) const - { - return Type::assign(dest, valueType, data()+valueSize()*n); - } - - template - T get(int n=0) const - { - T dest; - assign(dest, n); - return dest; - } - -}; - -} // namespace ftl - -#endif diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/vec.h b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/vec.h deleted file mode 100644 index 9d542a02e31..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/include/ftl/vec.h +++ /dev/null @@ -1,493 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR * -* Template Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 20054 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: include/ftl/vec.h * -* * -* Contacts: 20/09/2005 Clement Menier * -* * -******************************************************************/ -#ifndef FTL_VEC_H -#define FTL_VEC_H - -#include -#include "fixed_array.h" - -#include "type.h" - -#include - -namespace ftl -{ - -template -class Vec : public fixed_array -{ -public: - - static Type::Type getType() { return (Type::Type)Type::vector(Type::get(real()),N); } - - /// Default constructor: sets all values to 0. - Vec() - { - this->assign(0); - } - -/* - Vec(real r1) - { - static_assert(N == 1, ""); - this->elems[0]=r1; - } -*/ - - /// Specific constructor for 2-elements vectors. - Vec(real r1, real r2) - { - static_assert(N == 2, ""); - this->elems[0]=r1; - this->elems[1]=r2; - } - - /// Specific constructor for 3-elements vectors. - Vec(real r1, real r2, real r3) - { - static_assert(N == 3, ""); - this->elems[0]=r1; - this->elems[1]=r2; - this->elems[2]=r3; - } - - /// Specific constructor for 4-elements vectors. - Vec(real r1, real r2, real r3, real r4) - { - static_assert(N == 4, ""); - this->elems[0]=r1; - this->elems[1]=r2; - this->elems[2]=r3; - this->elems[3]=r4; - } - - /// Specific constructor for 5-elements vectors. - Vec(real r1, real r2, real r3, real r4, real r5) - { - static_assert(N == 5, ""); - this->elems[0]=r1; - this->elems[1]=r2; - this->elems[2]=r3; - this->elems[3]=r4; - this->elems[4]=r5; - } - - /// Specific constructor for 6-elements vectors (bounding-box). - Vec(real r1, real r2, real r3, real r4, real r5, real r6) - { - static_assert(N == 6, ""); - this->elems[0]=r1; - this->elems[1]=r2; - this->elems[2]=r3; - this->elems[3]=r4; - this->elems[4]=r5; - this->elems[5]=r6; - } - - /// Constructor from an N-1 elements vector and an additional value (added at the end). - Vec(const Vec& v, real r1) - { - static_assert(N > 1, ""); - for(int i=0;ielems[i] = v[i]; - this->elems[N-1]=r1; - } - - /// Constructor from an array of values. - template - explicit Vec(const real2* p) - { - std::copy(p, p+N, this->begin()); - } - - template - explicit Vec(const Vec &v) - { - std::copy(v.begin(), v.begin()+(N>M?M:N), this->begin()); - } - - /// Special access to first element. - real& x() { static_assert(N >= 1, ""); return this->elems[0]; } - /// Special access to second element. - real& y() { static_assert(N >= 2, ""); return this->elems[1]; } - /// Special access to third element. - real& z() { static_assert(N >= 3, ""); return this->elems[2]; } - /// Special access to fourth element. - real& w() { static_assert(N >= 4, ""); return this->elems[3]; } - - /// Special const access to first element. - const real& x() const { static_assert(N >= 1, ""); return this->elems[0]; } - /// Special const access to second element. - const real& y() const { static_assert(N >= 2, ""); return this->elems[1]; } - /// Special const access to third element. - const real& z() const { static_assert(N >= 3, ""); return this->elems[2]; } - /// Special const access to fourth element. - const real& w() const { static_assert(N >= 4, ""); return this->elems[3]; } - - /// Assignment operator from an array of values. - void operator=(const real* p) - { - std::copy(p, p+N, this->begin()); - } - - /// Assignment from a vector with different dimensions. -template void operator=(const Vec& v) - { - std::copy(v.begin(), v.begin()+(N>M?M:N), this->begin()); - } - - /// Sets every element to 0. - void clear() - { - this->assign(0); - } - - /// Sets every element to r. - void fill(real r) - { - this->assign(r); - } - - /// Access to i-th element. - real& operator[](int i) - { - return this->elems[i]; - } - - /// Const access to i-th element. - const real& operator[](int i) const - { - return this->elems[i]; - } - - /// Access to i-th element. - real& operator()(int i) - { - return this->elems[i]; - } - - /// Const access to i-th element. - const real& operator()(int i) const - { - return this->elems[i]; - } - - /// Cast into a const array of values. - const real* ptr() const - { - return this->elems; - } - - /// Cast into an array of values. - real* ptr() - { - return this->elems; - } - - // LINEAR ALGEBRA - - /// Multiplication by a scalar f. - Vec operator*(real f) const - { - Vec r; - for (int i=0;ielems[i]*f; - return r; - } - - /// Scalar multiplication assignment operator. - void operator*=(real f) - { - for (int i=0;ielems[i]*=f; - } - - /// Division by a scalar f. - Vec operator/(real f) const - { - Vec r; - for (int i=0;ielems[i]/f; - return r; - } - - /// On-place division by a scalar f. - void operator/=(real f) - { - for (int i=0;ielems[i]/=f; - } - - /// Dot product. - real operator*(const Vec& v) const - { - real r = this->elems[0]*v[0]; - for (int i=1;ielems[i]*v[i]; - return r; - } - - /// Vector addition. - Vec operator+(const Vec& v) const - { - Vec r; - for (int i=0;ielems[i]+v[i]; - return r; - } - - /// On-place vector addition. - void operator+=(const Vec& v) - { - for (int i=0;ielems[i]+=v[i]; - } - - /// Vector subtraction. - Vec operator-(const Vec& v) const - { - Vec r; - for (int i=0;ielems[i]-v[i]; - return r; - } - - /// On-place vector subtraction. - void operator-=(const Vec& v) - { - for (int i=0;ielems[i]-=v[i]; - } - - /// Vector negation. - Vec operator-() const - { - Vec r; - for (int i=0;ielems[i]; - return r; - } - - /// Squared norm. - real norm2() const - { - real r = this->elems[0]*this->elems[0]; - for (int i=1;ielems[i]*this->elems[i]; - return r; - } - - /// Euclidean norm. - real norm() const - { - return sqrt(norm2()); - } - - /// Normalize the vector. - void normalize() - { - real r = norm(); - if (r>1e-10) - for (int i=0;ielems[i]/=r; - } - -}; - -/// Cross product for 3-elements vectors. -template -inline Vec<3,real> cross(const Vec<3,real>& a, const Vec<3,real>& b) -{ - return Vec<3,real>(a.y()*b.z() - a.z()*b.y(), - a.z()*b.x() - a.x()*b.z(), - a.x()*b.y() - a.y()*b.x()); -} - -/// Dot product (alias for operator*) -template -inline real dot(const Vec& a, const Vec& b) -{ - return a*b; -} - -// Definition of VecXY (X = 2,3,4) and Y = (b = byte, i = int, f = float, d = double). -typedef Vec<2,unsigned char> Vec2b; -typedef Vec<2,int> Vec2i; -typedef Vec<2,float> Vec2f; -typedef Vec<2,double> Vec2d; - -typedef Vec<3,unsigned char> Vec3b; -typedef Vec<3,int> Vec3i; -typedef Vec<3,float> Vec3f; -typedef Vec<3,double> Vec3d; - -typedef Vec<4,unsigned char> Vec4b; -typedef Vec<4,int> Vec4i; -typedef Vec<4,float> Vec4f; -typedef Vec<4,double> Vec4d; - -// Typed data - -namespace Type -{ - -//template -//inline Type get(const Vec& v) -//{ -// return (Type)vector(get(v[0]),N); -//} - -/// Assign a vector from a string -template -bool assignString(Vec& dest, const std::string& data) -{ - const char* src = data.c_str(); - const char* end = src+data.length(); - bool res = true; - int n = 0; - while (n < N && src < end) - { - if (*src==' ' || *src==',') ++src; - else - { - const char* s0 = src; - if (n==0 && *s0 == '{') ++s0; - do - ++src; - while (src < end && *src != ' ' && *src != ','); - int s = (int)(src-s0); - if (src==end && src[-1]=='}') --s; - if (s>0) - { - std::string str (s0, s0+s); -#ifdef DEBUG - std::cerr << "dest["< -class Assign< Vec > -{ -public: -static bool do_assign(Vec& dest, int type, const void* data) -{ - if (isString(type)) - { - return assignString(dest, std::string((const char*) data, size(type))); - } - else if (isVector(type)) - { - int eSize = elemSize(type); - int eType = toSingle(type); - int n = nx(type); - if (n>N) n=N; - bool res = true; - for (int i=0;i -std::ostream& operator<<(std::ostream& o, const ftl::Vec& v) -{ - o << '<' << v[0]; - for (int i=1; i'; - return o; -} - -template -std::ostream& operator<<(std::ostream& o, ftl::Vec& v) -{ - o << '<' << v[0]; - for (int i=1; i'; - return o; -} - -template -std::istream& operator>>(std::istream& in, ftl::Vec& v) -{ - int c; - c = in.peek(); - while (c==' ' || c=='\n' || c=='<') - { - in.get(); - c = in.peek(); - } - in >> v[0]; - for (int i=1; i> v[i]; - } - c = in.peek(); - while (c==' ' || c=='\n' || c=='>') - { - in.get(); - c = in.peek(); - } - return in; -} - -/// Scalar multiplication operator. -template -ftl::Vec operator*(real r, const ftl::Vec& v) -{ - return v*r; -} - -#endif diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/cmdline.cpp b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/cmdline.cpp deleted file mode 100644 index f3835b8df61..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/cmdline.cpp +++ /dev/null @@ -1,325 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR * -* Template Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 20054 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: src/ftlm/cmdline.cpp * -* * -* Contacts: 10/26/2005 Jeremie Allard * -* * -******************************************************************/ -#include -#if defined(WIN32) -//short getopt_long_only replacement -struct option -{ - const char *name; - int has_arg; - int *flag; - int val; -}; -char* optarg = NULL; -int optind = 1; -int optopt = 0; -int getopt_long_only(int argc,char **argv, const char* shortoptions, option* options, int* option_index) -{ - int nopts = 0; - while (options[nopts].name != NULL) ++nopts; - optarg = NULL; - if (optind >= argc) return -1; - char* opt = argv[optind]; - if (opt[0] != '-') return -1; - ++optind; - ++opt; - if (opt[0] == '-') ++opt; // -- is same as - - if (opt[0] == '\0') return -1; // end of parsing - if (opt[1] == '\0') // short option - { - optopt = opt[0]; - const char* shorto = strchr(shortoptions, optopt); - if (shorto == NULL) - { - std::cerr << "ERROR: option "< -#endif -#include - -namespace ftl -{ - -FlagOption* helpoption = NULL; - -BaseOption::BaseOption(const char* _longname, char _shortname, const char* _description, ArgType _arg) -: longname(_longname), shortname(_shortname), description(_description), arg(_arg), hasdefault(false), count(0) -{ - std::vector& opts = CmdLine::opts(); - opts.push_back(this); -} - -BaseOption::~BaseOption() -{ - std::vector& opts = CmdLine::opts(); - std::vector::iterator it; - for (it = opts.begin(); it != opts.end(); ++it) - { - opts.erase(it); - break; - } -} - -CmdLine::CmdLine(const char* _description) -: description(_description) -{ -} - -bool CmdLine::parse(int argc, char** argv, bool* error) -{ - // First create the list of options - int nopts = (int)opts().size(); - struct option* options = new struct option[nopts+1]; - std::string shortoptions; - for (int i=0;ilongname; - options[i].has_arg = opts()[i]->arg; - options[i].flag = NULL; - options[i].val = 256+i; - if (opts()[i]->shortname!='\0') - { - shortoptions+=opts()[i]->shortname; - if (opts()[i]->arg == BaseOption::REQ_ARG) - shortoptions+=':'; - else if (opts()[i]->arg == BaseOption::OPT_ARG) - shortoptions+="::"; - } - } - options[nopts].name = NULL; - options[nopts].has_arg = 0; - options[nopts].flag = NULL; - options[nopts].val = 0; - - //std::cerr << "ShortOptions: "<=(unsigned)nopts) - { - longopt = false; - for (int i=0;ishortname==c) - { - option_index = i; - break; - } - } - if ((unsigned)option_index>=(unsigned)nopts) - { - std::cerr<<"Error parsing options"<arg == BaseOption::NO_ARG) - { - std::cerr << program << ": option "; - if (longopt) std::cerr << "--"<longname; - else std::cerr << '-'<shortname; - std::cerr << " requires no value while \""<set(optarg)) - { - std::cerr << program << ": incorrect value for option "; - if (longopt) std::cerr << "--"<longname; - else std::cerr << "-"<shortname; - std::cerr << " .\n"; - if (error!=NULL) *error = true; - delete [] options; - return false; - } - } - } - else - { - if (opt->arg == BaseOption::REQ_ARG) - { - std::cerr << program << ": option "; - if (longopt) std::cerr << "--"<longname; - else std::cerr << '-'<shortname; - std::cerr << " requires a value."<set()) - { - std::cerr << program << ": error while treating option "; - if (longopt) std::cerr << "--"<longname; - else std::cerr << '-'<shortname; - std::cerr << " .\n"; - if (error!=NULL) *error = true; - delete [] options; - return false; - } - } - } - option_index=-1; - } - for (int i=optind;icount>0) - { - if (description==NULL) std::cout << program<< "\n"; - std::cout << help() << std::endl; - delete [] options; - return false; - } - delete [] options; - return true; -} - -std::string CmdLine::help() -{ - std::ostringstream ss; - if (description!=NULL) - ss << description << std::endl; - ss << "Available options:\n"; - for (unsigned int i=0;ihelp() << std::endl; - return ss.str(); -} - - -std::vector& CmdLine::opts() -{ - static std::vector _opts; - - if (_opts.empty()) - { - // Default options are added first - _opts.push_back(NULL); // make sure the vector is not empty anymore - - helpoption = new FlagOption("help",'h',"Display available options"); - - // Then remove the bogus first entry - _opts.erase(_opts.begin()); - } - // Now I can add the current option - - return _opts; -} - -BaseOption* CmdLine::getOpt(const std::string longname_) - { - for( std::vector::const_iterator it =opts().begin(); it longname,longname_.c_str()) == 0) return (*it); - } - return NULL; - } - - void CmdLine::setDesc(const std::string // desc - ) - { - // strcpy(description,desc.c_str()); - } - -} // namespace ftl diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/crc.cpp b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/crc.cpp deleted file mode 100644 index 518a2344fa1..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/crc.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include "ftl/crc.h" - -namespace ftl { - -static const unsigned int _crc32TableMsbToLsb[] = { - 0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005, - 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd, - 0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9, 0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75, - 0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011, 0x791d4014, 0x7ddc5da3, 0x709f7b7a, 0x745e66cd, - 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039, 0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5, - 0xbe2b5b58, 0xbaea46ef, 0xb7a96036, 0xb3687d81, 0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d, - 0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49, 0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95, - 0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1, 0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d, - 0x34867077, 0x30476dc0, 0x3d044b19, 0x39c556ae, 0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072, - 0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16, 0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca, - 0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde, 0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02, - 0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, 0x53dc6066, 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba, - 0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, 0xbfa1b04b, 0xbb60adfc, 0xb6238b25, 0xb2e29692, - 0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6, 0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a, - 0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e, 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2, - 0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686, 0xd5b88683, 0xd1799b34, 0xdc3abded, 0xd8fba05a, - 0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637, 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb, - 0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f, 0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53, - 0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47, 0x36194d42, 0x32d850f5, 0x3f9b762c, 0x3b5a6b9b, - 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff, 0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623, - 0xf12f560e, 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7, 0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b, - 0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f, 0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3, - 0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7, 0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b, - 0x9b3660c6, 0x9ff77d71, 0x92b45ba8, 0x9675461f, 0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3, - 0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640, 0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c, - 0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8, 0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24, - 0x119b4be9, 0x155a565e, 0x18197087, 0x1cd86d30, 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec, - 0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, 0x2497d08d, 0x2056cd3a, 0x2d15ebe3, 0x29d4f654, - 0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0, 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c, - 0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18, 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4, - 0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0, 0x9abc8bd5, 0x9e7d9662, 0x933eb0bb, 0x97ffad0c, - 0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668, 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4 -}; - - -unsigned int crc32MsbToLsb(const void *buf, unsigned int size) -{ - unsigned int ulCRC = 0x00000000; - const unsigned char *buffer = (const unsigned char *) buf; - - while (size--) - ulCRC = (ulCRC << 8) ^ _crc32TableMsbToLsb[((ulCRC & 0xFF000000) >> 24) ^ *buffer++]; - return ulCRC; -} - -} - diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/mat.cpp b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/mat.cpp deleted file mode 100644 index 6acc787e5c6..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/mat.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR * -* Template Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 20054 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: src/ftlm/mat.cpp * -* * -* Contacts: 20/09/2005 Clement Menier * -* * -******************************************************************/ -#include "ftl/mat.h" -#include "ftl/quat.h" -#include - -namespace ftl -{ - -Mat4x4f matrixTranslation(const Vec3f& pos) -{ - Mat4x4f m; m.identity(); - m[0][3] = pos[0]; - m[1][3] = pos[1]; - m[2][3] = pos[2]; - return m; -} - -Mat4x4f matrixScale(const Vec3f& scale) -{ - Mat4x4f m; m.identity(); - m[0][0] = scale[0]; - m[1][1] = scale[1]; - m[2][2] = scale[2]; - return m; -} - -Mat4x4f matrixScale(const float scale) -{ - Mat4x4f m; m.identity(); - m[0][0] = scale; - m[1][1] = scale; - m[2][2] = scale; - return m; -} - -Mat4x4f matrixRotation(const Quat& rot) -{ - Mat3x3f r; rot.toMatrix(&r); - Mat4x4f m; m.identity(); - m = r; - return m; -} - -Mat4x4f matrixRotation(float ang, const Vec3f& axis) -{ - Quat q; q.fromDegreeAngAxis(ang, axis); - return matrixRotation(q); -} - - -Mat4x4f matrixTransform(const Vec3f& pos, const Quat& rot, const Vec3f& scale) -{ - return matrixTranslation(pos) * matrixRotation(rot) * matrixScale(scale); -} - -Mat4x4f matrixTransform(const Vec3f& pos, const Quat& rot, float scale) -{ - return matrixTranslation(pos) * matrixRotation(rot) * matrixScale(scale); -} - -Mat4x4f matrixTransform(const Vec3f& pos, float ang, const Vec3f& axis, const Vec3f& scale) -{ - return matrixTranslation(pos) * matrixRotation(ang,axis) * matrixScale(scale); -} - -Mat4x4f matrixTransform(const Vec3f& pos, float ang, const Vec3f& axis, float scale) -{ - return matrixTranslation(pos) * matrixRotation(ang,axis) * matrixScale(scale); -} - -Mat4x4f matrixTransform(const Vec3f& pos, const Vec3f& scale) -{ - return matrixTranslation(pos) * matrixScale(scale); -} - -Mat4x4f matrixTransform(const Vec3f& pos, float scale) -{ - return matrixTranslation(pos) * matrixScale(scale); -} - -Mat4x4f matrixTransform(const Vec3f& pos) -{ - return matrixTranslation(pos); -} - -} // namespace ftl diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/quat.cpp b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/quat.cpp deleted file mode 100644 index c56e697051f..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/quat.cpp +++ /dev/null @@ -1,320 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR * -* Template Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 20054 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: src/ftlm/quat.cpp * -* * -* Contacts: 20/09/2005 Clement Menier * -* * -******************************************************************/ -#include "ftl/quat.h" -#include - -#define PI 3.14159265358979323846264338327 - -namespace ftl -{ - -Quat & Quat::operator+=(const Quat &q) -{ - w+=q.w; - x+=q.x; - y+=q.y; - z+=q.z; - return *this; -} -Quat & Quat::operator-=(const Quat &q) -{ - w-=q.w; - x-=q.x; - y-=q.y; - z-=q.z; - return *this; -} -Quat & Quat::operator*=(const Quat &q) -{ - return *this=(*this)*q; -} - -Quat & Quat::operator*=(float f) -{ - w*=f; - x*=f; - y*=f; - z*=f; - return *this; -} -Quat & Quat::operator/=(float f) -{ - w/=f; - x/=f; - y/=f; - z/=f; - return *this; -} - -Quat Quat::operator+(const Quat &q) const -{ - return Quat( - w+q.w - ,x+q.x - ,y+q.y - ,z+q.z - ); -} -Quat Quat::operator-(const Quat &q) const -{ - return Quat( - w-q.w - ,x-q.x - ,y-q.y - ,z-q.z - ); -} -Quat Quat::operator*(const Quat &q) const -{ - return Quat( - w*q.w-x*q.x-y*q.y-z*q.z - ,w*q.x+x*q.w+y*q.z-z*q.y - ,w*q.y+y*q.w+z*q.x-x*q.z - ,w*q.z+z*q.w+x*q.y-y*q.x - ); -} - -Quat Quat::operator*(float f) const -{ - return Quat( - w*f - ,x*f - ,y*f - ,z*f - ); -} -Quat Quat::operator/(float f) const -{ - return Quat( - w/f - ,x/f - ,y/f - ,z/f - ); -} - -Quat operator*(float f,const Quat &q) -{ - return Quat( - f*q.w - ,f*q.x - ,f*q.y - ,f*q.z - ); -} - -Quat Quat::operator-() const -{ - return Quat( - -w - ,-x - ,-y - ,-z - ); -} - -Quat Quat::operator~() const -{ - return Quat( - w - ,-x - ,-y - ,-z - ); -} - -void Quat::fromAngAxis(float ang,Vec3f axis) -{ -// quaternions can represent a rotation. The rotation is an angle t, around a -// unit vector u. q=(s,v); s= cos(t/2); v= u*sin(t/2). - axis.normalize(); - - w=(float)cos(ang/2); - float f=(float)sin(ang/2); - x=axis.x()*f; - y=axis.y()*f; - z=axis.z()*f; -} - -void Quat::toAngAxis(float *ang,Vec3f *axis) const -{ - *ang = (float)acos (w) * 2; - axis->x() = x; - axis->y() = y; - axis->z() = z; - axis->normalize(); -} - -void Quat::fromDegreeAngAxis(float ang,Vec3f axis) -{ - return fromAngAxis((float)(ang*PI/180.0f), axis); -} -void Quat::toDegreeAngAxis(float *ang,Vec3f *axis) const -{ - toAngAxis(ang, axis); - *ang *= (float)(180.0f/PI); -} - -void Quat::fromMatrix(const Mat3x3f &m) -{ - float tr, s; - - tr = m.x().x() + m.y().y() + m.z().z(); - - // check the diagonal - if (tr > 0) - { - s = (float)sqrt (tr + 1); - w = s * 0.5f; // w OK - s = 0.5f / s; - x = (m.y().z() - m.z().y()) * s; // x OK - y = (m.z().x() - m.x().z()) * s; // y OK - z = (m.x().y() - m.y().x()) * s; // z OK - } - else - { - if (m.y().y() > m.x().x() && m.z().z() <= m.y().y()) - { - s = (float)sqrt ((m.y().y() - (m.z().z() + m.x().x())) + 1.0f); - - y = s * 0.5f; // y OK - - if (s != 0.0f) - s = 0.5f / s; - - z = (m.z().y() + m.y().z()) * s; // z OK - x = (m.y().x() + m.x().y()) * s; // x OK - w = (m.z().x() - m.x().z()) * s; // w OK - } - else if ((m.y().y() <= m.x().x() && m.z().z() > m.x().x()) || (m.z().z() > m.y().y())) - { - s = (float)sqrt ((m.z().z() - (m.x().x() + m.y().y())) + 1.0f); - - z = s * 0.5f; // z OK - - if (s != 0.0f) - s = 0.5f / s; - - x = (m.x().z() + m.z().x()) * s; // x OK - y = (m.z().y() + m.y().z()) * s; // y OK - w = (m.x().y() - m.y().x()) * s; // w OK - } - else - { - s = (float)sqrt ((m.x().x() - (m.y().y() + m.z().z())) + 1.0f); - - x = s * 0.5f; // x OK - - if (s != 0.0f) - s = 0.5f / s; - - y = (m.y().x() + m.x().y()) * s; // y OK - z = (m.x().z() + m.z().x()) * s; // z OK - w = (m.y().z() - m.z().y()) * s; // w OK - } - } -} - -void Quat::toMatrix(Mat3x3f *m) const -{ - float wx, wy, wz, xx, yy, yz, xy, xz, zz; - - xx = 2 * x * x; xy = 2 * x * y; xz = 2 * x * z; - yy = 2 * y * y; yz = 2 * y * z; zz = 2 * z * z; - wx = 2 * w * x; wy = 2 * w * y; wz = 2 * w * z; - - m->x().x() = 1 - yy - zz; m->y().x() = xy - wz; m->z().x() = xz + wy; - m->x().y() = xy + wz; m->y().y() = 1 - xx - zz; m->z().y() = yz - wx; - m->x().z() = xz - wy; m->y().z() = yz + wx; m->z().z() = 1 - xx - yy; -} - -float Quat::length() const -{ - return (float)sqrt(w*w+x*x+y*y+z*z); -} - -void Quat::clear() -{ - w=1; - x=0; - y=0; - z=0; -} - -void Quat::normalize() -{ - float f=length(); - if (f>0) - { - f=1/f; - w*=f; x*=f; y*=f; z*=f; - } -} - -bool Quat::isIdentity() const -{ - return w>=1.0f; -} - -// Assignment from typed data - -namespace Type -{ - -template <> -bool assign(Quat& dest, int type, const void* data) -{ - Vec<4,float> tmp(1,0,0,0); - if (!assign(tmp,type,data)) return false; - dest.w = tmp[0]; - dest.x = tmp[1]; - dest.y = tmp[2]; - dest.z = tmp[3]; - dest.normalize(); - return true; -} - -} // namespace Type - -} // namespace ftl - -std::ostream& operator<<(std::ostream& o, const ftl::Quat& q) -{ - o << ftl::Vec<4,float>(q.w,q.x,q.y,q.z); - return o; -} - -std::istream& operator>>(std::istream& in, ftl::Quat& q) -{ - ftl::Vec4f v; - in >> v; - q.w = v[0]; - q.x = v[1]; - q.y = v[2]; - q.z = v[3]; - return in; -} diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/type.cpp b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/type.cpp deleted file mode 100644 index 25cbd9c573e..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/ftlm/type.cpp +++ /dev/null @@ -1,436 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR * -* Template Library * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 20054 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU LGPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: src/ftlm/type.cpp * -* * -* Contacts: 20/09/2005 Clement Menier * -* * -******************************************************************/ -#include - -// snprintf() has been provided since MSVC++ 14 (Visual Studio 2015). For other -// versions, it is simply #defined to _snprintf(). -#if defined(_MSC_VER) && _MSC_VER < 1900 -# define snprintf _snprintf -#endif - -#if defined(WIN32) -#include -#define strcasecmp stricmp -#define atoll atoi -#else -#include -#endif -#include -#include -#include - -namespace ftl -{ - -namespace Type -{ - -/// assign specializations - -template<> -bool assignString(bool& dest, const std::string& data) -{ - if (data.empty() || !strcasecmp(data.c_str(),"false") || !strcasecmp(data.c_str(),"no")) - { - dest = false; - return true; - } - else if (!strcasecmp(data.c_str(),"true") || !strcasecmp(data.c_str(),"yes")) - { - dest = true; - return true; - } - else if (data[0] == '-' || (data[0]>='0' && data[0]<='9')) - { - dest = (atoi(data.c_str())!=0); - return true; - } - else return false; -} - -template<> -bool assignString(char& dest, const std::string& data) -{ - if (data.empty()) - { - dest = 0; - return true; - } - else if (data[0] == '-' || (data[0]>='0' && data[0]<='9')) - { - dest = (char) atoi(data.c_str()); - return true; - } - else return false; -} - -template<> -bool assignString(unsigned char& dest, const std::string& data) -{ - return assignString((char&)dest, data); -} - -template<> -bool assignString(short& dest, const std::string& data) -{ - if (data.empty()) - { - dest = 0; - return true; - } - else if (data[0] == '-' || (data[0]>='0' && data[0]<='9')) - { - dest = (short) atoi(data.c_str()); - return true; - } - else return false; -} - -template<> -bool assignString(unsigned short& dest, const std::string& data) -{ - return assignString((short&)dest, data); -} - -template<> -bool assignString(int& dest, const std::string& data) -{ - if (data.empty()) - { - dest = 0; - return true; - } - else if (data[0] == '-' || (data[0]>='0' && data[0]<='9')) - { - dest = atoi(data.c_str()); - return true; - } - else return false; -} - -template<> -bool assignString(unsigned int& dest, const std::string& data) -{ - return assignString((int&)dest, data); -} - -template<> -bool assignString(long long& dest, const std::string& data) -{ - if (data.empty()) - { - dest = 0; - return true; - } - else if (data[0] == '-' || (data[0]>='0' && data[0]<='9')) - { - dest = atoll(data.c_str()); - return true; - } - else return false; -} - -template<> -bool assignString(unsigned long long& dest, const std::string& data) -{ - return assignString((long long&)dest, data); -} - -template<> -bool assignString(float& dest, const std::string& data) -{ - if (data.empty()) - { - dest = 0; - return true; - } - else if (data[0] == '-' || (data[0]>='0' && data[0]<='9')) - { - dest = (float)atof(data.c_str()); - return true; - } - else return false; -} - -template<> -bool assignString(double& dest, const std::string& data) -{ - if (data.empty()) - { - dest = 0; - return true; - } - else if (data[0] == '-' || (data[0]>='0' && data[0]<='9')) - { - dest = (double)atof(data.c_str()); - return true; - } - else return false; -} - -template<> -bool assignString(long double& dest, const std::string& data) -{ - if (data.empty()) - { - dest = 0; - return true; - } - else if (data[0] == '-' || (data[0]>='0' && data[0]<='9')) - { - dest = (long double)atof(data.c_str()); - return true; - } - else return false; -} - -bool Assign::do_assign(std::string& dest, int type, const void* data) -{ - char buf[32]; - if (isSpecial(type)) - { - if (isString(type)) - { - dest.assign((const char*)data,size(type)); - return true; - } - switch (type) - { - case Null: - dest.clear(); - return true; - case Enum: - snprintf(buf,sizeof(buf),"%d",*(const int*)data); - dest = buf; - return true; - case ID: - snprintf(buf,sizeof(buf),"0x%016llx",*(const long long*)data); - dest = buf; - return true; - } - } - else if (isMatrix(type)) - { - std::string str; - dest = "{"; - for (int y=0;y string - -typedef struct { - int t; - const char* name; -} TypeToString; - -TypeToString names[] = -{ - { (int)Delete, "Delete" }, - { (int)Null, "Null" }, - { (int)Enum, "Enum" }, - { (int)ID, "ID" }, - { (int)Bool, "Bool" }, - { (int)Byte, "Byte" }, - { (int)Short, "Short" }, - { (int)Int, "Int" }, - { (int)Long, "Long" }, - { (int)QWord, "QWord" }, - { (int)Half, "Half" }, - { (int)Float, "Float" }, - { (int)Double, "Double" }, - { (int)LongDouble, "LongDouble" }, - { (int)String0, "String" }, - { (int)Null, NULL } -}; - -std::string name(int t) -{ - char buf[16]; - std::string res; - int i=0; - while (names[i].name != NULL && names[i].t != t) - ++i; - if (names[i].name != NULL) - { - res = names[i].name; - } - else if (isMatrix(t)) - { - res = "Mat<"; - snprintf(buf,sizeof(buf),"%d",ny(t)); - res += buf; - res += ","; - snprintf(buf,sizeof(buf),"%d",nx(t)); - res += buf; - res += ","; - res += name(toSingle(t)); - res += ">"; - } - else if (isVector(t)) - { - res = "Vec<"; - snprintf(buf,sizeof(buf),"%d",nx(t)); - res += buf; - res += ","; - res += name(toSingle(t)); - res += ">"; - } - else if (isString(t)) - { - if (size(t)==0) - res = "String"; - else - { - res = "String<"; - snprintf(buf,sizeof(buf),"%d",size(t)); - res += buf; - res += ">"; - } - } - else - { - snprintf(buf,sizeof(buf),"0x%x",t); - res = buf; - } - return res; -} - -int fromName(const std::string& name) -{ - int i=0; - while (names[i].name != NULL && name != names[i].name) - ++i; - if (names[i].name != NULL) - return names[i].t; - else if (name.substr(0,7)=="String<") - { - int n = 0; - sscanf(name.c_str(), "String<%d>", &n); - return buildString(n); - } - else if (name.substr(0,4)=="Mat<") - { - int nx = 0; - int ny = 0; - int pos = 8; - sscanf(name.c_str(), "Mat<%d,%d,%n", &ny,&nx,&pos); - int elem = fromName(name.substr(pos,name.length()-1-pos)); - return matrix(elem, nx, ny); - } - else if (name.substr(0,4)=="Vec<") - { - int nx = 0; - int pos = 8; - sscanf(name.c_str(), "Vec<%d,%n", &nx,&pos); - int elem = fromName(name.substr(pos,name.length()-1-pos)); - return vector(elem, nx); - } - else if (name.substr(0,2)=="0x") - { - int t = 0; - sscanf(name.c_str(), "0x%x", &t); - return t; - } - else - { - std::cerr << "ERROR converting type from name "< - -#include -#include -#include - -using std::min; -using std::max; -using namespace ftl; - -namespace flowvr -{ - -namespace render -{ - -static const float Fmax = std::numeric_limits::max(); -//static const float Fmin = std::numeric_limits::min(); - -BBox::BBox() - : a(Fmax,Fmax,Fmax), b(-Fmax,-Fmax,-Fmax) -{ -} - -BBox::BBox(const Vec3f& _a, const Vec3f& _b) - : a(_a), b(_b) -{ -} - -void BBox::clear() -{ - a.fill(Fmax); - b.fill(-Fmax); -} - -bool BBox::isEmpty() const -{ - return (a.x() > b.x()); -} - -BBox& BBox::operator+=(const BBox &p) -{ - if (p.a.x()b.x()) b.x()=p.b.x(); - if (p.b.y()>b.y()) b.y()=p.b.y(); - if (p.b.z()>b.z()) b.z()=p.b.z(); - return *this; -} - -BBox& BBox::operator+=(const Vec3f &p) -{ - if (p.x()b.x()) b.x()=p.x(); - if (p.y()b.y()) b.y()=p.y(); - if (p.z()b.z()) b.z()=p.z(); - return *this; -} - -BBox BBox::operator+(const BBox &p) -{ - return BBox(Vec3f(min(a.x(),p.a.x()) - ,min(a.y(),p.a.y()) - ,min(a.z(),p.a.z())) - ,Vec3f(max(b.x(),p.b.x()) - ,max(b.y(),p.b.y()) - ,max(b.z(),p.b.z())) - ); -} - -void BBox::apply(const BBox &p,const Mat4x4f &m) -{ - if (p.isEmpty()) - { - clear(); - } - else - { - for (int l=0;l<3;l++) - { - a[l] = b[l] = m[l][3]; - for (int c=0;c<3;c++) - { - if (m[l][c]>0.0f) { a[l]+=m[l][c]*p.a[c]; b[l]+=m[l][c]*p.b[c]; } - else { a[l]+=m[l][c]*p.b[c]; b[l]+=m[l][c]*p.a[c]; } - } - } - } -} - -BBox BBox::operator*(const Mat4x4f &m) -{ - BBox r; - r.apply(*this,m); - return r; -} - -/// Test if bbox intersect with half-space defined as eq.P >= 0 -bool BBox::inHalfspace(const ftl::Vec4f& eq) const -{ - Vec3f p; - if (eq[0]>=0) p[0]=b[0]; else p[0]=a[0]; - if (eq[1]>=0) p[1]=b[1]; else p[1]=a[1]; - if (eq[2]>=0) p[2]=b[2]; else p[2]=a[2]; - return (eq[0]*p[0]+eq[1]*p[1]+eq[2]*p[2]+eq[3])>=0; -} - -/// Test if bbox intersect with frustum defined by the given matrix -/// (interpreted as an OpenGL projection matrix) -bool BBox::inFrustum(const ftl::Mat4x4f& frustum, bool exact) const -{ - if (isEmpty()) return false; - if (exact) - { // axis-aligned tests - Mat3x3f m0; - m0(0,0)=frustum(0,0); m0(0,1)=frustum(0,1); m0(0,2)=frustum(0,2); - m0(1,0)=frustum(1,0); m0(1,1)=frustum(1,1); m0(1,2)=frustum(1,2); - m0(2,0)=frustum(2,0); m0(2,1)=frustum(2,1); m0(2,2)=frustum(2,2); - Mat3x3f minv; minv.invert(m0); - // m0.p0 + frustum.col(3) = 0; - // p0 = minv.(-frustum.col(3)); - Vec3f p0; - p0 = minv*Vec3f(-frustum(0,3),-frustum(1,3),-frustum(2,3)); - - // p1 = inv.( 1, 1,1) - // p2 = inv.(-1, 1,1) - // p3 = inv.( 1,-1,1) - // p4 = inv.(-1,-1,1) - // if p*[C] >= 0 then bb.b[C] >= p0[C] - // if p*[C] <= 0 then bb.a[C] <= p0[C] - const Mat4x3f pmat(Vec3f(1,1,1),Vec3f(-1,1,1),Vec3f(1,-1,1),Vec3f(-1,-1,1)); - - for (int c=0;c<3;c++) - { - Vec4f p = pmat*minv[c]; - if (p[0]>=0 && p[1]>=0 && p[2]>=0 && p[3]>=0) - { - if (b[c]p0[c]) return false; - } - } - } - // halfplanes from frustum - return inHalfspace(frustum[3]) // W>=0 - && inHalfspace(frustum[3]-frustum[0]) // X<=W : W-X>=0 - && inHalfspace(frustum[3]-frustum[1]) // Y<=W : W-Y>=0 - && inHalfspace(frustum[3]+frustum[0]) // X>=-W : W+X>=0 - && inHalfspace(frustum[3]+frustum[1]) // Y>=-W : W+Y>=0 - ; -} - -std::string BBox::toString() const -{ - std::ostringstream os; - os << *this; - return os.str(); -} - -void BBox::fromString(const std::string& text) -{ - std::istringstream is(text); - is >> *this; -} - -} // namespace render - -} // namespace flowvr - - -// iostream - -std::ostream& operator<<(std::ostream& o, const flowvr::render::BBox& bb) -{ - o << bb.a << '-' << bb.b; - return o; -} - -std::istream& operator>>(std::istream& in, flowvr::render::BBox& bb) -{ - in >> bb.a; - if (in.peek()=='-') in.get(); - in >> bb.b; - return in; -} - diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/librender/mesh.cpp b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/librender/mesh.cpp deleted file mode 100644 index b523d632000..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/librender/mesh.cpp +++ /dev/null @@ -1,1012 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR Render * -* Parallel Rendering Modules * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 2005 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU GPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: ./src/librender/mesh.cpp * -* * -* Contacts: * -* * -******************************************************************/ -#include -#ifndef MINI_FLOWVR -#include -#endif -#include - -#include -#include -#include -#include - -#include -#include - -namespace flowvr -{ - -namespace render -{ - -Mesh::Mesh() -: attrib(0), distmap(NULL), voxel(NULL) -{ -} - -Mesh::~Mesh() -{ - if(distmap!=NULL) free(distmap); - if(voxel!=NULL) free(voxel); -} - -void Mesh::operator=(const Mesh& mesh) -{ - if (&mesh == this) return; - clear(); - attrib = mesh.attrib; - points_p = mesh.points_p; - points_t = mesh.points_t; - points_n = mesh.points_n; - points_c = mesh.points_c; - points_v = mesh.points_v; - points_g = mesh.points_g; - groups_p0 = mesh.groups_p0; - faces_p = mesh.faces_p; - faces_n = mesh.faces_n; - faces_u = mesh.faces_u; - faces_v = mesh.faces_v; - edges = mesh.edges; - if (mesh.distmap!=NULL) - { - distmap = (DistMap*) malloc(mesh.distmap->size()); - memcpy(distmap,mesh.distmap,mesh.distmap->size()); - } - if (mesh.voxel!=NULL) - { - voxel = (Voxel*) malloc(mesh.voxel->size()); - memcpy(voxel,mesh.voxel,mesh.voxel->size()); - } -} - -void Mesh::clear() -{ - attrib=0; - points_p.clear(); - points_t.clear(); - points_n.clear(); - points_c.clear(); - points_v.clear(); - points_g.clear(); - groups_p0.clear(); - faces_p.clear(); - faces_n.clear(); - faces_u.clear(); - faces_v.clear(); - edges.clear(); - if(distmap!=NULL) { free(distmap); distmap = NULL; } - if(voxel!=NULL) { free(voxel); voxel = NULL; } -} - -void Mesh::init(int nbp, int nbf, int attribs) -{ - clear(); - attrib=attribs; - if (getAttrib(MESH_POINTS_POSITION)) - { - points_p.resize(nbp); - points_n.resize(nbp); - if (getAttrib(MESH_POINTS_TEXCOORD)) - points_t.resize(nbp); - if (getAttrib(MESH_POINTS_COLOR)) - points_c.resize(nbp); - if (getAttrib(MESH_POINTS_VALUE)) - points_v.resize(nbp); - if (getAttrib(MESH_POINTS_GROUP)) - { - points_g.resize(nbp); - groups_p0.resize(1); - } - } - if (getAttrib(MESH_FACES)) - { - faces_p.resize(nbf); - faces_n.resize(nbf); - faces_u.resize(nbf); - faces_v.resize(nbf); - } -} - -#if defined(WIN32) -#define strcasecmp stricmp -#endif - -bool Mesh::load(const char* filename) -{ - const char* anchor=strrchr(filename,'#'); - std::string fname(filename, (anchor?anchor : filename+strlen(filename))); -#ifndef MINI_FLOWVR - flowvr::render::ChunkRenderWriter::path.findFile(fname); -#endif - filename = fname.c_str(); - if (anchor) ++anchor; // skip '#' - - const char* ext=filename + strlen(filename); //strrchr(filename,'.'); - while (ext > filename+1 && *ext!='.' && *ext != '/' && *ext != '\\') - --ext; - if (*ext=='.') - { - if (!strcasecmp(ext,".obj")) - return loadObj(filename,anchor); - if (!strcasecmp(ext,".off")) - return loadOff(filename); - if (!strcasecmp(ext,".lwo")) - return loadLwo(filename); - if (!strcasecmp(ext,".vtk")) - return loadVtk(filename); - } - return loadMesh(filename); -} - -bool Mesh::save(const char* filename) const -{ - const char* ext=strrchr(filename,'.'); - if (ext!=NULL) - { - if (!strcasecmp(ext,".obj")) - return saveObj(filename); - if (!strcasecmp(ext,".off")) - return saveOff(filename); - if (!strcasecmp(ext,".stl")) - return saveStl(filename); - if (!strcasecmp(ext,".lwo")) - return saveLwo(filename); - if (!strcasecmp(ext,".vtk")) - return saveVtk(filename); - } - return saveMesh(filename); -} - -void Mesh::calcNormals(bool force) -{ - for (int i=0;i=0) { std::cerr << "Overlapping faces "< fdone; - int ntodo = nbf(); - fdone.resize(nbf()); - for (int i=0;i0) - { - int ndone = 0; - for (int i=0;i=0) pos = true; - if (getEdgeFace(getFP(i)[2],getFP(i)[1])>=0) pos = true; - if (getEdgeFace(getFP(i)[0],getFP(i)[2])>=0) pos = true; - if (getEdgeFace(getFP(i)[0],getFP(i)[1])>=0) neg = true; - if (getEdgeFace(getFP(i)[1],getFP(i)[2])>=0) neg = true; - if (getEdgeFace(getFP(i)[2],getFP(i)[0])>=0) neg = true; - if (!pos && !neg) continue; // untouching face - if (pos && neg) - { - std::cerr << "ERROR: Invalid face "< getPP(pmax)[2]) - pmax = i; - if (getPN(pmax)[2]<0) - { // Flip all faces - flipAll(); - } -} - -bool Mesh::isClosed() -{ - if (edges.empty()) calcEdges(); - int nopen = 0; - for (unsigned int i=0;i::iterator it = edges[i].begin(); it != edges[i].end(); it++) - { - if ((it->second.f1==-1) ^ (it->second.f2==-1)) - { - if (it->second.f1==-1) std::cerr << "Open edge "<first<<"-"<second.f2<first<<" from face "<second.f1<& loop, float dist) -{ - if (loop.size()<3) return; - Vertex v; - for (unsigned int j=0;j 0.0f) - for (unsigned int j=0;j maxdist) maxdist = d; - } - - if (dist <= 0.0f || maxdist < dist) - { - if (dist > 0.0f) - std::cout << "# open " << loop.size()<<"-sided loop : max radius " << maxdist < using " << nsteps << " steps."< loopOut, loopIn; - Vec3i f; - //loopOut = loop; - loopOut.resize(loop.size()); - for (unsigned int j=0;j next; - next.resize(nbp()); - for (int i=0;i::iterator it = edges[i].begin(); it != edges[i].end(); it++) - { - if ((it->second.f1==-1) ^ (it->second.f2==-1)) - { - if (it->second.f2==-1) { next[it->first] = i; } - else { next[i] = it->first; } - ++nopen; - } - } - } - int nloop = 0; - if (nopen>0) - { // close all loops - std::vector loop; - for (unsigned int i=0;i=0 && n!=(int)i && loop.size()<(unsigned)nbp()) - { - loop.push_back(n); - n = next[n]; - } - if (n==(int)i) - { // closed loop - closeLoop(loop, dist); - ++nloop; - for (unsigned int j=0;j0) - std::cout << "Closed "<mat[0][3]+=d[0]; - distmap->mat[1][3]+=d[1]; - distmap->mat[2][3]+=d[2]; - } - if (voxel!=NULL) - { - voxel->mat[0][3]+=d[0]; - voxel->mat[1][3]+=d[1]; - voxel->mat[2][3]+=d[2]; - } -} - -void Mesh::dilate(float dist) -{ - calcNormals(); - for (int i=0;inval(); - for (int i=0;idata[i]-=dist; - } - calcNormals(); -} - -/// Return true if some materials requires tangent vectors -bool Mesh::needTangents() const -{ - if (getAttrib(MESH_POINTS_TEXCOORD)) - { - // look for referenced materials with a bump texture - for (unsigned int i=0;imap_bump.empty()) - { - return true; - } - } - } - return false; -} - -/// Compute the tangent and co-tangent at each point -void Mesh::calcTangents(std::vector& tangent1, std::vector& tangent2) const -{ - tangent1.resize(nbp()); - tangent2.resize(nbp()); - - // see http://www.terathon.com/code/tangent.php - - for (int i=0;i& tangent) const -{ - tangent.resize(nbp()); - std::vector tangent1; - std::vector tangent2; - calcTangents(tangent1, tangent2); - for (unsigned int i=0;i tangent; - if (useTangent) - calcTangents(tangent); - - int ndata = 0; - int ip = ndata++; - int in = ndata++; - int it = (getAttrib(MESH_POINTS_TEXCOORD)?ndata++:-1); - int itan = (useTangent?ndata++:-1); - int ic = (getAttrib(MESH_POINTS_COLOR)?ndata++:-1); - int iv = (getAttrib(MESH_POINTS_VALUE)?ndata++:-1); - - int* dataType = new int[ndata]; - if (ip>=0) dataType[ip] = Type::Vec3f; - if (in>=0) dataType[in] = Type::Vec3f; - if (it>=0) dataType[it] = Type::Vec2f; - if (itan>=0) dataType[itan] = Type::Vec4f; - if (ic>=0) dataType[ic] = Type::Vec4b; - if (iv>=0) dataType[iv] = Type::Float; - { - ChunkVertexBuffer* vb = scene->addVertexBuffer(idVB, nbp(), ndata, dataType, bb); - vb->gen = gen; - char* dest = (char*) vb->data(); - for (int i=0;i=0) { *(Vec3f*)dest = getPP(i); dest+=sizeof(Vec3f); } - if (in>=0) { *(Vec3f*)dest = getPN(i); dest+=sizeof(Vec3f); } - if (it>=0) { *(Vec2f*)dest = getPT(i); dest+=sizeof(Vec2f); } - if (itan>=0) { *(Vec4f*)dest = tangent[i]; dest+=sizeof(Vec4f); } - if (ic>=0) { *(Vec4b*)dest = getPC(i); dest+=sizeof(Vec4b); } - if (iv>=0) { *(float*)dest = getPV(i); dest+=sizeof(float); } - } - } - - if (nbp()<=256) - { - ChunkIndexBuffer* ib = scene->addIndexBuffer(idIB, nbf()*3, Type::Byte, ChunkIndexBuffer::Triangle); - ib->gen = gen; - Vec<3,char>* f = (Vec<3,char>*)ib->data(); - for (int i=0;iaddIndexBuffer(idIB, nbf()*3, Type::Short, ChunkIndexBuffer::Triangle); - ib->gen = gen; - Vec<3,short> *f = (Vec<3,short>*)ib->data(); - for (int i=0;iaddIndexBuffer(idIB, nbf()*3, Type::Int, ChunkIndexBuffer::Triangle); - ib->gen = gen; - Vec<3,int> *f = (Vec<3,int>*)ib->data(); - for (int i=0;iaddParamID(idP, ChunkPrimParam::VBUFFER_ID,"position",idVB); - scene->addParam(idP, ChunkPrimParam::VBUFFER_NUMDATA,"position",ip); - scene->addParamID(idP, ChunkPrimParam::VBUFFER_ID,"normal",idVB); - scene->addParam(idP, ChunkPrimParam::VBUFFER_NUMDATA,"normal",in); - if (it>=0) - { - scene->addParamID(idP, ChunkPrimParam::VBUFFER_ID,"texcoord0",idVB); - scene->addParam(idP, ChunkPrimParam::VBUFFER_NUMDATA,"texcoord0",it); - } - if (itan>=0) - { - scene->addParamID(idP, ChunkPrimParam::VBUFFER_ID,"tangent",idVB); - scene->addParam(idP, ChunkPrimParam::VBUFFER_NUMDATA,"tangent",itan); - } - if (ic>=0) - { - scene->addParamID(idP, ChunkPrimParam::VBUFFER_ID,"color",idVB); - scene->addParam(idP, ChunkPrimParam::VBUFFER_NUMDATA,"color",ic); - } - if (iv>=0) - { - scene->addParamID(idP, ChunkPrimParam::VBUFFER_ID,"value",idVB); - scene->addParam(idP, ChunkPrimParam::VBUFFER_NUMDATA,"value",iv); - } - scene->addParamID(idP, ChunkPrimParam::IBUFFER_ID,"",idIB); -} -#endif //MINI_FLOWVR - -/// Optimize mesh by merging identical groups and reordering faces -void Mesh::optimize() -{ - int ng = (int)mat_groups.size(); - if (ng <= 1) return; // no need for optimizations - - // create a material -> group map - std::map< Material*,std::vector > matmap; - - for (unsigned int i=0;i new_mat_groups; - std::vector new_faces_p; - - new_mat_groups.reserve(matmap.size()); - new_faces_p.reserve(faces_p.size()); - for (std::map >::iterator it = matmap.begin(); it!=matmap.end(); ++it) - { - MaterialGroup m; - m.f0 = (int)new_faces_p.size(); - m.mat = it->first; - if (m.mat) - m.matname = m.mat->matname; - const std::vector& srcs = it->second; - std::set gnames; - for (unsigned int i=0;i::iterator it = gnames.begin(); it != gnames.end(); ++it) - { - if (!m.gname.empty()) m.gname += ' '; - m.gname += *it; - } - if (m.nbf > 0) - new_mat_groups.push_back(m); - } - faces_p.swap(new_faces_p); - mat_groups.swap(new_mat_groups); -} - - -/// Merge vertices closer than the given distance -void Mesh::mergeVertices(float dist) -{ - std::vector old2new_v; - std::vector new_pos; - std::vector new_pos_sum; - std::vector new_pos_count; - std::map pos2new; - - bool group = getAttrib(MESH_POINTS_GROUP); - int nbp0 = (group) ? nbg() : nbp(); - int new_nbp = 0; - old2new_v.resize(nbp0); - new_pos.reserve(nbp0); - new_pos_sum.reserve(nbp0); - new_pos_count.reserve(nbp0); - for (int p0 = 0; p0 < nbp0; ++p0) - { - Vec3f pos = (group) ? getPP(getGP0(p0)) : getPP(p0); - int new_p = -1; - float new_d2 = dist*dist; - std::pair::iterator,bool> it = pos2new.insert(std::pair(pos,new_nbp)); - if (!it.second) - { - new_p = it.first->second; - new_d2 = 0; - } - else if (dist > 0.0f) - { - for (int p1 = 0; p1 < new_nbp; ++p1) - { - float d2 = (new_pos[p1] - pos).norm2(); - if (d2 <= new_d2) - { - new_p = p1; - new_d2 = d2; - } - } - } - if (new_p != -1) - { - //std::cout << "vertex " << p0 << " merged, dist=" << sqrtf(new_d2) << std::endl; - new_pos_sum[new_p] += pos; - new_pos_count[new_p] += 1; - if (new_d2 > 0.0f) - new_pos[new_p] = new_pos_sum[new_p] / (float)new_pos_count[new_p]; - } - else - { - new_p = new_nbp; - ++new_nbp; - new_pos_sum.push_back(pos); - new_pos_count.push_back(1); - new_pos.push_back(pos); - } - old2new_v[p0] = new_p; - } - if (new_nbp == nbp0) - { - std::cout << "No vertices merged with distance = " << dist << std::endl; - return; - } - std::cout << nbp0 - new_nbp << "/"< -#include -#ifndef MINI_FLOWVR -#include -#endif -#include - -#include -#include -#include -#include - -namespace flowvr -{ - -namespace render -{ - -/// Brute-force distance computation -float Mesh::calcDist(Vec3f pos) const -{ - if (nbf()<0) return 1e10; - // Init with first point - Vec3f dp = pos-getPP(0); - bool inside = dot(dp,getPN(0))<=0; - float dist2 = dp.norm2(); - - enum { TFACE=0, TEDGE, TPOINT }; - int nearest_prim = TPOINT; - int nearest_num = 0; - - // Check faces - for (int i=0;i::const_iterator it = edges[i].begin(); it != edges[i].end(); it++, num++) - { - if ((int)num==nearest_num) - { - Vec3f dir = getPP(it->first) - getPP(i); - float a = dot(pos-getPP(i),dir); - Vec3f n; - if (it->second.f1>=0) n+=getFN(it->second.f1); - if (it->second.f2>=0) n+=getFN(it->second.f2); - std::cerr << "P="<first)<<" dir="<second.f2 << " n="<nx = nx; - distmap->ny = ny; - distmap->nz = nz; - distmap->mat = mat; - calcExtDistMap(mat, distmap->data, nx, ny, nz, maxDist); -/* - const float *src = distmap->data; - for (int z=0;znz;z++) - { - for (int y=0;yny;y++) - { - for (int x=0;xnx;x++) - { - std::cout << ' ' << *src; - ++src; - } - std::cout << std::endl; - } - std::cout << std::endl; - } -*/ - setAttrib(MESH_DISTMAP,true); -} - -void Mesh::calcDistMap(int nx, int ny, int nz, float maxDist) -{ - BBox bb = calcBBox(); - bb.a -= Vec3f(maxDist,maxDist,maxDist); - bb.b += Vec3f(maxDist,maxDist,maxDist); - Vec3f d = bb.b-bb.a; - - d[0] /=(nx-1); - d[1] /=(ny-1); - d[2] /=(nz-1); - - float maxd = d[0]; - if (d[1]>maxd) maxd = d[1]; - if (d[2]>maxd) maxd = d[2]; - - BBox distbb; - distbb.a = (bb.a+bb.b - Vec3f((float)(nx-1),(float)(ny-1),(float)(nz-1))*maxd)*0.5f; - distbb.b = distbb.a + Vec3f((float)(nx-1),(float)(ny-1),(float)(nz-1))*maxd; - std::cout << "Distance field bbox="< -#include -#include -#include -#include -#include - -#include - - -namespace flowvr -{ - -namespace render -{ - -class MeshLwoWriter -{ -public: - std::vector data; - struct ChunkHeader - { - unsigned int id; - unsigned int lengthPos; - unsigned int lengthSize; - }; - std::stack chunkStack; - - template - unsigned int writeSwap(T v, int pos = -1) - { - union { - T t; - char b[sizeof(T)]; - } tmp; - tmp.t = v; - unsigned int wpos = (pos < 0) ? (unsigned int)data.size() : (unsigned int)pos; - if (wpos + sizeof(T) > data.size()) - data.resize(wpos+sizeof(T)); - for (unsigned int i=0;i data.size()) - data.resize(wpos+size); - for (unsigned int i=0;i groups_pos; - int nbpos = 0; - - out.openChunk(ID("FORM")); - { - out.writeU4(ID("LWO2")); - - out.openChunk(ID("TAGS")); - { - out.writeStr("Default"); - } - out.closeChunk(ID("TAGS")); - /* - out.openChunk(ID("LAYR")); - { - out.writeU2(0); // number - out.writeU2(0); // flags - out.writeF4(0.0f); // pivot - out.writeF4(0.0f); - out.writeF4(0.0f); - out.writeStr("Default"); // name - //out.writeI2(-1); // parent - } - out.closeChunk(ID("LAYR")); - */ - out.openChunk(ID("PNTS")); - { - //std::cout<= 0) - { // this is not a subgroup - ++nbpos; - } - groups_pos[i] = nbpos-1; - } - fprintf(fp,"#Vertex Count %d\n",nbpos); - for (int i=0;i= 0) - { - out.writeF4(getPP(p)[0]); - out.writeF4(getPP(p)[1]); - out.writeF4(getPP(p)[2]); - } - } - } - else - { - nbpos = nbp(); - for (int i=0;i -#include -#include -#include - -#include - -namespace flowvr -{ - -namespace render -{ - -static bool swap = false; - -static bool swapread(void* ptr, size_t size, FILE* fp) -{ - //std::cout << "Reading "<=sizeof(unsigned int)) - { - *p = ftl::dswap(*p); - ++p; - size-=sizeof(unsigned int); - } - } - return true; -} - - -template -static bool swapread(std::vector& dest, int size, FILE* fp) -{ - dest.resize(size/sizeof(T)); - return swapread(&(dest[0]),size,fp); -} - -template -static bool swapread(std::vector& dest, FILE* fp) -{ - int size=0; - if (!swapread(&size,sizeof(int),fp)) return false; - //std::cout << "size = "< -static bool swapwrite(const std::vector& src, FILE* fp) -{ - int size=(int)src.size()*sizeof(T); - if (fwrite(&size,sizeof(int),1,fp)<=0) return false; - if (fwrite(&(src[0]),size,1,fp)<=0) return false; - return true; -} - -template -static bool swapread_malloc(T*& dest, bool swapdata, FILE* fp) -{ - int size=0; - if (!swapread((void *)&size,sizeof(int),fp)) return false; - if (size<(int)sizeof(T)) dest = NULL; - else - { - dest = (T*)malloc(size); - if (dest == NULL) return false; - if (swapdata) - { - if (!swapread((void*)dest, size, fp)) return false; - } - else - { - if (!swapread((void*)dest, sizeof(T), fp)) return false; - if (fread(dest+1, size-sizeof(T), 1, fp)<=0) return false; - } - } - return true; -} - -template -static bool swapwrite(const T* src, FILE* fp) -{ - int size=0; - if (src!=NULL) size = src->size(); - if (fwrite(&size,sizeof(int),1,fp)<=0) return false; - if (size>0) - if (fwrite(src,size,1,fp)<=0) return false; - return true; -} - -bool Mesh::loadMesh(const char* filename) -{ -#ifdef WIN32 - FILE* fp = fopen(filename,"rb"); -#else - FILE* fp = fopen(filename,"r"); -#endif - if (fp==NULL) return false; - std::cout<<"Loading Mesh file "< -#include -#include -#include -#include - -#include - - -namespace flowvr -{ - -namespace render -{ - -class MeshObjData -{ -public: - Mesh* mesh; - std::vector t_v; - std::vector t_vt; - std::vector t_vn; - std::vector t_fp; - - int v0; - int merge; - int smoothgroup; - enum { SMOOTHGROUP_SHIFT = 20 }; - - //std::vector t_v_g; // group of points using this position - - - MeshObjData(Mesh* m): mesh(m), v0(0), merge(0), smoothgroup(0) {} - - const char* skip(const char* c) - { - while (*c==' ' || *c=='\t') ++c; - return c; - } - - const char* endtoken(const char* c) - { - while (*c!=' ' && *c!='\t' && *c != '\0') ++c; - return c; - } - - void addMaterial(const Mesh::Material& m) - { - if (m.matname.empty()) return; - Mesh::Material*& dest = mesh->mat_map[m.matname]; - if (dest) *dest = m; - else dest = new Mesh::Material(m); - std::cout << "Material "<setAttrib(Mesh::MESH_MATERIALS,true); - } - else - { - std::cerr << "ERROR: material "<mat_groups.push_back(matg); - } - - Vec3f readColor(const char* c) - { - Vec3f res(1,1,1); - if (*(c = skip(c))) - { - res.x() = (float)strtod(skip(c),(char**)&c); - if (*(c = skip(c))) - { - res.y() = (float)strtod(skip(c),(char**)&c); - if (*(c = skip(c))) - { - res.z() = (float)strtod(skip(c),(char**)&c); - } - else - res.z() = res.y(); - } - else - { - res.y() = res.x(); - res.z() = res.x(); - } - } - return res; - } - - static std::string getParentDir(const std::string& filename) - { - std::string::size_type pos = filename.find_last_of("/\\"); - if (pos == std::string::npos) - return ""; // no directory - else - return filename.substr(0,pos); - } - - std::string getRelativeFile(std::string filename, const std::string& basefile) - { - std::cout << filename << std::endl; - if (filename.empty() || filename[0] == '/') return filename; - std::string base = getParentDir(basefile); - // remove any ".." - while ((filename.substr(0,3)=="../" || filename.substr(0,3)=="..\\") && !base.empty()) - { - filename = filename.substr(3); - base = getParentDir(base); - } - if (base.empty()) - return filename; - else if (base[base.length()-1] == '/') - return base + filename; - else - return base + "/" + filename; - } - - bool loadmtl(const char* filename, const char* basefilename) - { - std::string fpath = getRelativeFile(filename, basefilename); - - FILE* fp = fopen(fpath.c_str(),"r"); - if (fp==NULL) - { - std::cerr << "ERROR: Loading MTL file "<=0 && (line[i] == '\n' || line[i] == '\r' || line[i] == ' ' || line[i] == '\t')) - { - line[i] = '\0'; - --i; - } - } - if (!line[0]) continue; // empty line - const char* c = line; - const char* end = endtoken(c); - std::string t(c,end); - c = end; - if (t == "newmtl") - { - addMaterial(mat); - mat = Mesh::Material(); - mat.mtllib = filename; - mat.matname = skip(c); - } - else if (t == "Ka") - { - mat.ambient = readColor(c); - } - else if (t == "Kd") - { - mat.diffuse = readColor(c); - } - else if (t == "Ks") - { - mat.specular = readColor(c); - } - else if (t == "Ke") - { - mat.emmisive = readColor(c); - } - else if (t == "Ns") - { // specular power - mat.shininess = (float)strtod(skip(c),(char**)&c); - } - else if (t == "Tf") - { // transmission filter - mat.alpha = readColor(c)[0]; - } - else if (t == "d") - { // dissolve - mat.alpha = (float)strtod(skip(c),(char**)&c); - } - else if (t == "Ni") - { // optical density - mat.optical_density = (float)strtod(skip(c),(char**)&c); - } - else if (t == "illum") - { - int i = atoi(skip(c)); - switch (i) - { - case 0: // Color on and Ambient off - mat.ambient.clear(); - mat.specular.clear(); - mat.shininess = 0; - break; - case 1: // Color on and Ambient on - mat.specular.clear(); - mat.shininess = 0; - break; - case 2: // Highlight on - break; - case 3: // Reflection on and Ray trace on - break; - case 4: // Transparency: Glass on - // Reflection: Ray trace on - break; - case 5: // Reflection: Fresnel on and Ray trace on - break; - case 6: // Transparency: Refraction on - // Reflection: Fresnel off and Ray trace on - break; - case 7: // Transparency: Refraction on - // Reflection: Fresnel on and Ray trace on - break; - case 8: // Reflection on and Ray trace off - break; - case 9: // Transparency: Glass on - // Reflection: Ray trace off - break; - case 10: // Casts shadows onto invisible surfaces - break; - } - } - else if (t == "map_Ka") - { - mat.map_ambient = getRelativeFile(skip(c),fpath); - } - else if (t == "map_Kd") - { - mat.map_diffuse = getRelativeFile(skip(c),fpath); - } - else if (t == "map_Ks") - { - mat.map_specular = getRelativeFile(skip(c),fpath); - } - else if (t == "map_Tf" || t == "map_d") - { - mat.map_alpha = getRelativeFile(skip(c),fpath); - } - else if (t == "bump" || t == "disp") - { - mat.map_bump = getRelativeFile(skip(c),fpath); - } - else - { - std::cerr << "ERROR: MTL parsing on line "<=t_v.size()) - { - std::cerr << "Incorrect point "<=t_vt.size()) - { - std::cerr << "Incorrect texcoord "<=t_vn.size()) - { - std::cerr << "Incorrect normal "<getGP0(t_v_g[iv]); - int match_n = -1; - while (i>=0) - { - if (mesh->getPN(i) == v_n) - { - match_n = i; - if (mesh->getPT(i) == v_t) - { // found a match - ++merge; - index = i; - return; - } - } - // else find next point - i = mesh->getPGPNext(i); - } - // no match : new point - i = mesh->nbp(); - mesh->PP(i)=v_p; - mesh->PN(i)=v_n; - if (mesh->getAttrib(Mesh::MESH_POINTS_TEXCOORD)) - mesh->PT(i)=v_t; - if (t_v_g[iv]<0) - t_v_g[iv] = mesh->nbg(); - mesh->PG(i) = t_v_g[iv]; - mesh->PGPNext(i) = mesh->getGP0(t_v_g[iv]); - mesh->GP0(t_v_g[iv]) = i; - index = i; - } -*/ - - bool load(const char* filename, const char* filter) - { - FILE* fp = fopen(filename,"r"); - if (fp==NULL) return false; - - Vec3f v3; - Vec2f v2; - Vec3i fp0,fp1,fp2; - - BBox bb; - if (filter && *filter=='<') - { - int len = 0; - sscanf(filter,"<%f,%f,%f>-<%f,%f,%f>%n",&bb.a[0],&bb.a[1],&bb.a[2],&bb.b[0],&bb.b[1],&bb.b[2],&len); - filter+=len; - } - bool simple=false; - if (filter && std::string(filter)=="simple") - { - simple = true; - filter += 6; - } - - std::string gname = "default"; - std::string matname = ""; - - bool inc = true; - - int f0 = 0; - - merge = 0; - - mesh->setAttrib(Mesh::MESH_POINTS_POSITION,true); - //mesh->setAttrib(Mesh::MESH_POINTS_GROUP,true); - mesh->setAttrib(Mesh::MESH_FACES,true); - - // char * line = NULL; - //size_t len = 0; - // ssize_t read; - char line[5000]; - int lnum = 0; - - t_v.clear(); t_v.push_back(v3); - //t_v_g.clear(); t_v_g.push_back(-1); - t_vt.clear(); t_vt.push_back(v2); - t_vn.clear(); t_vn.push_back(v3); - - // Not Posix compatible - // while ((read = getline(&line, &len, fp)) != -1) - line [sizeof(line)-1]='\0'; - while (fgets(line, sizeof(line), fp) != NULL) - { - // remove EOL - { - int i = (int)strlen(line)-1; - while (i>=0 && (line[i] == '\n' || line[i] == '\r' || line[i] == ' ' || line[i] == '\t')) - { - line[i] = '\0'; - --i; - } - } - ++lnum; -#ifdef DEBUG - std::cout << lnum << ' ' << line << std::endl; -#endif - const char* c = line; - switch (*c) - { - case '#': - if (!strncmp(line,"#Vertex Count ",strlen("#Vertex Count "))) - { - int n = atoi(line+strlen("#Vertex Count ")); - std::cout << "Allocating "<setAttrib(Mesh::MESH_POINTS_TEXCOORD,true); - } - break; - case 'n': // normal - ++c; - if (!simple) - { - v3.x() = (float)strtod(skip(c),(char**)&c); - v3.y() = (float)strtod(skip(c),(char**)&c); - v3.z() = (float)strtod(skip(c),(char**)&c); - t_vn.push_back(v3); - mesh->setAttrib(Mesh::MESH_POINTS_NORMAL,true); - } - break; - } - break; - case 'f': - ++c; - switch(*c) - { - case ' ': // face - { - if (inc) - { - fp0 = readPoint(&c, simple); - fp1 = readPoint(&c, simple); - c = skip(c); - while (*c != '\n' && *c!='\0' && *c!='\r') - { - fp2 = readPoint(&c, simple); - - if (bb.isEmpty() || (bb.in(t_v[fp0[0]]) && bb.in(t_v[fp1[0]]) && bb.in(t_v[fp2[0]]))) - { - t_fp.push_back(fp0); - t_fp.push_back(fp1); - t_fp.push_back(fp2); - // put the position index temporarily in the mesh face array - mesh->FP(mesh->nbf())=Vec3i(fp0[0],fp1[0],fp2[0]); - //mesh->FP(mesh->nbf())=f; - } - fp1 = fp2; - c = skip(c); - } - } - } - break; - } - break; - case 'g': - ++c; - addMatGroup(matname, gname, f0, mesh->nbf()-f0); - f0 = mesh->nbf(); - gname = skip(c); - inc = (!filter || !strncmp(gname.c_str(), filter, strlen(filter)) || !strncmp(matname.c_str(), filter, strlen(filter))); - break; - case 's': - ++c; - if (!simple) - { - smoothgroup = atoi(skip(c)); - //std::cout << "smooth group "<nbf()-f0); - f0 = mesh->nbf(); - matname = skip(c); - inc = (!filter || !strncmp(gname.c_str(), filter, strlen(filter)) || !strncmp(matname.c_str(), filter, strlen(filter))); - } - break; - } - } - if (gname != "default" || !matname.empty() || f0 > 0) - addMatGroup(matname, gname, f0, mesh->nbf()-f0); - - // compute final vertex groups - - // First we compute for each point how many pair of normal/texcoord indices are used - // The map store the final index of each combinaison - std::cout << "Init vertex map"<, int > > vertNormTexMap; - vertNormTexMap.resize(inputnbp); - for (unsigned int i = 0; i < t_fp.size(); i++) - { - fp0 = t_fp[i]; - if ((unsigned)(fp0[0]) >= (unsigned)inputnbp) - { - std::cerr << "ERROR: invalid vertex index "< "<0) - { - nbp += s; - ++nbg; - } - if (s>1) // more that one point is created from a single position - mesh->setAttrib(Mesh::MESH_POINTS_GROUP,true); - - } - - // Then we can create the final arrays - std::cout << "Create final position"<<(mesh->getAttrib(Mesh::MESH_POINTS_TEXCOORD)?", texcoord":"")<<(mesh->getAttrib(Mesh::MESH_POINTS_NORMAL)?", normal":"")<<(mesh->getAttrib(Mesh::MESH_POINTS_GROUP)?", group":"")<<" arrays with "<points_p.resize(nbp); - if (mesh->getAttrib(Mesh::MESH_POINTS_TEXCOORD)) - mesh->points_t.resize(nbp); - if (mesh->getAttrib(Mesh::MESH_POINTS_NORMAL)) - mesh->points_n.resize(nbp); - //if (nbg != nbp) - if (mesh->getAttrib(Mesh::MESH_POINTS_GROUP)) - { - //mesh->setAttrib(Mesh::MESH_POINTS_GROUP, true); - mesh->points_g.resize(nbp); - //mesh->groups_p0.resize(nbg); - } - - for (int i = 0, j = 0; i < inputnbp; i++) - { - if (vertNormTexMap.empty()) continue; - Vec3f p = t_v[i]; - - int last_n = -1; - int pg0 = (int)mesh->groups_p0.size(); - - //std::map normTexMap; - for (std::map, int>::iterator it = vertNormTexMap[i].begin(); - it != vertNormTexMap[i].end(); ++it) - { - mesh->points_p[j] = p; - int n = it->first.first; - int t = it->first.second; - if (mesh->getAttrib(Mesh::MESH_POINTS_TEXCOORD)) - mesh->points_t[j] = t_vt[t]; - if (mesh->getAttrib(Mesh::MESH_POINTS_NORMAL)) - mesh->points_n[j] = t_vn[(n & ((1<getAttrib(Mesh::MESH_POINTS_GROUP)) - { - mesh->points_g[j] = (int)mesh->groups_p0.size(); - if (n != last_n) - { - if ((int)mesh->groups_p0.size() == pg0) - mesh->groups_p0.push_back(j); // new group - else - mesh->groups_p0.push_back(-j); // new subgroup - } - } - last_n = n; - it->second = j++; - } - } - - std::cout << "Update face array"<FP(i/3)[i%3] = index; - } - - - //if (line) - // free(line); - fclose(fp); - std::cout<<"Loaded file "<nbp()<<" final points, "<nbf()<<" triangles, "<nbg()<<" normal groups"<nbmatg()<<" material groups"<nbg()==mesh->nbp()) - //{ // groups are not required - // mesh->setAttrib(Mesh::MESH_POINTS_GROUP,false); - //} - - // Computing normals - mesh->calcNormals(); - mesh->calcBBox(); - - return true; - } -}; - -bool Mesh::loadObj(const char* filename, const char* filter) -{ - MeshObjData data(this); - return data.load(filename, filter); -} - -bool Mesh::saveObj(const char* filename) const -{ - FILE* fp = fopen(filename,"w+"); - if (fp==NULL) return false; - std::cout<<"Writing Obj file "<= 0) - ++nbgpos; - std::cout << nbg() << " normal groups, " << nbgpos << " position groups" << std::endl; - } - fprintf(fp,"# OBJ output by FlowVR Render\n"); - - // first output material libs - std::set mtllibs; - for (std::map::const_iterator it = mat_map.begin(), itend = mat_map.end(); it != itend; ++it) - { - const Material* m = it->second; - if (!m || m->mtllib.empty()) continue; - if (mtllibs.insert(m->mtllib).second) - fprintf(fp,"mtllib %s\n",m->mtllib.c_str()); - } - - fprintf(fp,"#Face Count %d\n",nbf()); - std::vector groups_pos; - int nbpos = 0; - if (group) - { - groups_pos.resize(nbg()); - for (int i=0;i= 0) - { // this is not a subgroup - ++nbpos; - } - groups_pos[i] = nbpos-1; - } - fprintf(fp,"#Vertex Count %d\n",nbpos); - for (int i=0;i= 0) - fprintf(fp,"v %f %f %f\n",getPP(p)[0],getPP(p)[1],getPP(p)[2]); - } - } - else - { - nbpos = nbp(); - fprintf(fp,"#Vertex Count %d\n",nbpos); - for (int i=0;i= mat_f1 && mat+1 < (int)mat_groups.size()) - { - ++mat; - if (lastgname != mat_groups[mat].gname) - { - fprintf(fp,"g %s\n",mat_groups[mat].gname.c_str()); - lastgname = mat_groups[mat].gname; - } - if (mat_groups[mat].matname.empty()) - fprintf(fp,"usemtl default\n"); - else - fprintf(fp,"usemtl %s\n", mat_groups[mat].matname.c_str()); - mat_f1 = mat_groups[mat].f0+mat_groups[mat].nbf; - } - fprintf(fp,"f"); - for (int j=0;j<3;j++) - { - int p = getFP(i)[j]; - int pp=p, pn=p, pt=p; - if (group) - { - pp = groups_pos[getPG(p)]; - pn = getPG(p); - } - if (normal && texcoord) - fprintf(fp," %d/%d/%d",pp+v0,pt+vt0,pn+vn0); - else if (normal) - fprintf(fp," %d//%d",pp+v0,pn+vn0); - else if (texcoord) - fprintf(fp," %d/%d",pp+v0,pt+vt0); - else - fprintf(fp," %d",pp+v0); - } - fprintf(fp,"\n"); - } - v0 += nbpos; - if (normal) vn0 += (group?nbg():nbp()); - if (texcoord) vt0 += nbp(); - return true; -} - -} // namespace render - -} // namespace flowvr diff --git a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/librender/mesh_io_off.cpp b/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/librender/mesh_io_off.cpp deleted file mode 100644 index 0df4885dac1..00000000000 --- a/applications/plugins/SofaDistanceGrid/extlibs/miniFlowVR/src/librender/mesh_io_off.cpp +++ /dev/null @@ -1,158 +0,0 @@ -/******* COPYRIGHT ************************************************ -* * -* FlowVR Render * -* Parallel Rendering Modules * -* * -*-----------------------------------------------------------------* -* COPYRIGHT (C) 2005 by * -* Laboratoire Informatique et Distribution (UMR5132) and * -* INRIA Project MOVI. ALL RIGHTS RESERVED. * -* * -* This source is covered by the GNU GPL, please refer to the * -* COPYING file for further information. * -* * -*-----------------------------------------------------------------* -* * -* Original Contributors: * -* Jeremie Allard, * -* Clement Menier. * -* * -******************************************************************* -* * -* File: ./src/librender/mesh.cpp * -* * -* Contacts: * -* * -******************************************************************/ -#include -#include -#include -#include - -#include - -namespace flowvr -{ - -namespace render -{ - -bool Mesh::loadOff(const char* filename) -{ - FILE* fp = fopen(filename,"r"); - if (fp==NULL) return false; - int nbp=0, nbf=0, nbe=0; - char line[5000]; - - //fscanf(fp,"%s\n",line); - if (!fgets(line,sizeof(line),fp)) { fclose(fp); return false; } - bool color = (strchr(line,'C')!=NULL); - bool normals = (strchr(line,'N')!=NULL); - bool texcoord = (strchr(line,'T')!=NULL); - if (!(line[0]>='0' && line[0]<='9')) - if (!fgets(line,sizeof(line),fp)) { fclose(fp); return false; } - sscanf(line,"%d %d %d",&nbp, &nbf, &nbe); - - int flags = MESH_POINTS_POSITION|MESH_FACES; - if (texcoord) flags |= MESH_POINTS_TEXCOORD; - if (normals) flags |= MESH_POINTS_NORMAL; - if (color) flags |= MESH_POINTS_COLOR; - init(nbp, nbf, flags); - for(int i=0;inbp()<<" final points, "<nbf()<<" triangles."< -#include -#include -#include - -#include - -namespace flowvr -{ - -namespace render -{ - -//bool Mesh::loadStl(const char* filename) -//{ -// return false; -//} - -bool Mesh::saveStl(const char* filename) const -{ - FILE* fp = fopen(filename,"w+"); - if (fp==NULL) return false; - std::cout<<"Writing Stl file "< -#include -#include -#include -#include -#include -#include - -#include - -#if defined(WIN32) -#define strcasecmp stricmp -#endif - -// Format doc: http://www.vtk.org/VTK/img/file-formats.pdf -// http://www.cacr.caltech.edu/~slombey/asci/vtk/vtk_formats.simple.html - -namespace flowvr -{ - -namespace render -{ - -class BaseVTKDataIO -{ -public: - int dataSize; - BaseVTKDataIO() : dataSize(0) {} - virtual ~BaseVTKDataIO() {} - virtual void resize(int n) = 0; - virtual bool read(std::ifstream& f, int n, bool binary) = 0; - virtual bool write(std::ofstream& f, int n, int groups, bool binary) = 0; - virtual void copyTo(std::vector& v) = 0; -}; - -template -class VTKDataIO : public BaseVTKDataIO -{ -public: - T* data; - VTKDataIO() : data(NULL) {} - ~VTKDataIO() { if (data) delete[] data; } - virtual void resize(int n) - { - if (dataSize != n) - { - if (data) delete[] data; - data = new T[n]; - } - dataSize = n; - } - virtual bool read(std::ifstream& in, int n, bool binary) - { - resize(n); - if (binary) - { - in.read((char*)data, n * sizeof(T)); - if (in.eof() || in.bad()) - { - resize(0); - return false; - } - } - else - { - int i = 0; - std::string line; - while(i < dataSize && !in.eof() && !in.bad()) - { - std::getline(in, line); - std::istringstream ln(line); - while (i < n && ln >> data[i]) - ++i; - } - if (i < n) - { - resize(0); - return false; - } - } - return true; - } - virtual bool write(std::ofstream& out, int n, int groups, bool binary) - { - if (n > dataSize && !data) return false; - if (binary) - { - out.write((char*)data, n * sizeof(T)); - } - else - { - if (groups <= 0 || groups > n) groups = n; - for (int i = 0; i < n; ++i) - { - if ((i % groups) > 0) - out << ' '; - out << data[i]; - if ((i % groups) == groups-1) - out << '\n'; - } - } - if (out.bad()) - return false; - return true; - } - virtual void copyTo(std::vector& v) - { - if (!data) return; - for (unsigned int i=0; i < v.size(); ++i) - for (int c=0;c<3;++c) - v[i][c] = (float)data[3*i+c]; - } -}; - -BaseVTKDataIO* newVTKDataIO(const std::string& typestr) -{ - if (!strcasecmp(typestr.c_str(), "char")) return new VTKDataIO; - else if (!strcasecmp(typestr.c_str(), "unsigned_char")) return new VTKDataIO; - else if (!strcasecmp(typestr.c_str(), "short")) return new VTKDataIO; - else if (!strcasecmp(typestr.c_str(), "unsigned_short")) return new VTKDataIO; - else if (!strcasecmp(typestr.c_str(), "int")) return new VTKDataIO; - else if (!strcasecmp(typestr.c_str(), "unsigned_int")) return new VTKDataIO; - else if (!strcasecmp(typestr.c_str(), "long")) return new VTKDataIO; - else if (!strcasecmp(typestr.c_str(), "unsigned_long")) return new VTKDataIO; - else if (!strcasecmp(typestr.c_str(), "float")) return new VTKDataIO; - else if (!strcasecmp(typestr.c_str(), "double")) return new VTKDataIO; - else return NULL; -} - -bool Mesh::loadVtk(const char* filename) -{ - std::ifstream inVTKFile(filename, std::ifstream::in & std::ifstream::binary); - if( !inVTKFile.is_open() ) - { - return false; - } - std::string line; - - // Part 1 - std::getline(inVTKFile, line); - if (std::string(line,0,23) != "# vtk DataFile Version ") return false; - std::string version(line,23); - - // Part 2 - std::string header; - std::getline(inVTKFile, header); - - // Part 3 - std::getline(inVTKFile, line); - - bool binary; - if (line == "BINARY") binary = true; - else if (line == "ASCII") binary = false; - else return false; - - // Part 4 - do - std::getline(inVTKFile, line); - while (line == ""); - if (line != "DATASET POLYDATA") - { - return false; - } - - std::cout << (binary ? "Binary" : "Text") << " VTK File (version " << version << "): " << header << std::endl; - BaseVTKDataIO* inputPoints = NULL; - VTKDataIO* inputPolygons = NULL; - int flags = MESH_POINTS_POSITION|MESH_FACES; - int nbp = 0, nbf = 0; - while(!inVTKFile.eof()) - { - std::getline(inVTKFile, line); - std::istringstream ln(line); - std::string kw; - ln >> kw; - if (kw == "POINTS") - { - int n; - std::string typestr; - ln >> n >> typestr; - std::cout << "Found " << n << " " << typestr << " points" << std::endl; - inputPoints = newVTKDataIO(typestr); - if (inputPoints == NULL) return false; - if (!inputPoints->read(inVTKFile, 3*n, binary)) - { - if (inputPoints) delete inputPoints; - if (inputPolygons) delete inputPolygons; - return false; - } - nbp = n; - } - else if (kw == "POLYGONS") - { - int n, ni; - ln >> n >> ni; - std::cout << "Found " << n << " polygons ( " << (ni - 3*n) << " triangles )" << std::endl; - inputPolygons = new VTKDataIO; - if (!inputPolygons->read(inVTKFile, ni, binary)) - { - if (inputPoints) delete inputPoints; - if (inputPolygons) delete inputPolygons; - return false; - } -// nbf = ni - 3*n; - } - else - std::cerr << "WARNING: Unknown keyword " << kw << std::endl; - if (inputPoints && inputPolygons) break; // already found the mesh description, skip the rest - } - const int* inFP = inputPolygons->data; - nbf = 0; - for (int i=0; i < inputPolygons->dataSize;) - { - int nv = inFP[i]; ++i; - if (nv >= 3) - nbf += nv - 2; - i += nv; - } - //std::cout << "init " << nbp << " points and " << nbf << " faces ( flags = " << flags << " )" << std::endl; - init(nbp, nbf, flags); - inputPoints->copyTo(points_p); - nbf = 0; - for (int i=0; i < inputPolygons->dataSize;) - { - int nv = inFP[i]; ++i; - if (nv >= 3) - { - Vec3i f; - f[0] = inFP[i+0]; - f[1] = inFP[i+1]; - for (int j=2;j out; - out.resize(np*3); - for (int p=0;p out; - out.resize(4*nf); - for (int f=0;f. * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#include - -#include -#include - -#include - -#if SOFADISTANCEGRID_HAVE_MINIFLOWVR -#include -#endif - -#include -#include - -#include - -#define FMM_VERBOSE false - -namespace sofa -{ - -using helper::rabs; -using helper::rmax; - -namespace component -{ - -namespace container -{ - -namespace _distancegrid_ -{ - -std::map > DistanceGrid::instances; - -using namespace defaulttype; - -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// - -const Coord calcCellWidth(const int nx, const int ny,const int nz, - const Coord& pmin, const Coord& pmax) -{ - return Coord((pmax[0]-pmin[0])/(nx-1), (pmax[1]-pmin[1])/(ny-1),(pmax[2]-pmin[2])/(nz-1)) ; -} - -const Coord calcInvCellWidth(const int nx, const int ny,const int nz, - const Coord& pmin, const Coord& pmax) -{ - return Coord((nx-1)/(pmax[0]-pmin[0]), (ny-1)/(pmax[1]-pmin[1]),(nz-1)/(pmax[2]-pmin[2])) ; -} - -int validateDim(int n) -{ - if(n<0){ - dmsg_warning("DistanceGrid") << "Invalid dimension" << n << ". Dimension for a distance grid must be of size >= 0. Use '0' instead. " ; - n=0; - } - return n; -} - -DistanceGrid::DistanceGrid(int nx, int ny, int nz, Coord pmin, Coord pmax) - : meshPts() - , m_nx(validateDim(nx)), m_ny(validateDim(ny)), m_nz(validateDim(nz)) - , m_nxny(m_nx*m_ny), m_nxnynz(m_nx*m_ny*m_nz) - , m_dists(m_nx*m_ny*m_nz) - , m_pmin(pmin), m_pmax(pmax) - , m_cellWidth (calcCellWidth(m_nx,m_ny,m_nz,pmin,pmax)) - , m_invCellWidth(calcInvCellWidth(m_nx,m_ny,m_nz,pmin,pmax)) - , m_cubeDim(0) -{ -} - -DistanceGrid::~DistanceGrid() -{} - -//todo(dmarchal) we should make a loader for that... -std::unique_ptr DistanceGrid::load(const std::string& filename, - double scale, double sampling, - int nx, int ny, int nz, Coord pmin, Coord pmax) -{ - double absscale=fabs(scale); - if (filename == "#cube") - { - float dim = (float)scale; - int np = 5; - Coord bbmin(-dim, -dim, -dim), bbmax(dim,dim,dim); - if (pmin[0]<=pmax[0]) - { - pmin = bbmin; - pmax = bbmax; - Coord margin = (bbmax-bbmin)*0.1; - pmin -= margin; - pmax += margin; - } - else - { - for (int c=0; c<3; c++) - { - if (bbmin[c] < pmin[c]) pmin[c] = bbmin[c]; - if (bbmax[c] > pmax[c]) pmax[c] = bbmax[c]; - } - } - std::unique_ptr grid = std::make_unique(nx, ny, nz, pmin, pmax); - grid->calcCubeDistance(dim, np); - if (sampling) - grid->sampleSurface(sampling); - return grid; - } - else if (filename.length()>4 && filename.substr(filename.length()-4) == ".raw") - { - std::unique_ptr grid = std::make_unique(nx, ny, nz, pmin, pmax); - std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary); - in.read((char*)&(grid->m_dists[0]), grid->m_nxnynz*sizeof(SReal)); - if (scale != 1.0) - { - for (int i=0; i< grid->m_nxnynz; i++) - grid->m_dists[i] *= (float)scale; - } - grid->computeBBox(); - if (sampling) - grid->sampleSurface(sampling); - return grid; - } - else if (filename.length()>4 && filename.substr(filename.length()-4) == ".vtk") - { - return loadVTKFile(filename, scale, sampling); - } - else if (filename.length()>6 && filename.substr(filename.length()-6) == ".fmesh") - { -#if SOFADISTANCEGRID_HAVE_MINIFLOWVR - flowvr::render::Mesh mesh; - if (!mesh.load(filename.c_str())) - { - msg_error("DistanceGrid")<<"loading FlowVR mesh file "<nx; - ny = mesh.distmap->ny; - nz = mesh.distmap->nz; - ftl::Vec3f fpmin = ftl::transform(mesh.distmap->mat,ftl::Vec3f(0,0,0))*(float)absscale; - ftl::Vec3f fpmax = ftl::transform(mesh.distmap->mat,ftl::Vec3f((float)(nx-1),(float)(ny-1),(float)(nz-1)))*(float)absscale; - pmin = Coord(fpmin.ptr()); - pmax = Coord(fpmax.ptr()); - std::unique_ptr grid = std::make_unique(nx, ny, nz, pmin, pmax); - for (int i=0; i< grid->m_nxnynz; i++) - grid->m_dists[i] = mesh.distmap->data[i]*scale; - if (sampling) - grid->sampleSurface(sampling); - else if (mesh.getAttrib(flowvr::render::Mesh::MESH_POINTS_GROUP)) - { - int nbpos = 0; - for (int i=0; i= 0) - ++nbpos; - } - grid->meshPts.resize(nbpos); - int p = 0; - for (int i=0; i= 0) - grid->meshPts[p++] = Coord(mesh.getPP(p0).ptr())*absscale; - } - } - else - { - int nbpos = mesh.nbp(); - grid->meshPts.resize(nbpos); - for (int i=0; imeshPts[i] = Coord(mesh.getPP(i).ptr())*absscale; - } - if (scale < 0) - { - grid->m_bbmin = grid->m_pmin; - grid->m_bbmax = grid->m_pmax; - } - else - grid->computeBBox(); - return grid; -#else - msg_error("DistanceGrid")<<"Loading a .fmesh file requires the FlowVR library"; - return NULL; -#endif // SOFADISTANCEGRID_HAVE_MINIFLOWVR - } - else if (filename.length()>4 && filename.substr(filename.length()-4) == ".obj") - { - Mesh* mesh = Mesh::Create(filename); - const auto & vertices = mesh->getVertices(); - - Coord bbmin, bbmax; - if (!vertices.empty()) - { - bbmin = vertices[0]; - bbmax = bbmin; - for(unsigned int i=1; i bbmax[c]) bbmax[c] = (SReal)vertices[i][c]; - } - bbmin *= absscale; - bbmax *= absscale; - } - - if (pmin[0]<=pmax[0]) - { - pmin = bbmin; - pmax = bbmax; - Coord margin = (bbmax-bbmin)*0.1; - pmin -= margin; - pmax += margin; - } - else if (!vertices.empty()) - { - for (int c=0; c<3; c++) - { - if (bbmin[c] < pmin[c]) pmin[c] = bbmin[c]; - if (bbmax[c] > pmax[c]) pmax[c] = bbmax[c]; - } - } - std::unique_ptr grid = std::make_unique(nx, ny, nz, pmin, pmax); - grid->calcDistance(mesh, scale); - if (sampling) - grid->sampleSurface(sampling); - else - { - grid->meshPts.resize(vertices.size()); - for(unsigned int i=0; imeshPts[i] = vertices[i]*absscale; - } - grid->computeBBox(); - delete mesh; - return grid; - } - else - { - msg_error("DistanceGrid")<< "Unknown extension: "<4 && filename.substr(filename.length()-4) == ".raw") - { - std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary); - out.write((char*)&(m_dists[0]), m_nxnynz*sizeof(SReal)); - } - else - { - msg_error("DistanceGrid")<<" save(): Unsupported extension: "< bool readData(std::istream& in, int dataSize, bool binary, DistanceGrid::VecSReal& data, double scale) -{ - if (binary) - { - T* buffer = new T[dataSize]; - in.read((char*)buffer, dataSize * sizeof(T)); - if (in.eof() || in.bad()) - { - delete[] buffer; - return false; - } - else - { - for (int i=0; i> buffer) - { - data[i] = (SReal)(buffer*scale); - ++i; - } - } - return (i == dataSize); - } -} - -std::unique_ptr DistanceGrid::loadVTKFile(const std::string& filename, double scale, double sampling) -{ - // Format doc: http://www.vtk.org/pdf/file-formats.pdf - // http://www.cacr.caltech.edu/~slombey/asci/vtk/vtk_formats.simple.html - - std::ifstream inVTKFile(filename.c_str(), std::ifstream::in & std::ifstream::binary); - if( !inVTKFile.is_open() ) - { - return nullptr; - } - std::string line; - - // Part 1 - std::getline(inVTKFile, line); - if (std::string(line,0,23) != "# vtk DataFile Version ") return nullptr; - std::string version(line,23); - - // Part 2 - std::string header; - std::getline(inVTKFile, header); - - // Part 3 - std::getline(inVTKFile, line); - - bool binary; - if (line == "BINARY") binary = true; - else if (line == "ASCII") binary = false; - else return nullptr; - - // Part 4 - std::getline(inVTKFile, line); - if (line != "DATASET STRUCTURED_POINTS") - { - return nullptr; - } - - msg_info("DistanceGrid")<< (binary ? "Binary" : "Text") << " VTK File " << filename << " (version " << version << "): " << header; - int dataSize = 0; - int nx = 0, ny = 0, nz = 0; - Coord origin, spacing(1.0f,1.0f,1.0f); - while(!inVTKFile.eof()) - { - std::getline(inVTKFile, line); - std::istringstream ln(line); - std::string kw; - ln >> kw; - if (kw == "DIMENSIONS") - { - ln >> nx >> ny >> nz; - } - else if (kw == "SPACING") - { - ln >> spacing[0] >> spacing[1] >> spacing[2]; - spacing *= scale; - } - else if (kw == "ORIGIN") - { - ln >> origin[0] >> origin[1] >> origin[2]; - origin *= scale; - } - else if (kw == "CELL_DATA") - { - //section = CellData; - ln >> dataSize; - } - else if (kw == "POINT_DATA") - { - //section = PointData; - ln >> dataSize; - } - else if (kw == "SCALARS") - { - std::string name, typestr; - ln >> name >> typestr; - msg_info("DistanceGrid")<< "Found " << typestr << " data: " << name; - std::getline(inVTKFile, line); // lookup_table, ignore - msg_info("DistanceGrid")<< "Loading " << nx<<"x"< grid = std::make_unique(nx, ny, nz, origin, - origin + Coord(spacing[0] * nx, spacing[1] * ny, spacing[2] * nz)); - bool ok = true; - if (typestr == "char") ok = readData(inVTKFile, dataSize, binary, grid->m_dists, scale); - else if (typestr == "unsigned_char") ok = readData(inVTKFile, dataSize, binary, grid->m_dists, scale); - else if (typestr == "short") ok = readData(inVTKFile, dataSize, binary, grid->m_dists, scale); - else if (typestr == "unsigned_short") ok = readData(inVTKFile, dataSize, binary, grid->m_dists, scale); - else if (typestr == "int") ok = readData(inVTKFile, dataSize, binary, grid->m_dists, scale); - else if (typestr == "unsigned_int") ok = readData(inVTKFile, dataSize, binary, grid->m_dists, scale); - else if (typestr == "long") ok = readData(inVTKFile, dataSize, binary, grid->m_dists, scale); - else if (typestr == "unsigned_long") ok = readData(inVTKFile, dataSize, binary, grid->m_dists, scale); - else if (typestr == "float") ok = readData(inVTKFile, dataSize, binary, grid->m_dists, scale); - else if (typestr == "double") ok = readData(inVTKFile, dataSize, binary, grid->m_dists, scale); - else - { - msg_error("DistanceGrid")<< "Invalid type " << typestr; - ok = false; - } - if (!ok) - { - return nullptr; - } - msg_info("DistanceGrid")<< "Volume data loading OK."; - grid->computeBBox(); - if (sampling) - grid->sampleSurface(sampling); - return grid; // we read one scalar field, stop here. - } - } - return nullptr; -} - -template -void * readData(std::istream& in, int dataSize, bool binary) -{ - T* buffer = new T[dataSize]; - if (binary) - { - in.read((char*)buffer, dataSize * sizeof(T)); - if (in.eof() || in.bad()) - { - delete[] buffer; - return nullptr; - } - } - else - { - int i = 0; - std::string line; - while(i < dataSize && !in.eof() && !in.bad()) - { - std::getline(in, line); - std::istringstream ln(line); - while (i < dataSize && ln >> buffer[i]) - ++i; - } - if (i < dataSize) - { - delete[] buffer; - return nullptr; - } - } - return buffer; -} - - -template -bool pointInTriangle(const Coord& p, const Coord& p0, const Coord& p1, const Coord& p2) -{ - SReal u0 = p [U] - p0[U], v0 = p [V] - p0[V]; - SReal u1 = p1[U] - p0[U], v1 = p1[V] - p0[V]; - SReal u2 = p2[U] - p0[U], v2 = p2[V] - p0[V]; - SReal alpha, beta; - if (u1 == 0) - { - beta = u0/u2; - if ( beta < 0 || beta > 1 ) return false; - alpha = (v0 - beta*v2)/v1; - if ( alpha < 0 || (alpha+beta) > 1 ) return false; - } - else - { - beta = (v0*u1 - u0*v1)/(v2*u1 - u2*v1); - if ( beta < 0 || beta > 1 ) return false; - alpha = (u0 - beta*u2)/u1; - if ( alpha < 0 || (alpha+beta) > 1 ) return false; - } - return true; -} - -int DistanceGrid::index(const Coord& p, Coord& coefs) const -{ - coefs[0] = (p[0]-m_pmin[0])*m_invCellWidth[0]; - coefs[1] = (p[1]-m_pmin[1])*m_invCellWidth[1]; - coefs[2] = (p[2]-m_pmin[2])*m_invCellWidth[2]; - int x = helper::rfloor(coefs[0]); - if (x<0) x=0; else if (x>=m_nx-1) x=m_nx-2; - coefs[0] -= x; - int y = helper::rfloor(coefs[1]); - if (y<0) y=0; else if (y>=m_ny-1) y=m_ny-2; - coefs[1] -= y; - int z = helper::rfloor(coefs[2]); - if (z<0) z=0; else if (z>=m_nz-1) z=m_nz-2; - coefs[2] -= z; - return x+m_nx*(y+m_ny*(z)); -} - -void DistanceGrid::computeBBox() -{ - if (!meshPts.empty()) - { - m_bbmin = meshPts[0]; - m_bbmax = m_bbmin; - for(unsigned int i=1; i m_bbmax[c]) m_bbmax[c] = (SReal)meshPts[i][c]; - } - } - else - { - m_bbmin = m_pmin; - m_bbmax = m_pmax; - /// \todo compute the SReal bbox from the grid content - } -} - - -/// Compute distance field for a cube of the given half-size. -/// Also create a mesh of points using np points per axis -void DistanceGrid::calcCubeDistance(SReal dim, int np) -{ - m_cubeDim = dim; - if (np > 1) - { - int nbp = np*np*np - (np-2)*(np-2)*(np-2); - meshPts.resize(nbp); - - for (int i=0,z=0; z dim2) { s[c] = dim2; out = true; } - } - SReal d; - if (out) - d = (p - s).norm(); - else - d = rmax(rmax(rabs(s[0]),rabs(s[1])),rabs(s[2])) - dim2; - m_dists[i] = d - (dim-dim2); - } - m_bbmin = Coord(-dim,-dim,-dim); - m_bbmax = Coord( dim, dim, dim); -} - -/// Compute distance field from given mesh -void DistanceGrid::calcDistance(sofa::helper::io::Mesh* mesh, double scale) -{ - m_fmm_status.resize(m_nxnynz); - m_fmm_heap.resize(m_nxnynz); - m_fmm_heap_size = 0; - dmsg_info("DistanceGrid")<< "FMM: Init."; - - std::fill(m_fmm_status.begin(), m_fmm_status.end(), FMM_FAR); - std::fill(m_dists.begin(), m_dists.end(), maxDist()); - - const auto& vertices = mesh->getVertices(); - const auto& facets = mesh->getFacets(); - - // Initialize distance of edges crossing triangles - dmsg_info("DistanceGrid")<< "FMM: Initialize distance of edges crossing triangles."; - - for (unsigned int i=0; i bbmax[c]) bbmax[c] = p1[c]; - for (int c=0; c<3; c++) - if (p2[c] < bbmin[c]) bbmin[c] = p2[c]; - else if (p2[c] > bbmax[c]) bbmax[c] = p2[c]; - - Coord normal = (p1-p0).cross(p2-p0); - normal.normalize(); - SReal d = -(p0*normal); - int ix0 = ix(bbmin)-1; if (ix0 < 0) ix0 = 0; - int iy0 = iy(bbmin)-1; if (iy0 < 0) iy0 = 0; - int iz0 = iz(bbmin)-1; if (iz0 < 0) iz0 = 0; - int ix1 = ix(bbmax)+2; if (ix1 >= m_nx) ix1 = m_nx-1; - int iy1 = iy(bbmax)+2; if (iy1 >= m_ny) iy1 = m_ny-1; - int iz1 = iz(bbmax)+2; if (iz1 >= m_nz) iz1 = m_nz-1; - for (int z=iz0; z cellWidth) continue; // no edge from this point can cross the plane - - // X edge - if (rabs(normal[0]) > 1e-6) - { - SReal dist1 = -dist / normal[0]; - int ind2 = ind+1; - if (dist1 >= -0.01*m_cellWidth[0] && dist1 <= 1.01*m_cellWidth[0]) - { - // edge crossed plane - if (pointInTriangle<1,2>(pos,p0,p1,p2)) - { - // edge crossed triangle - SReal dist2 = m_cellWidth[0] - dist1; - if (normal[0]<0) - { - // p1 is in outside, p2 inside - if (dist1 < (m_dists[ind])) - { - // nearest triangle - m_dists[ind] = dist1; - m_fmm_status[ind] = FMM_KNOWN_OUT; - } - if (dist2 < (m_dists[ind2])) - { - // nearest triangle - m_dists[ind2] = dist2; - m_fmm_status[ind2] = FMM_KNOWN_IN; - } - } - else - { - // p1 is in inside, p2 outside - if (dist1 < (m_dists[ind])) - { - // nearest triangle - m_dists[ind] = dist1; - m_fmm_status[ind] = FMM_KNOWN_IN; - } - if (dist2 < (m_dists[ind2])) - { - // nearest triangle - m_dists[ind2] = dist2; - m_fmm_status[ind2] = FMM_KNOWN_OUT; - } - } - } - } - } - - // Y edge - if (rabs(normal[1]) > 1e-6) - { - SReal dist1 = -dist / normal[1]; - int ind2 = ind+m_nx; - if (dist1 >= -0.01*m_cellWidth[1] && dist1 <= 1.01*m_cellWidth[1]) - { - // edge crossed plane - if (pointInTriangle<2,0>(pos,p0,p1,p2)) - { - // edge crossed triangle - SReal dist2 = m_cellWidth[1] - dist1; - if (normal[1]<0) - { - // p1 is in outside, p2 inside - if (dist1 < (m_dists[ind])) - { - // nearest triangle - m_dists[ind] = dist1; - m_fmm_status[ind] = FMM_KNOWN_OUT; - } - if (dist2 < (m_dists[ind2])) - { - // nearest triangle - m_dists[ind2] = dist2; - m_fmm_status[ind2] = FMM_KNOWN_IN; - } - } - else - { - // p1 is in inside, p2 outside - if (dist1 < (m_dists[ind])) - { - // nearest triangle - m_dists[ind] = dist1; - m_fmm_status[ind] = FMM_KNOWN_IN; - } - if (dist2 < (m_dists[ind2])) - { - // nearest triangle - m_dists[ind2] = dist2; - m_fmm_status[ind2] = FMM_KNOWN_OUT; - } - } - } - } - } - - // Z edge - if (rabs(normal[2]) > 1e-6) - { - SReal dist1 = -dist / normal[2]; - int ind2 = ind+m_nxny; - if (dist1 >= -0.01*m_cellWidth[2] && dist1 <= 1.01*m_cellWidth[2]) - { - // edge crossed plane - if (pointInTriangle<0,1>(pos,p0,p1,p2)) - { - // edge crossed triangle - SReal dist2 = m_cellWidth[2] - dist1; - if (normal[2]<0) - { - // p1 is in outside, p2 inside - if (dist1 < (m_dists[ind])) - { - // nearest triangle - m_dists[ind] = dist1; - m_fmm_status[ind] = FMM_KNOWN_OUT; - } - if (dist2 < (m_dists[ind2])) - { - // nearest triangle - m_dists[ind2] = dist2; - m_fmm_status[ind2] = FMM_KNOWN_IN; - } - } - else - { - // p1 is in inside, p2 outside - if (dist1 < (m_dists[ind])) - { - // nearest triangle - m_dists[ind] = dist1; - m_fmm_status[ind] = FMM_KNOWN_IN; - } - if (dist2 < (m_dists[ind2])) - { - // nearest triangle - m_dists[ind2] = dist2; - m_fmm_status[ind2] = FMM_KNOWN_OUT; - } - } - } - } - } - } - } - } - - // Update known points neighbors - for (int z=0, ind=0; z0) - { - ind2 = ind-1; - if (x>0 && m_fmm_status[ind2] >= FMM_FAR && (m_dists[ind2]) > dist2) - { - m_dists[ind2] = dist2; - fmm_push(ind2); - } - } - // X+1 - if (x0 && m_fmm_status[ind2] >= FMM_FAR && (m_dists[ind2]) > dist2) - { - m_dists[ind2] = dist2; - fmm_push(ind2); - } - } - dist2 = dist1+m_cellWidth[1]; - // Y-1 - if (y>0) - { - ind2 = ind-m_nx; - if (x>0 && m_fmm_status[ind2] >= FMM_FAR && (m_dists[ind2]) > dist2) - { - m_dists[ind2] = dist2; - fmm_push(ind2); - } - } - // Y+1 - if (y0 && m_fmm_status[ind2] >= FMM_FAR && (m_dists[ind2]) > dist2) - { - m_dists[ind2] = dist2; - fmm_push(ind2); - } - } - dist2 = dist1+m_cellWidth[2]; - // Z-1 - if (z>0) - { - ind2 = ind-m_nxny; - if (x>0 && m_fmm_status[ind2] >= FMM_FAR && (m_dists[ind2]) > dist2) - { - m_dists[ind2] = dist2; - fmm_push(ind2); - } - } - // Z+1 - if (z0 && m_fmm_status[ind2] >= FMM_FAR && (m_dists[ind2]) > dist2) - { - m_dists[ind2] = dist2; - fmm_push(ind2); - } - } - } - } - - // March through the heap - while (m_fmm_heap_size > 0) - { - int ind = fmm_pop(); - int nbin = 0, nbout = 0; - int x = ind%m_nx; - int y = (ind/m_nx)%m_ny; - int z = ind/m_nxny; - - int ind2; - SReal dist1 = m_dists[ind]; - SReal dist2 = dist1+m_cellWidth[0]; - // X-1 - if (x>0) - { - ind2 = ind-1; - if (m_fmm_status[ind2] < FMM_FAR) - { - if (m_fmm_status[ind2] == FMM_KNOWN_IN) ++nbin; else ++nbout; - } - else if ((m_dists[ind2]) > dist2) - { - m_dists[ind2] = dist2; - fmm_push(ind2); // create or update the corresponding entry in the heap - } - } - // X+1 - if (x dist2) - { - m_dists[ind2] = dist2; - fmm_push(ind2); // create or update the corresponding entry in the heap - } - } - dist2 = dist1+m_cellWidth[1]; - // Y-1 - if (y>0) - { - ind2 = ind-m_nx; - if (m_fmm_status[ind2] < FMM_FAR) - { - if (m_fmm_status[ind2] == FMM_KNOWN_IN) ++nbin; else ++nbout; - } - else if ((m_dists[ind2]) > dist2) - { - m_dists[ind2] = dist2; - fmm_push(ind2); // create or update the corresponding entry in the heap - } - } - // Y+1 - if (y dist2) - { - m_dists[ind2] = dist2; - fmm_push(ind2); // create or update the corresponding entry in the heap - } - } - dist2 = dist1+m_cellWidth[2]; - // Z-1 - if (z>0) - { - ind2 = ind-m_nxny; - if (m_fmm_status[ind2] < FMM_FAR) - { - if (m_fmm_status[ind2] == FMM_KNOWN_IN) ++nbin; else ++nbout; - } - else if ((m_dists[ind2]) > dist2) - { - m_dists[ind2] = dist2; - fmm_push(ind2); // create or update the corresponding entry in the heap - } - } - // Z+1 - if (z dist2) - { - m_dists[ind2] = dist2; - fmm_push(ind2); // create or update the corresponding entry in the heap - } - } - if (nbin && nbout) - { - msg_warning("DistanceGrid")<< "FMM WARNING: in/out conflict at cell "< <"<<(res%m_nx)<<','<<((res/m_nx)%m_ny)<<','<<(res/m_nxny)<<">="<0) - { - fmm_swap(0, m_fmm_heap_size); - int i=0; - SReal phi = (m_dists[m_fmm_heap[i]]); - while (i*2+1 < m_fmm_heap_size) - { - SReal phi1 = (m_dists[m_fmm_heap[i*2+1]]); - if (i*2+2 < m_fmm_heap_size) - { - SReal phi2 = (m_dists[m_fmm_heap[i*2+2]]); - if (phi1 < phi) - { - if (phi1 < phi2) - { - fmm_swap(i, i*2+1); - i = i*2+1; - } - else - { - fmm_swap(i, i*2+2); - i = i*2+2; - } - } - else if (phi2 < phi) - { - fmm_swap(i, i*2+2); - i = i*2+2; - } - else break; - } - else if (phi1 < phi) - { - fmm_swap(i, i*2+1); - i = i*2+1; - } - else break; - } - } - - if(FMM_VERBOSE){ - std::stringstream tmp; - tmp << "fmm_heap = ["; - for (int i=0; i="<= FMM_FRONT0) - { - i = m_fmm_status[index] - FMM_FRONT0; - - if(FMM_VERBOSE) - dmsg_info("DistanceGrid") << "fmm update <"<<(index%m_nx)<<','<<((index/m_nx)%m_ny)<<','<<(index/m_nxny)<<">="<0 && phi < (m_dists[m_fmm_heap[(i-1)/2]])) - { - fmm_swap(i,(i-1)/2); - i = (i-1)/2; - } - while (i*2+1 < m_fmm_heap_size) - { - SReal phi1 = (m_dists[m_fmm_heap[i*2+1]]); - if (i*2+2 < m_fmm_heap_size) - { - SReal phi2 = (m_dists[m_fmm_heap[i*2+2]]); - if (phi1 < phi) - { - if (phi1 < phi2) - { - fmm_swap(i, i*2+1); - i = i*2+1; - } - else - { - fmm_swap(i, i*2+2); - i = i*2+2; - } - } - else if (phi2 < phi) - { - fmm_swap(i, i*2+2); - i = i*2+2; - } - else break; - } - else if (phi1 < phi) - { - fmm_swap(i, i*2+1); - i = i*2+1; - } - else break; - } - } - else - { - if(FMM_VERBOSE) - dmsg_info("DistanceGrid") << "fmm push <"<<(index%m_nx)<<','<<((index/m_nx)%m_ny)<<','<<(index/m_nxny)<<">="<0 && phi < (m_dists[m_fmm_heap[(i-1)/2]])) - { - fmm_swap(i,(i-1)/2); - i = (i-1)/2; - } - } - - if(FMM_VERBOSE){ - std::stringstream tmp; - tmp << "fmm_heap = ["; - for (int i=0; i="< pts; - if (sampling <= -1.0 && sampling == floor(sampling)) - { - int stepX, stepY, stepZ; - stepX = stepY = stepZ = (int)(-sampling); - msg_info("DistanceGrid")<< "sampling steps: " << stepX << " " << stepY << " " << stepZ; - - SReal maxD = (SReal)sqrt((m_cellWidth[0]*stepX)*(m_cellWidth[0]*stepX) + (m_cellWidth[1]*stepY)*(m_cellWidth[1]*stepY) + (m_cellWidth[2]*stepZ)*(m_cellWidth[2]*stepZ)); - for (int z=1; z maxD) continue; - - type::Vec3 pos = coord(x,y,z); - type::Vec3 n = grad(index(x,y,z), Coord()); // note that there are some redundant computations between interp() and grad() - n.normalize(); - pos -= n * (d * 0.99); // push pos back to the surface - d = interp(pos); - int it = 1; - while (rabs(d) > 0.01f*maxD && it < 10) - { - n = grad(pos); - n.normalize(); - pos -= n * (d * 0.99); // push pos back to the surface - d = interp(pos); - ++it; - } - if (it == 10 && rabs(d) > 0.1f*maxD) - { - msg_warning("DistanceGrid") - << "Failed to converge at ("< maxD) continue; - type::Vec3 n = grad(pos); - n.normalize(); - pos -= n * (d * 0.99); // push pos back to the surface - d = interp(pos); - int it = 1; - while (rabs(d) > 0.01f*maxD && it < 10) - { - n = grad(pos); - n.normalize(); - pos -= n * (d * 0.99); // push pos back to the surface - d = interp(pos); - ++it; - } - if (it == 10 && rabs(d) > 0.1f*maxD) - { - msg_warning("DistanceGrid")<< "Failed to converge at ("< DistanceGrid::loadShared(const std::string& filename, - double scale, double sampling, int nx, int ny, int nz, Coord pmin, Coord pmax) -{ - DistanceGridParams params; - params.filename = filename; - params.scale = scale; - params.sampling = sampling; - params.nx = nx; - params.ny = ny; - params.nz = nz; - params.pmin = pmin; - params.pmax = pmax; - std::map >::iterator it = instances.find(params); - if (it != instances.end()) - { - if (!it->second.expired()) - return it->second.lock(); - } - std::shared_ptr grid = load(filename, scale, sampling, nx, ny, nz, pmin, pmax); - instances[params] = grid; - return instances[params].lock(); -} - - -SReal DistanceGrid::quickeval(const Coord& x) const -{ - SReal d; - if (inGrid(x)) - { - d = m_dists[index(x)] - m_cellWidth[0]; // we underestimate the distance - } - else - { - Coord xclamp = clamp(x); - d = m_dists[index(xclamp)] - m_cellWidth[0]; // we underestimate the distance - d = helper::rsqrt((x-xclamp).norm2() + d*d); - } - return d; -} - -SReal DistanceGrid::eval2(const Coord& x) const -{ - SReal d2; - if (inGrid(x)) - { - SReal d = interp(x); - d2 = d*d; - } - else - { - Coord xclamp = clamp(x); - SReal d = interp(xclamp); - d2 = ((x-xclamp).norm2() + d*d); // we underestimate the distance - } - return d2; -} - -SReal DistanceGrid::quickeval2(const Coord& x) const -{ - SReal d2; - if (inGrid(x)) - { - SReal d = m_dists[index(x)] - m_cellWidth[0]; // we underestimate the distance - d2 = d*d; - } - else - { - Coord xclamp = clamp(x); - SReal d = m_dists[index(xclamp)] - m_cellWidth[0]; // we underestimate the distance - d2 = ((x-xclamp).norm2() + d*d); - } - return d2; -} - -SReal DistanceGrid::interp(int index, const Coord& coefs) const -{ - return interp(coefs[2],interp(coefs[1],interp(coefs[0],m_dists[index ],m_dists[index+1 ]), - interp(coefs[0],m_dists[index +m_nx ],m_dists[index+1+m_nx ])), - interp(coefs[1],interp(coefs[0],m_dists[index +m_nxny],m_dists[index+1 +m_nxny]), - interp(coefs[0],m_dists[index +m_nx+m_nxny],m_dists[index+1+m_nx+m_nxny]))); -} - - -SReal DistanceGrid::interp(const Coord& p) const -{ - Coord coefs; - int i = index(p, coefs); - return interp(i, coefs); -} - -Coord DistanceGrid::grad(int index, const Coord& coefs) const -{ - // val = dist[0][0][0] * (1-x) * (1-y) * (1-z) - // + dist[1][0][0] * ( x) * (1-y) * (1-z) - // + dist[0][1][0] * (1-x) * ( y) * (1-z) - // + dist[1][1][0] * ( x) * ( y) * (1-z) - // + dist[0][0][1] * (1-x) * (1-y) * ( z) - // + dist[1][0][1] * ( x) * (1-y) * ( z) - // + dist[0][1][1] * (1-x) * ( y) * ( z) - // + dist[1][1][1] * ( x) * ( y) * ( z) - // dval / dx = (dist[1][0][0]-dist[0][0][0]) * (1-y) * (1-z) - // + (dist[1][1][0]-dist[0][1][0]) * ( y) * (1-z) - // + (dist[1][0][1]-dist[0][0][1]) * (1-y) * ( z) - // + (dist[1][1][1]-dist[0][1][1]) * ( y) * ( z) - const SReal dist000 = m_dists[index ]; - const SReal dist100 = m_dists[index+1 ]; - const SReal dist010 = m_dists[index +m_nx ]; - const SReal dist110 = m_dists[index+1+m_nx ]; - const SReal dist001 = m_dists[index +m_nxny]; - const SReal dist101 = m_dists[index+1 +m_nxny]; - const SReal dist011 = m_dists[index +m_nx+m_nxny]; - const SReal dist111 = m_dists[index+1+m_nx+m_nxny]; - return Coord( - interp(coefs[2],interp(coefs[1],dist100-dist000,dist110-dist010),interp(coefs[1],dist101-dist001,dist111-dist011)), //*invCellWidth[0], - interp(coefs[2],interp(coefs[0],dist010-dist000,dist110-dist100),interp(coefs[0],dist011-dist001,dist111-dist101)), //*invCellWidth[1], - interp(coefs[1],interp(coefs[0],dist001-dist000,dist101-dist100),interp(coefs[0],dist011-dist010,dist111-dist110))); //*invCellWidth[2]); -} - -Coord DistanceGrid::grad(const Coord& p) const -{ - Coord coefs; - int i = index(p, coefs); - return grad(i, coefs); -} - -SReal DistanceGrid::eval(const Coord& x) const -{ - SReal d; - if (inGrid(x)) - { - d = interp(x); - } - else - { - Coord xclamp = clamp(x); - d = interp(xclamp); - d = helper::rsqrt((x-xclamp).norm2() + d*d); // we underestimate the distance - } - return d; -} - -bool DistanceGrid::DistanceGridParams::operator==(const DistanceGridParams& v) const -{ - if (!(filename == v.filename)) return false; - if (!(scale == v.scale )) return false; - if (!(sampling == v.sampling)) return false; - if (!(nx == v.nx )) return false; - if (!(ny == v.ny )) return false; - if (!(nz == v.nz )) return false; - if (!(pmin[0] == v.pmin[0] )) return false; - if (!(pmin[1] == v.pmin[1] )) return false; - if (!(pmin[2] == v.pmin[2] )) return false; - if (!(pmax[0] == v.pmax[0] )) return false; - if (!(pmax[1] == v.pmax[1] )) return false; - if (!(pmax[2] == v.pmax[2] )) return false; - return true; -} - -bool DistanceGrid::DistanceGridParams::operator<(const DistanceGridParams& v) const -{ - if (filename < v.filename) return false; - if (filename > v.filename) return true; - if (scale < v.scale ) return false; - if (scale > v.scale ) return true; - if (sampling < v.sampling) return false; - if (sampling > v.sampling) return true; - if (nx < v.nx ) return false; - if (nx > v.nx ) return true; - if (ny < v.ny ) return false; - if (ny > v.ny ) return true; - if (nz < v.nz ) return false; - if (nz > v.nz ) return true; - if (pmin[0] < v.pmin[0] ) return false; - if (pmin[0] > v.pmin[0] ) return true; - if (pmin[1] < v.pmin[1] ) return false; - if (pmin[1] > v.pmin[1] ) return true; - if (pmin[2] < v.pmin[2] ) return false; - if (pmin[2] > v.pmin[2] ) return true; - if (pmax[0] < v.pmax[0] ) return false; - if (pmax[0] > v.pmax[0] ) return true; - if (pmax[1] < v.pmax[1] ) return false; - if (pmax[1] > v.pmax[1] ) return true; - if (pmax[2] < v.pmax[2] ) return false; - if (pmax[2] > v.pmax[2] ) return true; - return false; -} - -bool DistanceGrid::DistanceGridParams::operator>(const DistanceGridParams& v) const -{ - if (filename > v.filename) return false; - if (filename < v.filename) return true; - if (scale > v.scale ) return false; - if (scale < v.scale ) return true; - if (sampling < v.sampling) return false; - if (sampling > v.sampling) return true; - if (nx > v.nx ) return false; - if (nx < v.nx ) return true; - if (ny > v.ny ) return false; - if (ny < v.ny ) return true; - if (nz > v.nz ) return false; - if (nz < v.nz ) return true; - if (pmin[0] > v.pmin[0] ) return false; - if (pmin[0] < v.pmin[0] ) return true; - if (pmin[1] > v.pmin[1] ) return false; - if (pmin[1] < v.pmin[1] ) return true; - if (pmin[2] > v.pmin[2] ) return false; - if (pmin[2] < v.pmin[2] ) return true; - if (pmax[0] > v.pmax[0] ) return false; - if (pmax[0] < v.pmax[0] ) return true; - if (pmax[1] > v.pmax[1] ) return false; - if (pmax[1] < v.pmax[1] ) return true; - if (pmax[2] > v.pmax[2] ) return false; - if (pmax[2] < v.pmax[2] ) return true; - return false; -} - -} // namespace _distancegrid_ - -} // namespace container - -} // namespace component - -} // namespace sofa diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/DistanceGrid.h b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/DistanceGrid.h deleted file mode 100644 index ba4a686f495..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/DistanceGrid.h +++ /dev/null @@ -1,264 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_SOFADISTANCEGRID_DISTANCEGRID_H -#define SOFA_SOFADISTANCEGRID_DISTANCEGRID_H -#include - -#include -#include -#include - -///// Forward declaration -namespace sofa::helper::io -{ - class Mesh ; -} - - -////// DistanceGrid declaration -namespace sofa -{ -//todo(dmarchal) I see no reason why this is into component as DistanceGrid obviously isn't one -// can someone suggest a refactoring to have things in the right location. -namespace component -{ -namespace container -{ - - -/// Private namespace to avoid leaking data types into the files. -namespace _distancegrid_ -{ -using sofa::helper::io::Mesh; -using sofa::type::Vec3 ; -typedef Vec3 Coord; - -class SOFA_SOFADISTANCEGRID_API DistanceGrid -{ -public: - static SReal maxDist() { return std::numeric_limits::max(); } - typedef type::Vec3 Coord; - typedef type::vector VecSReal; - typedef type::vector VecCoord; - - DistanceGrid(int m_nx, int m_ny, int m_nz, Coord m_pmin, Coord m_pmax); - - ~DistanceGrid(); - -public: - /// Load a distance grid - static std::unique_ptr load(const std::string& filename, - double scale=1.0, double sampling=0.0, - int m_nx=64, int m_ny=64, int m_nz=64, - Coord m_pmin = Coord(), Coord m_pmax = Coord()); - - static std::unique_ptr loadVTKFile(const std::string& filename, - double scale=1.0, double sampling=0.0); - - /// Load or reuse a distance grid - static std::shared_ptr loadShared(const std::string& filename, - double scale=1.0, double sampling=0.0, - int m_nx=64, int m_ny=64, int m_nz=64, - Coord m_pmin = Coord(), Coord m_pmax = Coord()); - - /// Save current grid - bool save(const std::string& filename); - - /// Compute distance field from given mesh - void calcDistance(Mesh* mesh, double scale=1.0); - - /// Compute distance field for a cube of the given half-size. - /// Also create a mesh of points using np points per axis - void calcCubeDistance(SReal dim=1, int np=5); - - /// Sample the surface with points approximately separated by the given sampling distance - /// (expressed in voxels if the value is negative) - void sampleSurface(double sampling=-1.0); - - /// Update bbox - void computeBBox(); - - inline int getNx() const { return m_nx; } - inline int getNy() const { return m_ny; } - inline int getNz() const { return m_nz; } - inline const Coord& getCellWidth() const { return m_cellWidth; } - - inline int size() const { return m_nxnynz; } - - inline const Coord& getBBMin() const { return m_bbmin; } - inline const Coord& getBBMax() const { return m_bbmax; } - inline void setBBMin(const Coord& val) { m_bbmin = val; } - inline void setBBMax(const Coord& val) { m_bbmax = val; } - inline Coord getBBCorner(int i) const { - return Coord((i&1)?m_bbmax[0]:m_bbmin[0],(i&2)?m_bbmax[1]:m_bbmin[1],(i&4)?m_bbmax[2]:m_bbmin[2]); - } - - inline bool inBBox(const Coord& p, SReal margin=0.0f) const - { - for (int c=0; c<3; ++c) - if (p[c] < m_bbmin[c]-margin || p[c] > m_bbmax[c]+margin) return false; - return true; - } - - inline const Coord& getPMin() const { return m_pmin; } - inline const Coord& getPMax() const { return m_pmax; } - inline Coord getCorner(int i) const { - return Coord((i&1)?m_pmax[0]:m_pmin[0],(i&2)?m_pmax[1]:m_pmin[1],(i&4)?m_pmax[2]:m_pmin[2]); - } - - inline bool isCube() const { return m_cubeDim != 0; } - inline SReal getCubeDim() const { return m_cubeDim; } - - bool inGrid(const Coord& p) const - { - Coord epsilon = m_cellWidth*0.1; - for (int c=0; c<3; ++c) - if (p[c] < m_pmin[c]+epsilon[c] || p[c] > m_pmax[c]-epsilon[c]) return false; - return true; - } - - Coord clamp(Coord p) const - { - for (int c=0; c<3; ++c) - if (p[c] < m_pmin[c]) p[c] = m_pmin[c]; - else if (p[c] > m_pmax[c]) p[c] = m_pmax[c]; - return p; - } - - int ix(const Coord& p) const - { - return helper::rfloor((p[0]-m_pmin[0])*m_invCellWidth[0]); - } - - int iy(const Coord& p) const - { - return helper::rfloor((p[1]-m_pmin[1])*m_invCellWidth[1]); - } - - int iz(const Coord& p) const - { - return helper::rfloor((p[2]-m_pmin[2])*m_invCellWidth[2]); - } - - int index(const Coord& p, Coord& coefs) const ; - - int index(const Coord& p) const - { - Coord coefs; - return index(p, coefs); - } - - int index(int x, int y, int z) - { - return x+m_nx*(y+m_ny*(z)); - } - - Coord coord(int x, int y, int z) - { - return m_pmin+Coord(x*m_cellWidth[0], y*m_cellWidth[1], z*m_cellWidth[2]); - } - - SReal operator[](int index) const { return m_dists[index]; } - SReal& operator[](int index) { return m_dists[index]; } - - static SReal interp(SReal coef, SReal a, SReal b) - { - return a+coef*(b-a); - } - - SReal interp(int index, const Coord& coefs) const ; - SReal interp(const Coord& p) const ; - Coord grad(int index, const Coord& coefs) const ; - Coord grad(const Coord& p) const ; - SReal eval(const Coord& x) const ; - SReal quickeval(const Coord& x) const ; - SReal eval2(const Coord& x) const ; - SReal quickeval2(const Coord& x) const ; - - template - T tgrad(const T& p) const - { - Coord cp; - for (unsigned int i=0; i < cp.size() && i < p.size(); ++i) cp[i] = (SReal) p[i]; - Coord cr = grad(cp); - T r; - for (unsigned int i=0; i < cr.size() && i < p.size(); ++i) r[i] = (typename T::value_type) cr[i]; - return r; - } - - template - typename T::value_type teval(const T& x) const - { - Coord cx; - for (unsigned int i=0; i < cx.size() && i < x.size(); ++i) cx[i] = (SReal) x[i]; - return (typename T::value_type) eval(cx); - } - - VecCoord meshPts; - -protected: - const int m_nx,m_ny,m_nz; - const int m_nxny, m_nxnynz; - VecSReal m_dists; - const Coord m_pmin, m_pmax; - const Coord m_cellWidth, m_invCellWidth; - Coord m_bbmin, m_bbmax; ///< bounding box of the object, smaller than the grid - - SReal m_cubeDim; ///< Cube dimension (!=0 if this is actually a cube - - /// Fast Marching Method Update - enum Status { FMM_FRONT0 = 0, FMM_FAR = -1, FMM_KNOWN_OUT = -2, FMM_KNOWN_IN = -3 }; - type::vector m_fmm_status; - type::vector m_fmm_heap; - int m_fmm_heap_size; - - int fmm_pop(); - void fmm_push(int index); - void fmm_swap(int entry1, int entry2); - - /// Grid shared resources - struct DistanceGridParams - { - std::string filename; - double scale; - double sampling; - int nx,ny,nz; - Coord pmin,pmax; - bool operator==(const DistanceGridParams& v) const ; - bool operator<(const DistanceGridParams& v) const ; - bool operator>(const DistanceGridParams& v) const ; - }; - - static std::map > instances; -}; - -} // namespace _distancegrid - -using _distancegrid_::DistanceGrid ; - -} // namespace container - -} // namespace component - -} // namespace sofa - -#endif diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/RegisterModelToCollisionFactory.cpp b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/RegisterModelToCollisionFactory.cpp deleted file mode 100644 index 210876816cd..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/RegisterModelToCollisionFactory.cpp +++ /dev/null @@ -1,89 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#include - -#include -using sofa::core::collision::Contact ; - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -#include "components/collision/DistanceGridCollisionModel.h" -#include "components/collision/FFDDistanceGridDiscreteIntersection.h" - - - -namespace sofa -{ - -namespace component -{ - -namespace collision -{ - -int registerDistanceGridCollisionModel() -{ - return 0; -} - -using namespace sofa::component::collision::response::contact; -using namespace sofa::component::collision::geometry; - -///////////////////////////////// BARYCENTRICSTICK ///////////////////////////////////////////////// -Creator > DistanceGridDistanceGridStickContactClass("StickContactForceField", true); -Creator> > DistanceGridPointStickContactClass("StickContactForceField", true); -Creator> > DistanceGridSphereStickContactClass("StickContactForceField", true); -Creator> > DistanceGridTriangleStickContactClass("StickContactForceField", true); - -Creator > FFDDistanceGridStickContactClass("StickContactForceField", true); -Creator > FFDDistanceGridRigidDistanceGridStickContactClass("StickContactForceField", true); -Creator> > FFDDistanceGridPointStickContactClass("StickContactForceField", true); -Creator> > FFDDistanceGridSphereStickContactClass("StickContactForceField", true); -Creator> > FFDDistanceGridTriangleStickContactClass("StickContactForceField", true); - -/////////////////////////////////////// FRICTION /////////////////////////////////////////////////// -Creator > DistanceGridDistanceGridFrictionContactClass("FrictionContactConstraint", true); -Creator> > DistanceGridPointFrictionContactClass("FrictionContactConstraint", true); -Creator> > DistanceGridSphereFrictionContactClass("FrictionContactConstraint", true); -Creator> > DistanceGridTriangleFrictionContactClass("FrictionContactConstraint", true); - -Creator > FFDDistanceGridFrictionContactClass("FrictionContactConstraint", true); -Creator > FFDDistanceGridRigidDistanceGridFrictionContactClass("FrictionContactConstraint", true); -Creator> > FFDDistanceGridPointFrictionContactClass("FrictionContactConstraint", true); -Creator> > FFDDistanceGridSphereFrictionContactClass("FrictionContactConstraint", true); -Creator> > FFDDistanceGridTriangleFrictionContactClass("FrictionContactConstraint", true); - - -} /// collision -} /// component -} /// sofa diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/RegisterModelToCollisionFactory.h b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/RegisterModelToCollisionFactory.h deleted file mode 100644 index b9ede8c6ba7..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/RegisterModelToCollisionFactory.h +++ /dev/null @@ -1,34 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ - -namespace sofa -{ - -namespace component -{ - -namespace collision -{ - int registerDistanceGridCollisionModel() ; -} /// collision -} /// component -} /// sofa diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/BarycentricPenalityContact_DistanceGrid.cpp b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/BarycentricPenalityContact_DistanceGrid.cpp deleted file mode 100644 index cda3303eca0..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/BarycentricPenalityContact_DistanceGrid.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#include -#include -#include - -#include -#include -#include - -#include "DistanceGridCollisionModel.h" - -namespace sofa -{ - -namespace component -{ - -namespace collision -{ - -using namespace sofa::core::collision; -using namespace sofa::component::collision::response::contact; -using namespace sofa::component::collision::geometry; - -Creator > DistanceGridDistanceGridContactClass("PenalityContactForceField", true); -Creator> > DistanceGridPointContactClass("PenalityContactForceField", true); -Creator> > DistanceGridSphereContactClass("PenalityContactForceField", true); -Creator> > DistanceGridTriangleContactClass("PenalityContactForceField", true); - -Creator > FFDDistanceGridContactClass("PenalityContactForceField", true); -Creator > FFDDistanceGridRigidDistanceGridContactClass("PenalityContactForceField", true); -Creator> > FFDDistanceGridPointContactClass("PenalityContactForceField", true); -Creator> > FFDDistanceGridSphereContactClass("PenalityContactForceField", true); -Creator> > FFDDistanceGridTriangleContactClass("PenalityContactForceField", true); - -} // namespace collision - -} // namespace component - -} // namespace sofa - diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/DistanceGridCollisionModel.cpp b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/DistanceGridCollisionModel.cpp deleted file mode 100644 index f10b376ac7c..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/DistanceGridCollisionModel.cpp +++ /dev/null @@ -1,783 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#define SOFA_COMPONENT_COLLISION_DISTANCEGRIDCOLLISIONMODEL_CPP -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "DistanceGridCollisionModel.h" - -#if SOFADISTANCEGRID_HAVE_SOFA_GL == 1 -#include -#include -#endif // SOFADISTANCEGRID_HAVE_SOFA_GL == 1 - -namespace sofa -{ - -namespace component -{ - -namespace collision -{ - -void registerRigidDistanceGridCollisionModel(sofa::core::ObjectFactory* factory) -{ - factory->registerObjects(sofa::core::ObjectRegistrationData("Grid-based distance field.") - .add< RigidDistanceGridCollisionModel >()); -} - -void registerFFDDistanceGridCollisionModel(sofa::core::ObjectFactory* factory) -{ - factory->registerObjects(sofa::core::ObjectRegistrationData("Grid-based deformable distance field.") - .add< FFDDistanceGridCollisionModel >()); -} - -using namespace sofa::type; -using namespace defaulttype; -using namespace sofa::component::collision::geometry; -using namespace sofa::component::collision::response::mapper; - -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// - -RigidDistanceGridCollisionModel::RigidDistanceGridCollisionModel() - : modified(true) - , fileRigidDistanceGrid( initData( &fileRigidDistanceGrid, "filename", "Load distance grid from specified file")) - , scale( initData( &scale, 1.0, "scale", "scaling factor for input file")) - , translation( initData( &translation, "translation", "translation to apply to input file")) - , rotation( initData( &rotation, "rotation", "rotation to apply to input file")) - , sampling( initData( &sampling, 0.0, "sampling", "if not zero: sample the surface with points approximately separated by the given sampling distance (expressed in voxels if the value is negative)")) - , box( initData( &box, "box", "Field bounding box defined by xmin,ymin,zmin, xmax,ymax,zmax") ) - , nx( initData( &nx, 64, "nx", "number of values on X axis") ) - , ny( initData( &ny, 64, "ny", "number of values on Y axis") ) - , nz( initData( &nz, 64, "nz", "number of values on Z axis") ) - , dumpfilename( initData( &dumpfilename, "dumpfilename","write distance grid to specified file")) - , usePoints( initData( &usePoints, true, "usePoints", "use mesh vertices for collision detection")) - , flipNormals( initData( &flipNormals, false, "flipNormals", "reverse surface direction, i.e. points are considered in collision if they move outside of the object instead of inside")) - , showMeshPoints( initData( &showMeshPoints, true, "showMeshPoints", "Enable rendering of mesh points")) - , showGridPoints( initData( &showGridPoints, false, "showGridPoints", "Enable rendering of grid points")) - , showMinDist ( initData( &showMinDist, 0.0, "showMinDist", "Min distance to render gradients")) - , showMaxDist ( initData( &showMaxDist, 0.0, "showMaxDist", "Max distance to render gradients")) -{ - addAlias(&fileRigidDistanceGrid,"fileRigidDistanceGrid"); -} - -RigidDistanceGridCollisionModel::~RigidDistanceGridCollisionModel() -{} - -void RigidDistanceGridCollisionModel::init() -{ - Inherit1::init(); - Inherit2::init(); - - if (!this->mstate) - { - msg_error() << "MechanicalState is empty. Either no MechanicalState object was found in" - " current context or the one provided is not using a Rigid type template."; - return; - } - - std::shared_ptr grid; - if (fileRigidDistanceGrid.getValue().empty()) - { - if (elems.size() == 0 || elems[0].grid == nullptr) - msg_error() << "An input filename is required."; - // else the grid has already been set - return; - } - msg_info() << "Creating "<-<"<"; - - grid = DistanceGrid::loadShared(fileRigidDistanceGrid.getFullPath(), scale.getValue(), sampling.getValue(), nx.getValue(),ny.getValue(),nz.getValue(),box.getValue()[0],box.getValue()[1]); - if (grid->getNx() != this->nx.getValue()) - this->nx.setValue(grid->getNx()); - if (grid->getNy() != this->ny.getValue()) - this->ny.setValue(grid->getNy()); - if (grid->getNz() != this->nz.getValue()) - this->nz.setValue(grid->getNz()); - resize(1); - elems[0].grid = grid; - if (grid && !dumpfilename.getValue().empty()) - { - msg_info() << "Dump grid to "<save(dumpfilename.getFullPath()); - } - updateState(); - msg_info() << "Initialisation done."; -} - -void RigidDistanceGridCollisionModel::resize(sofa::Size s) -{ - this->core::CollisionModel::resize(s); - elems.resize(s); -} - -void RigidDistanceGridCollisionModel::setNewState(sofa::Index index, double dt, const std::shared_ptr grid, const Matrix3& rotation, const Vec3& translation) -{ - elems[index].prevGrid = elems[index].grid; - elems[index].grid = grid; - elems[index].prevRotation = elems[index].rotation; - elems[index].rotation = rotation; - elems[index].prevTranslation = elems[index].translation; - elems[index].translation = translation; - if (!elems[index].isTransformed) - { - Matrix3 I; I.identity(); - if (!(rotation == I) || !(translation == Vec3())) - elems[index].isTransformed = true; - } - elems[index].prevDt = dt; - modified = true; -} - -/// Update transformation matrices from current rigid state -void RigidDistanceGridCollisionModel::updateState() -{ - const Vec3& initTranslation = this->translation.getValue(); - const Vec3& initRotation = this->rotation.getValue(); - bool useInitTranslation = (initTranslation != DistanceGrid::Coord()); - bool useInitRotation = (initRotation != Vec3(0,0,0)); - - for (sofa::Size i=0; imstate) - { - const RigidTypes::Coord& xform =(this->mstate->read(core::vec_id::read_access::position)->getValue())[i]; - elems[i].translation = xform.getCenter(); - xform.getOrientation().toMatrix(elems[i].rotation); - if (useInitRotation) - elems[i].rotation = getInitRotation(); - if (useInitTranslation) - elems[i].translation += elems[i].rotation * initTranslation; - elems[i].isTransformed = true; - } - else - { - if(useInitRotation) - { - elems[i].rotation = getInitRotation(); - elems[i].isTransformed = true; - } - if(useInitTranslation) - { - elems[i].translation = initTranslation; - } - - } - } -} - -/// Create or update the bounding volume hierarchy. -void RigidDistanceGridCollisionModel::computeBoundingTree(int maxDepth) -{ - CubeCollisionModel* cubeModel = this->createPrevious(); - - if (!modified && !isMoving() && !cubeModel->empty()) return; // No need to recompute BBox if immobile - - updateGrid(); - updateState(); - - const bool flipped = isFlipped(); - cubeModel->resize(size); - for (sofa::Size i=0; igetCorner(j) : elems[i].grid->getBBCorner(j)); - if (j == 0) - { - emin = corner; - emax = emin; - } - else - { - for(int c=0; c<3; c++) - if (corner[c] < emin[c]) emin[c] = corner[c]; - else if (corner[c] > emax[c]) emax[c] = corner[c]; - } - } - } - else - { - emin = flipped ? elems[i].grid->getPMin() : elems[i].grid->getBBMin(); - emax = flipped ? elems[i].grid->getPMax() : elems[i].grid->getBBMax(); - } - cubeModel->setParentOf(i, emin, emax); // define the bounding box of the current element - } - cubeModel->computeBoundingTree(maxDepth); - modified = false; -} - -void RigidDistanceGridCollisionModel::updateGrid() -{ -} - -void RigidDistanceGridCollisionModel::drawCollisionModel(const core::visual::VisualParams* vparams) -{ -#if SOFADISTANCEGRID_HAVE_SOFA_GL == 1 - if (vparams->displayFlags().getShowWireFrame()) - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - glDisable(GL_LIGHTING); - glColor4fv(getColor4f()); - glPointSize(3); - for (unsigned int i = 0; i < elems.size(); i++) - { - draw(vparams, i); - } - glPointSize(1); - if (vparams->displayFlags().getShowWireFrame()) - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); -#endif // SOFADISTANCEGRID_HAVE_SOFA_GL == 1 -} - -void RigidDistanceGridCollisionModel::draw(const core::visual::VisualParams* ,sofa::Index index) -{ -#if SOFADISTANCEGRID_HAVE_SOFA_GL == 1 - const bool flipped = isFlipped(); - - if (elems[index].isTransformed) - { - glPushMatrix(); - Matrix4 m; - m.identity(); - m = elems[index].rotation; - m.transpose(); - m[3] = Vec4(elems[index].translation,1.0); - - sofa::gl::glMultMatrix(m.ptr()); - } - - const std::shared_ptr grid = getGrid(index); - DistanceGrid::Coord corners[8]; - for(unsigned int i=0; i<8; i++) - corners[i] = grid->getCorner(i); - if (!isSimulated()) - glColor4f(0.25f, 0.25f, 0.25f, 0.1f); - else - glColor4f(0.5f, 0.5f, 0.5f, 0.1f); - glBegin(GL_LINES); - { - sofa::gl::glVertexT(corners[0]); sofa::gl::glVertexT(corners[4]); - sofa::gl::glVertexT(corners[1]); sofa::gl::glVertexT(corners[5]); - sofa::gl::glVertexT(corners[2]); sofa::gl::glVertexT(corners[6]); - sofa::gl::glVertexT(corners[3]); sofa::gl::glVertexT(corners[7]); - sofa::gl::glVertexT(corners[0]); sofa::gl::glVertexT(corners[2]); - sofa::gl::glVertexT(corners[1]); sofa::gl::glVertexT(corners[3]); - sofa::gl::glVertexT(corners[4]); sofa::gl::glVertexT(corners[6]); - sofa::gl::glVertexT(corners[5]); sofa::gl::glVertexT(corners[7]); - sofa::gl::glVertexT(corners[0]); sofa::gl::glVertexT(corners[1]); - sofa::gl::glVertexT(corners[2]); sofa::gl::glVertexT(corners[3]); - sofa::gl::glVertexT(corners[4]); sofa::gl::glVertexT(corners[5]); - sofa::gl::glVertexT(corners[6]); sofa::gl::glVertexT(corners[7]); - } - glEnd(); - glDisable(GL_BLEND); - glDepthMask(1); - for(unsigned int i=0; i<8; i++) - corners[i] = grid->getBBCorner(i); - - if (!isSimulated()) - glColor4f(0.5f, 0.5f, 0.5f, 1.0f); - else - glColor4f(1.0f, 1.0f, 1.0f, 1.0f); - glBegin(GL_LINES); - { - sofa::gl::glVertexT(corners[0]); sofa::gl::glVertexT(corners[4]); - sofa::gl::glVertexT(corners[1]); sofa::gl::glVertexT(corners[5]); - sofa::gl::glVertexT(corners[2]); sofa::gl::glVertexT(corners[6]); - sofa::gl::glVertexT(corners[3]); sofa::gl::glVertexT(corners[7]); - sofa::gl::glVertexT(corners[0]); sofa::gl::glVertexT(corners[2]); - sofa::gl::glVertexT(corners[1]); sofa::gl::glVertexT(corners[3]); - sofa::gl::glVertexT(corners[4]); sofa::gl::glVertexT(corners[6]); - sofa::gl::glVertexT(corners[5]); sofa::gl::glVertexT(corners[7]); - sofa::gl::glVertexT(corners[0]); sofa::gl::glVertexT(corners[1]); - sofa::gl::glVertexT(corners[2]); sofa::gl::glVertexT(corners[3]); - sofa::gl::glVertexT(corners[4]); sofa::gl::glVertexT(corners[5]); - sofa::gl::glVertexT(corners[6]); sofa::gl::glVertexT(corners[7]); - } - glEnd(); - - const SReal mindist = (SReal)(this->showMinDist.isSet() ? this->showMinDist.getValue() : - -(grid->getPMax()-grid->getPMin()).norm()*0.1); - const SReal maxdist = (SReal)(this->showMaxDist.isSet() ? this->showMaxDist.getValue() : - (grid->getPMax()-grid->getPMin()).norm()*0.025); - - if (this->showGridPoints.getValue()) - { - int dnz = (grid->getNz() < 128) ? grid->getNz() : 128; - int dny = (grid->getNy() < 128) ? grid->getNy() : 128; - int dnx = (grid->getNx() < 128) ? grid->getNx() : 128; - glBegin(GL_POINTS); - if (dnx >= 2 && dny >= 2 && dnz >= 2) - { - for (int iz=0; izgetNz()-1))/(dnz-1); - for (int iy=0; iygetNy()-1))/(dny-1); - for (int ix=0; ixgetNx()-1))/(dnx-1); - DistanceGrid::Coord p = grid->coord(x,y,z); - SReal d = (*grid)[grid->index(x,y,z)]; - if (flipped) d = -d; - if (d < mindist || d > maxdist) continue; - d /= maxdist; - if (d<0) - glColor3d(1+d*0.25, 0, 1+d); - else - continue; //glColor3d(0, 1-d*0.25, 1-d); - sofa::gl::glVertexT(p); - } - } - } - } - glEnd(); - } - if (!grid->meshPts.empty() && this->showMeshPoints.getValue()) - { - glColor3d(1, 1 ,1); - glBegin(GL_POINTS); - for (unsigned int i=0; imeshPts.size(); i++) - { - DistanceGrid::Coord p = grid->meshPts[i]; - sofa::gl::glVertexT(p); - } - glEnd(); - glBegin(GL_LINES); - for (unsigned int i=0; imeshPts.size(); i++) - { - DistanceGrid::Coord p = grid->meshPts[i]; - glColor3d(1, 1 ,1); - DistanceGrid::Coord grad = grid->grad(p); - if (flipped) grad = -grad; - grad.normalize(); - for (int j = -2; j <= 2; j++) - { - DistanceGrid::Coord p2 = p + grad * (j*maxdist/2); - SReal d = grid->eval(p2); - if (flipped) d = -d; - //if (rabs(d) > maxdist) continue; - d /= maxdist; - if (d<0) - glColor3d(1+d*0.25, 0, 1+d); - else - glColor3d(0, 1-d*0.25, 1-d); - sofa::gl::glVertexT(p2); - if (j>-2 && j < 2) - sofa::gl::glVertexT(p2); - } - } - glEnd(); - } - if (elems[index].isTransformed) - { - glPopMatrix(); - } -#endif // SOFADISTANCEGRID_HAVE_SOFA_GL == 1 -} - -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// - -FFDDistanceGridCollisionModel::FFDDistanceGridCollisionModel() - : fileFFDDistanceGrid( initData( &fileFFDDistanceGrid, "filename", "Load distance grid from specified file")) - , scale( initData( &scale, 1.0, "scale", "scaling factor for input file")) - , sampling( initData( &sampling, 0.0, "sampling", "if not zero: sample the surface with points approximately separated by the given sampling distance (expressed in voxels if the value is negative)")) - , box( initData( &box, "box", "Field bounding box defined by xmin,ymin,zmin, xmax,ymax,zmax") ) - , nx( initData( &nx, 64, "nx", "number of values on X axis") ) - , ny( initData( &ny, 64, "ny", "number of values on Y axis") ) - , nz( initData( &nz, 64, "nz", "number of values on Z axis") ) - , dumpfilename( initData( &dumpfilename, "dumpfilename","write distance grid to specified file")) - , usePoints( initData( &usePoints, true, "usePoints", "use mesh vertices for collision detection")) - , singleContact( initData( &singleContact, false, "singleContact", "keep only the deepest contact in each cell")) - , l_ffdMesh(initLink("topology", "link to the topological mesh associated with this collision model")) -{ - addAlias(&fileFFDDistanceGrid,"fileFFDDistanceGrid"); - enum_type = FFDDISTANCE_GRIDE_TYPE; -} - -FFDDistanceGridCollisionModel::~FFDDistanceGridCollisionModel() -{} - -void FFDDistanceGridCollisionModel::init() -{ - Inherit1::init(); - Inherit2::init(); - if (l_ffdMesh.empty()) - { - msg_warning() << "Link to Topology should be set to a RegularGridTopology or SparseGridTopology. First Topology found in context will be used"; - l_ffdMesh.set(this->getContext()->getMeshTopologyLink()); - } - topology::container::grid::RegularGridTopology* ffdRGrid = dynamic_cast< topology::container::grid::RegularGridTopology* > (l_ffdMesh.get()); - topology::container::grid::SparseGridTopology* ffdSGrid = dynamic_cast< topology::container::grid::SparseGridTopology* > (l_ffdMesh.get()); - if (!this->mstate || (!ffdRGrid && !ffdSGrid)) - { - msg_error() << "Requires a Vec3-based deformable model with associated RegularGridTopology or SparseGridTopology"; - return; - } - - std::shared_ptr grid; - if (fileFFDDistanceGrid.getValue().empty()) - { - msg_error() << "Requires an input filename"; - return; - } - msg_info() << "Creating "<-<"<"; - - grid = DistanceGrid::loadShared(fileFFDDistanceGrid.getFullPath(), scale.getValue(), sampling.getValue(), nx.getValue(),ny.getValue(),nz.getValue(),box.getValue()[0],box.getValue()[1]); - if (!dumpfilename.getValue().empty()) - { - msg_info() << "Dump grid to "<save(dumpfilename.getFullPath()); - } - /// place points in ffd elements - int nbp = grid->meshPts.size(); - elems.resize(l_ffdMesh->getNbHexahedra()); - msg_info() << "Placing "<getNbHexahedra()<<" cubes."; - - for (int i=0; imeshPts[i]; - Vec3 bary; - sofa::Index elem = (ffdRGrid ? ffdRGrid->findCube(p0,bary[0],bary[1],bary[2]) : ffdSGrid->findCube(p0,bary[0],bary[1],bary[2])); - if (elem == sofa::InvalidID) continue; - if (elem >= elems.size()) - { - msg_error() << "point "<grad(p0); - n.normalize(); - elems[elem].normals.push_back(n); - } - } - /// fill other data and remove inactive elements - - msg_info() << "Initializing "<getNbHexahedra()<<" cubes."; - sofa::Size c=0; - for (sofa::Size e=0; egetNbHexahedra(); e++) - { - if (c != e) - elems[c].points.swap(elems[e].points); // move the list of points to the new - elems[c].elem = e; - - core::topology::BaseMeshTopology::Hexa cube = (ffdRGrid ? ffdRGrid->getHexaCopy(e) : ffdSGrid->getHexahedron(e)); - { int t = cube[2]; cube[2] = cube[3]; cube[3] = t; } - { int t = cube[6]; cube[6] = cube[7]; cube[7] = t; } - - elems[c].initP0 = GCoord(l_ffdMesh->getPX(cube[0]), l_ffdMesh->getPY(cube[0]), l_ffdMesh->getPZ(cube[0])); - elems[c].initDP = GCoord(l_ffdMesh->getPX(cube[7]), l_ffdMesh->getPY(cube[7]), l_ffdMesh->getPZ(cube[7]))-elems[c].initP0; - elems[c].invDP[0] = 1/elems[c].initDP[0]; - elems[c].invDP[1] = 1/elems[c].initDP[1]; - elems[c].invDP[2] = 1/elems[c].initDP[2]; - elems[c].grid = grid; - ++c; - } - resize(c); - - /// compute neighbors - type::vector > shells; - shells.resize(l_ffdMesh->getNbPoints()); - for (unsigned i = 0; i < elems.size(); ++i) - { - int e = elems[i].elem; - core::topology::BaseMeshTopology::Hexa cube = (ffdRGrid ? ffdRGrid->getHexaCopy(e) : ffdSGrid->getHexahedron(e)); - { int t = cube[2]; cube[2] = cube[3]; cube[3] = t; } - { int t = cube[6]; cube[6] = cube[7]; cube[7] = t; } - - for (int j=0; j<8; ++j) - shells[cube[j]].insert(i); - } - - for (unsigned i = 0; i < elems.size(); ++i) - { - int e = elems[i].elem; - core::topology::BaseMeshTopology::Hexa cube = (ffdRGrid ? ffdRGrid->getHexaCopy(e) : ffdSGrid->getHexahedron(e)); - { int t = cube[2]; cube[2] = cube[3]; cube[3] = t; } - { int t = cube[6]; cube[6] = cube[7]; cube[7] = t; } - - for (int j=0; j<8; ++j) - elems[i].neighbors.insert(shells[cube[j]].begin(), shells[cube[j]].end()); - elems[i].neighbors.erase(i); - } - - msg_info() << c <<" active cubes."; -} - -void FFDDistanceGridCollisionModel::resize(sofa::Size s) -{ - this->core::CollisionModel::resize(s); - elems.resize(s); -} - -bool FFDDistanceGridCollisionModel::canCollideWithElement(sofa::Index index, CollisionModel* model2, sofa::Index index2) -{ - if (model2 != this) return true; - if (!this->bSelfCollision.getValue()) return true; - - if (index >= index2) return false; - if (elems[index].neighbors.count(index2)) return false; - return true; -} - -/// Create or update the bounding volume hierarchy. -void FFDDistanceGridCollisionModel::computeBoundingTree(int maxDepth) -{ - CubeCollisionModel* cubeModel = this->createPrevious(); - - if (!isMoving() && !cubeModel->empty()) return; // No need to recompute BBox if immobile - - updateGrid(); - - cubeModel->resize(size); - for (sofa::Size i=0; i emax[c]) emax[c] = corner[c]; - } - } - cubeModel->setParentOf(i, emin, emax); // define the bounding box of the current element - } - cubeModel->computeBoundingTree(maxDepth); -} - -void FFDDistanceGridCollisionModel::updateGrid() -{ - for (sofa::Size index=0; index& cubeCorners = l_ffdMesh->getHexahedra(); - - const Vec3Types::VecCoord& x = this->mstate->read(core::vec_id::read_access::position)->getValue(); - { - int e = cube.elem; - DistanceGrid::Coord center; - core::topology::BaseMeshTopology::Hexa c = cubeCorners[e]; - { int t = c[2]; c[2] = c[3]; c[3] = t; } - { int t = c[6]; c[6] = c[7]; c[7] = t; } - - for (int j=0; j<8; j++) - { - cube.corners[j] = x[c[j]]; - center += cube.corners[j]; - } - cube.center = center * 0.125f; - SReal radius2 = 0.0f; - for (int j=0; j<8; j++) - { - SReal r2 = (cube.corners[j] - cube.center).norm2(); - if (r2 > radius2) radius2 = r2; - } - cube.radius = sofa::helper::rsqrt(radius2); - cube.updateDeform(); - cube.pointsUpdated = false; - cube.facesUpdated = false; - } - } -} - -/// Update the deformation precomputed values -void FFDDistanceGridCollisionModel::DeformedCube::updateDeform() -{ - Dx = corners[C100]-corners[C000]; // Dx = -C000+C100 - Dy = corners[C010]-corners[C000]; // Dy = -C000+C010 - Dz = corners[C001]-corners[C000]; // Dx = -C000+C001 - Dxy = corners[C110]-corners[C010]-Dx; // Dxy = C000-C100-C010+C110 = C110-C010-Dx - Dxz = corners[C101]-corners[C001]-Dx; // Dxz = C000-C100-C001+C101 = C101-C001-Dx - Dyz = corners[C011]-corners[C001]-Dy; // Dyz = C000-C010-C001+C011 = C011-C001-Dy - Dxyz = corners[C111]-corners[C101]-corners[C011]+corners[C001]-Dxy; // Dxyz = - C000 + C100 + C010 - C110 + C001 - C101 - C011 + C111 = C001 - C101 - C011 + C111 - Dxy -} - -/// Update the deformedPoints position if not done yet (i.e. if pointsUpdated==false) -void FFDDistanceGridCollisionModel::DeformedCube::updatePoints() -{ - if (!pointsUpdated) - { - deformedPoints.resize(points.size()); - deformedNormals.resize(points.size()); - for (unsigned int i=0; idisplayFlags().getShowWireFrame()) - { - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - } - glDisable(GL_LIGHTING); - glColor4fv(getColor4f()); - for (unsigned int i = 0; i < elems.size(); i++) - { - draw(vparams, i); - } - if (vparams->displayFlags().getShowWireFrame()) - { - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); - } -#endif // SOFADISTANCEGRID_HAVE_SOFA_GL == 1 -} - -void FFDDistanceGridCollisionModel::draw(const core::visual::VisualParams* vparams, sofa::Index index) -{ -#if SOFADISTANCEGRID_HAVE_SOFA_GL == 1 - DeformedCube& cube = getDeformCube( index ); - float cscale; - if (!isSimulated()) - cscale = 0.5f; - else - cscale = 1.0f; - if (cube.pointsUpdated && cube.facesUpdated) - glColor4f(1.0f*cscale, 0.0f*cscale, 0.0f*cscale, 1.0f); - else if (cube.pointsUpdated) - glColor4f(1.0f*cscale, 0.5f*cscale, 0.0f*cscale, 1.0f); - else if (cube.facesUpdated) - glColor4f(0.5f*cscale, 1.0f*cscale, 0.0f*cscale, 1.0f); - else - glColor4f(0.0f*cscale, 1.0f*cscale, 0.5f*cscale, 1.0f); - glBegin(GL_LINES); - { - sofa::gl::glVertexT(cube.corners[0]); sofa::gl::glVertexT(cube.corners[4]); - sofa::gl::glVertexT(cube.corners[1]); sofa::gl::glVertexT(cube.corners[5]); - sofa::gl::glVertexT(cube.corners[2]); sofa::gl::glVertexT(cube.corners[6]); - sofa::gl::glVertexT(cube.corners[3]); sofa::gl::glVertexT(cube.corners[7]); - sofa::gl::glVertexT(cube.corners[0]); sofa::gl::glVertexT(cube.corners[2]); - sofa::gl::glVertexT(cube.corners[1]); sofa::gl::glVertexT(cube.corners[3]); - sofa::gl::glVertexT(cube.corners[4]); sofa::gl::glVertexT(cube.corners[6]); - sofa::gl::glVertexT(cube.corners[5]); sofa::gl::glVertexT(cube.corners[7]); - sofa::gl::glVertexT(cube.corners[0]); sofa::gl::glVertexT(cube.corners[1]); - sofa::gl::glVertexT(cube.corners[2]); sofa::gl::glVertexT(cube.corners[3]); - sofa::gl::glVertexT(cube.corners[4]); sofa::gl::glVertexT(cube.corners[5]); - sofa::gl::glVertexT(cube.corners[6]); sofa::gl::glVertexT(cube.corners[7]); - } - glEnd(); - glLineWidth(2); - glPointSize(5); - { - glBegin(GL_POINTS); - { - sofa::gl::glVertexT(cube.center); - - } - glEnd(); - } - glLineWidth(1); - if (cube.pointsUpdated) - { - glPointSize(2); - glColor4f(1.0f, 0.5f, 0.5f, 1.0f); - glBegin(GL_POINTS); - for (unsigned int j=0; jdisplayFlags().getShowNormals()) - { - glBegin(GL_LINES); - for (unsigned int j=0; j > FFDDistanceGridContactMapperClass("PenalityContactForceField", true); - -template class SOFA_SOFADISTANCEGRID_API response::mapper::ContactMapper; - - -ContactMapperCreator< response::mapper::ContactMapper > DistanceGridContactMapperClass("PenalityContactForceField", true); - -template class SOFA_SOFADISTANCEGRID_API response::mapper::ContactMapper; - -} // namespace collision - -} // namespace component - -} // namespace sofa - diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/DistanceGridCollisionModel.h b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/DistanceGridCollisionModel.h deleted file mode 100644 index 675094f8922..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/DistanceGridCollisionModel.h +++ /dev/null @@ -1,623 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_COMPONENT_COLLISION_DISTANCEGRIDCOLLISIONMODEL_H -#define SOFA_COMPONENT_COLLISION_DISTANCEGRIDCOLLISIONMODEL_H -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "../../DistanceGrid.h" - - -namespace sofa -{ - -namespace component -{ - -namespace collision -{ - -typedef container::DistanceGrid DistanceGrid; - -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// - -class SOFA_SOFADISTANCEGRID_API RigidDistanceGridCollisionModel; - -class SOFA_SOFADISTANCEGRID_API RigidDistanceGridCollisionElement : public core::TCollisionElementIterator -{ -public: - - RigidDistanceGridCollisionElement(RigidDistanceGridCollisionModel* model, Index index); - - explicit RigidDistanceGridCollisionElement(const core::CollisionElementIterator& i); - - std::shared_ptr getGrid(); - - bool isTransformed(); - const type::Matrix3& getRotation(); - const type::Vec3& getTranslation(); - bool isFlipped(); - - /// @name Previous state data - /// Used to estimate velocity in case the distance grid itself is dynamic - /// @{ - std::shared_ptr getPrevGrid(); - const type::Matrix3& getPrevRotation(); - const type::Vec3& getPrevTranslation(); - double getPrevDt(); - /// @} - - /// Set new grid and transform, keeping the old state to estimate velocity - void setNewState(double dt, const std::shared_ptr grid, const type::Matrix3& rotation, const type::Vec3& translation); -}; - -class SOFA_SOFADISTANCEGRID_API RigidDistanceGridCollisionModel : public core::CollisionModel - , public core::behavior::SingleStateAccessor -{ -public: - SOFA_CLASS2(RigidDistanceGridCollisionModel, sofa::core::CollisionModel, SOFA_TEMPLATE(SingleStateAccessor, defaulttype::Rigid3Types)); - -protected: - - class ElementData - { - public: - type::Matrix3 rotation; - type::Vec3 translation; - std::shared_ptr grid; - - /// @name Previous state data - /// Used to estimate velocity in case the distance grid itself is dynamic - /// @{ - std::shared_ptr prevGrid; ///< Previous grid - type::Matrix3 prevRotation; ///< Previous rotation - type::Vec3 prevTranslation; ///< Previous translation - double prevDt; ///< Time difference between previous and current state - /// @} - - bool isTransformed; ///< True if translation/rotation was set - ElementData() : grid(nullptr), prevGrid(nullptr), prevDt(0.0), isTransformed(false) { rotation.identity(); prevRotation.identity(); } - }; - - sofa::type::vector elems; - bool modified; - void updateGrid(); - -public: - typedef defaulttype::Rigid3Types InDataTypes; - typedef defaulttype::Vec3Types DataTypes; - typedef RigidDistanceGridCollisionElement Element; - - // Input data parameters - sofa::core::objectmodel::DataFileName fileRigidDistanceGrid; - Data< double > scale; ///< scaling factor for input file - Data< type::Vec3 > translation; ///< translation to apply to input file - Data< type::Vec3 > rotation; ///< rotation to apply to input file - Data< double > sampling; ///< if not zero: sample the surface with points approximately separated by the given sampling distance (expressed in voxels if the value is negative) - Data< type::fixed_array > box; ///< Field bounding box defined by xmin,ymin,zmin, xmax,ymax,zmax - Data< int > nx; ///< number of values on X axis - Data< int > ny; ///< number of values on Y axis - Data< int > nz; ///< number of values on Z axis - sofa::core::objectmodel::DataFileName dumpfilename; - - Data< bool > usePoints; ///< use mesh vertices for collision detection - Data< bool > flipNormals; ///< reverse surface direction, i.e. points are considered in collision if they move outside of the object instead of inside - Data< bool > showMeshPoints; ///< Enable rendering of mesh points - Data< bool > showGridPoints; ///< Enable rendering of grid points - Data< double > showMinDist; ///< Min distance to render gradients - Data< double > showMaxDist; ///< Max distance to render gradients -protected: - RigidDistanceGridCollisionModel(); - - ~RigidDistanceGridCollisionModel() override; - - void drawCollisionModel(const core::visual::VisualParams* vparams) override; -public: - core::behavior::MechanicalState* getRigidModel() { return this->mstate ; } - core::behavior::MechanicalState* getMechanicalState() { return this->mstate ; } - - void init() override; - - std::shared_ptr getGrid(sofa::Index index=0) - { - return elems[index].grid; - } - bool isTransformed(sofa::Index index=0) const - { - return elems[index].isTransformed; - } - const type::Matrix3& getRotation(sofa::Index index=0) const - { - return elems[index].rotation; - } - const type::Vec3& getTranslation(sofa::Index index=0) const - { - return elems[index].translation; - } - - const type::Vec3& getInitTranslation() const - { - return translation.getValue(); - } - - const type::Matrix3 getInitRotation() const - { - SReal x = rotation.getValue()[0] * M_PI / 180; - SReal y = rotation.getValue()[1] * M_PI / 180; - SReal z = rotation.getValue()[2] * M_PI / 180; - - type::Matrix3 X(type::Vec3(1,0,0), type::Vec3(0, cos(x), -sin(x)), type::Vec3(0, sin(x), cos(x))); - type::Matrix3 Y(type::Vec3(cos(y), 0, sin(y)), type::Vec3(0, 1, 0), type::Vec3(-sin(y), 0, cos(y))); - type::Matrix3 Z(type::Vec3(cos(z), -sin(z), 0), type::Vec3(sin(z), cos(z), 0), type::Vec3(0, 0, 1)); - - return X * Y * Z; - } - - bool isFlipped() const - { - return flipNormals.getValue(); - } - - std::shared_ptr getPrevGrid(sofa::Index index=0) - { - return elems[index].prevGrid; - } - const type::Matrix3& getPrevRotation(sofa::Index index=0) const - { - return elems[index].prevRotation; - } - const type::Vec3& getPrevTranslation(sofa::Index index=0) const - { - return elems[index].prevTranslation; - } - double getPrevDt(sofa::Index index=0) const - { - return elems[index].prevDt; - } - - /// Set new grid and transform, keeping the old state to estimate velocity - void setNewState(sofa::Index index, double dt, const std::shared_ptr grid, const type::Matrix3& rotation, const type::Vec3& translation); - - /// @} - - - /// Update transformation matrices from current rigid state - void updateState(); - - void resize(sofa::Size size) override; - - /// Create or update the bounding volume hierarchy. - void computeBoundingTree(int maxDepth=0) override; - - void draw(const core::visual::VisualParams*, sofa::Index index) override; - -}; - -inline RigidDistanceGridCollisionElement::RigidDistanceGridCollisionElement(RigidDistanceGridCollisionModel* model, Index index) - : core::TCollisionElementIterator(model, index) -{} - -inline RigidDistanceGridCollisionElement::RigidDistanceGridCollisionElement(const core::CollisionElementIterator& i) - : core::TCollisionElementIterator(static_cast(i.getCollisionModel()), i.getIndex()) -{ -} - -inline std::shared_ptr RigidDistanceGridCollisionElement::getGrid() { return model->getGrid(index); } - -inline bool RigidDistanceGridCollisionElement::isTransformed() { return model->isTransformed(index); } -inline const type::Matrix3& RigidDistanceGridCollisionElement::getRotation() { return model->getRotation(index); } -inline const type::Vec3& RigidDistanceGridCollisionElement::getTranslation() { return model->getTranslation(index); } -inline bool RigidDistanceGridCollisionElement::isFlipped() { return model->isFlipped(); } - -inline std::shared_ptr RigidDistanceGridCollisionElement::getPrevGrid() { return model->getPrevGrid(index); } -inline const type::Matrix3& RigidDistanceGridCollisionElement::getPrevRotation() { return model->getPrevRotation(index); } -inline const type::Vec3& RigidDistanceGridCollisionElement::getPrevTranslation() { return model->getPrevTranslation(index); } -inline double RigidDistanceGridCollisionElement::getPrevDt() { return model->getPrevDt(index); } - -inline void RigidDistanceGridCollisionElement::setNewState(double dt, const std::shared_ptr grid, const type::Matrix3& rotation, const type::Vec3& translation) -{ - return model->setNewState(this->getIndex(), dt, grid, rotation, translation); -} - -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// - -class FFDDistanceGridCollisionModel; - -class FFDDistanceGridCollisionElement : public core::TCollisionElementIterator -{ -public: - - FFDDistanceGridCollisionElement(FFDDistanceGridCollisionModel* model, Index index); - - explicit FFDDistanceGridCollisionElement(const core::CollisionElementIterator& i); - - std::shared_ptr getGrid(); -}; - -class SOFA_SOFADISTANCEGRID_API FFDDistanceGridCollisionModel : public core::CollisionModel - , public core::behavior::SingleStateAccessor -{ -public: - SOFA_CLASS2(FFDDistanceGridCollisionModel, sofa::core::CollisionModel, SOFA_TEMPLATE(SingleStateAccessor, defaulttype::Vec3Types)); - - typedef SReal GSReal; - typedef DistanceGrid::Coord GCoord; - class SOFA_SOFADISTANCEGRID_API DeformedCube - { - public: - std::shared_ptr grid; - DeformedCube() : grid(nullptr) {} - int elem; ///< Index of the corresponding element in the topology - std::set neighbors; ///< Index of the neighbors (used for self-collisions) - struct Point - { - GCoord bary; ///< Barycentric coordinates - sofa::Index index; ///< Index of corresponding point in DistanceGrid - }; - type::vector points; ///< barycentric coordinates of included points - type::vector normals; ///< normals in barycentric coordinates of included points - GCoord initP0,initDP,invDP; ///< Initial corners position - GCoord corners[8]; ///< Current corners position - enum {C000 = 0+0+0, - C100 = 1+0+0, - C010 = 0+2+0, - C110 = 1+2+0, - C001 = 0+0+4, - C101 = 1+0+4, - C011 = 0+2+4, - C111 = 1+2+4 - }; - typedef type::Vec<4,GSReal> Plane; ///< plane equation as defined by Plane.(x y z 1) = 0 - Plane faces[6]; ///< planes corresponding to the six faces (FX0,FX1,FY0,FY1,FZ0,FZ1) - enum {FX0 = 0+0, - FX1 = 0+1, - FY0 = 2+0, - FY1 = 2+1, - FZ0 = 4+0, - FZ1 = 4+1 - }; - /// @name Precomputed deformation factors - /// We have : - /// deform(b) = C000(1-b[0])(1-b[1])(1-b[2]) + C100(b[0])(1-b[1])(1-b[2]) + C010(1-b[0])(b[1])(1-b[2]) + C110(b[0])(b[1])(1-b[2]) - /// + C001(1-b[0])(1-b[1])( b[2]) + C101(b[0])(1-b[1])( b[2]) + C011(1-b[0])(b[1])( b[2]) + C111(b[0])(b[1])( b[2]) - /// = C000 + Dx b[0] + Dy b[1] + Dz b[2] + Dxy b[0]b[1] + Dxz b[0]b[2] + dyz b[1]b[2] + dxyz b[0]b[1]b[2] - /// @{ - GCoord Dx; ///< Dx = -C000+C100 - GCoord Dy; ///< Dy = -C000+C010 - GCoord Dz; ///< Dx = -C000+C001 - GCoord Dxy; ///< Dxy = C000-C100-C010+C110 = C110-C010-Dx - GCoord Dxz; ///< Dxz = C000-C100-C001+C101 = C101-C001-Dx - GCoord Dyz; ///< Dyz = C000-C010-C001+C011 = C011-C001-Dy - GCoord Dxyz; ///< Dxyz = - C000 + C100 + C010 - C110 + C001 - C101 - C011 + C111 = C001 - C101 - C011 + C111 - Dxy - /// @} - /// Update the deformation precomputed values - void updateDeform(); - - GCoord center; ///< current center; - GSReal radius; ///< radius of enclosing sphere - type::vector deformedPoints; ///< deformed points - type::vector deformedNormals; ///< deformed normals - bool pointsUpdated; ///< true the deformedPoints vector has been updated with the latest positions - void updatePoints(); ///< Update the deformedPoints position if not done yet (i.e. if pointsUpdated==false) - bool facesUpdated; ///< true the faces plane vector has been updated with the latest positions - void updateFaces(); ///< Update the face planes if not done yet (i.e. if facesUpdated==false) - /// Compute the barycentric coordinates of a point from its initial position - DistanceGrid::Coord baryCoords(const GCoord& c) const - { - return GCoord( (c[0]-initP0[0])*invDP[0], - (c[1]-initP0[1])*invDP[1], - (c[2]-initP0[2])*invDP[2]); - } - /// Compute the initial position of a point from its barycentric coordinates - GCoord initpos(const GCoord& b) const - { - return GCoord( initP0[0]+initDP[0]*b[0], - initP0[1]+initDP[1]*b[1], - initP0[2]+initDP[2]*b[2]); - } - /// Compute the deformed position of a point from its barycentric coordinates - GCoord deform(const GCoord& b) const - { - return corners[C000] + Dx*b[0] + (Dy + Dxy*b[0])*b[1] + (Dz + Dxz*b[0] + (Dyz + Dxyz*b[0])*b[1])*b[2]; - } - - static GSReal interp(GSReal coef, GSReal a, GSReal b) - { - return a+coef*(b-a); - } - - /// deform a direction relative to a point in barycentric coordinates - GCoord deformDir(const GCoord& b, const GCoord& dir) const - { - GCoord r; - // dp/dx = Dx + Dxy*y + Dxz*z + Dxyz*y*z - r = (Dx + Dxy*b[1] + (Dxz + Dxyz*b[1])*b[2])*dir[0]; - // dp/dy = Dy + Dxy*x + Dyz*z + Dxyz*x*z - r += (Dy + Dxy*b[0] + (Dyz + Dxyz*b[0])*b[2])*dir[1]; - // dp/dz = Dz + Dxz*x + Dyz*y + Dxyz*x*y - r += (Dz + Dxz*b[0] + (Dyz + Dxyz*b[0])*b[1])*dir[2]; - return r; - } - - /// Get the local jacobian matrix of the deformation - type::Mat<3,3,double> Jdeform(const GCoord& b) const - { - type::Mat<3,3,double> J; - for (int i=0; i<3; i++) - { - // dp/dx = Dx + Dxy*y + Dxz*z + Dxyz*y*z - J[i][0] = (Dx[i] + Dxy[i]*b[1] + (Dxz[i] + Dxyz[i]*b[1])*b[2]); - // dp/dy = Dy + Dxy*x + Dyz*z + Dxyz*x*z - J[i][1] = (Dy[i] + Dxy[i]*b[0] + (Dyz[i] + Dxyz[i]*b[0])*b[2]); - // dp/dz = Dz + Dxz*x + Dyz*y + Dxyz*x*y - J[i][2] = (Dz[i] + Dxz[i]*b[0] + (Dyz[i] + Dxyz[i]*b[0])*b[1]); - } - return J; - } - - /// Compute an initial estimate to the barycentric coordinate of a point given its deformed position - GCoord undeform0(const GCoord& p) const - { - GCoord b; - for (int i=0; i<3; i++) - { - GSReal b0 = faces[2*i+0]*Plane(p,1); - GSReal b1 = faces[2*i+1]*Plane(p,1); - b[i] = b0 / (b0 + b1); - } - return b; - } - /// Undeform a direction relative to a point in barycentric coordinates - GCoord undeformDir(const GCoord& b, const GCoord& dir) const - { - // we want to find b2 so that deform(b2)-deform(b) = dir - // we can use Newton's method using the jacobian of the deformation. - type::Mat<3,3,double> m = Jdeform(b); - type::Mat<3,3,double> minv; - if(!minv.invert(m)) - msg_error("FFDDistanceGridCollisionModel")<<"Non-invertible matrix in undeformDir"; - return minv*dir; - } - - /// Compute a plane equation given 4 corners - Plane computePlane(int c00, int c10, int c01, int c11); - }; - -protected: - - sofa::type::vector elems; - - // Input data parameters - sofa::core::objectmodel::DataFileName fileFFDDistanceGrid; - Data< double > scale; ///< scaling factor for input file - Data< double > sampling; ///< if not zero: sample the surface with points approximately separated by the given sampling distance (expressed in voxels if the value is negative) - Data< type::fixed_array > box; ///< Field bounding box defined by xmin,ymin,zmin, xmax,ymax,zmax - Data< int > nx; ///< number of values on X axis - Data< int > ny; ///< number of values on Y axis - Data< int > nz; ///< number of values on Z axis - sofa::core::objectmodel::DataFileName dumpfilename; - - core::objectmodel::SingleLink l_ffdMesh; - - void updateGrid(); -public: - typedef defaulttype::Vec3Types InDataTypes; - typedef defaulttype::Vec3Types DataTypes; - typedef topology::container::grid::RegularGridTopology Topology; - typedef FFDDistanceGridCollisionElement Element; - - Data< bool > usePoints; ///< use mesh vertices for collision detection - Data< bool > singleContact; ///< keep only the deepest contact in each cell -protected: - FFDDistanceGridCollisionModel(); - - ~FFDDistanceGridCollisionModel() override; - - void drawCollisionModel(const core::visual::VisualParams* vparams) override; -public: - core::behavior::MechanicalState* getDeformModel() { return this->mstate; } - core::topology::BaseMeshTopology* getDeformGrid() { return l_ffdMesh; } - - /// alias used by ContactMapper - core::behavior::MechanicalState* getMechanicalState() { return this->mstate; } - core::topology::BaseMeshTopology* getCollisionTopology() override { return l_ffdMesh; } - - void init() override; - - std::shared_ptr getGrid(sofa::Index index=0) - { - return elems[index].grid; - } - - DeformedCube& getDeformCube(sofa::Index index=0) - { - return elems[index]; - } - - /// CollisionModel interface - void resize(sofa::Size size) override; - - /// Create or update the bounding volume hierarchy. - void computeBoundingTree(int maxDepth=0) override; - - bool canCollideWithElement(sofa::Index index, CollisionModel* model2, sofa::Index index2) override; - - void draw(const core::visual::VisualParams*, sofa::Index index) override; -}; - -inline FFDDistanceGridCollisionElement::FFDDistanceGridCollisionElement(FFDDistanceGridCollisionModel* model, Index index) - : core::TCollisionElementIterator(model, index) -{} - -inline FFDDistanceGridCollisionElement::FFDDistanceGridCollisionElement(const core::CollisionElementIterator& i) - : core::TCollisionElementIterator(static_cast(i.getCollisionModel()), i.getIndex()) -{ -} - -inline std::shared_ptr FFDDistanceGridCollisionElement::getGrid() { return model->getGrid(index); } - -/// Mapper for FFDDistanceGridCollisionModel -template -class response::mapper::ContactMapper : public BarycentricContactMapper -{ -public: - typedef typename DataTypes::Real Real; - typedef typename DataTypes::Coord Coord; - using Index = sofa::Index; - - Index addPoint(const Coord& P, Index index, Real&) - { - type::Vec3 bary; - Index elem = this->model->getDeformCube(index).elem; - bary = this->model->getDeformCube(index).baryCoords(P); - return this->mapper->addPointInCube(elem,bary.ptr()); - } -}; - - -/// Mapper for RigidDistanceGridCollisionModel -template -class response::mapper::ContactMapper : public RigidContactMapper -{ -public: - typedef typename DataTypes::Real Real; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef RigidContactMapper Inherit; - typedef typename Inherit::MMechanicalState MMechanicalState; - typedef typename Inherit::MCollisionModel MCollisionModel; - using Index = sofa::Index; - - MMechanicalState* createMapping(const char* name="contactPoints") - { - using sofa::component::mapping::linear::IdentityMapping; - - MMechanicalState* outmodel = Inherit::createMapping(name); - if (this->child!=nullptr && this->mapping==nullptr) - { - //TODO(dmarchal):2017-05-26 This comment may become a conditional code. - // add velocity visualization - /* sofa::component::visual::DrawV* visu = new sofa::component::visual::DrawV; - this->child->addObject(visu); - visu->useAlpha.setValue(true); - visu->vscale.setValue(this->model->getContext()->getDt()); - IdentityMapping< DataTypes, StdVectorTypes< Vec<3,GLfloat>, Vec<3,GLfloat> > > * map = new IdentityMapping< DataTypes, StdVectorTypes< Vec<3,GLfloat>, Vec<3,GLfloat> > >( outmodel, visu ); - this->child->addObject(map); - visu->init(); - map->init(); */ - } - return outmodel; - } - - Index addPoint(const Coord& P, Index index, Real& r) - { - Coord trans = this->model->getInitRotation() * this->model->getInitTranslation(); - int i = Inherit::addPoint(P+trans, index, r); - if (!this->mapping) - { - MCollisionModel* model = this->model; - MMechanicalState* outmodel = this->outmodel.get(); - { - helper::WriteAccessor > xData = *outmodel->write(core::vec_id::write_access::position); - Coord& x = xData.wref()[i]; - - if (model->isTransformed(index)) - x = model->getTranslation(index) + model->getRotation(index) * P; - else - x = P; - } - helper::ReadAccessor > xData = *outmodel->read(core::vec_id::read_access::position); - helper::WriteAccessor > vData = *outmodel->write(core::vec_id::write_access::velocity); - const Coord& x = xData.ref()[i]; - Deriv& v = vData.wref()[i]; - v.clear(); - - // estimating velocity - double gdt = model->getPrevDt(index); - if (gdt > 0.000001) - { - if (model->isTransformed(index)) - { - v = (x - (model->getPrevTranslation(index) + model-> getPrevRotation(index) * P)) * (1.0/gdt); - } - std::shared_ptr prevGrid = model->getPrevGrid(index); - //DistanceGrid* grid = model->getGrid(index); - //if (prevGrid != nullptr && prevGrid != grid && prevGrid->inGrid(P)) - { - DistanceGrid::Coord coefs; - int ii = prevGrid->index(P, coefs); - SReal d = prevGrid->interp(ii, coefs); - if (sofa::helper::rabs(d) < 0.3) // todo : control threshold - { - DistanceGrid::Coord n = prevGrid->grad(ii, coefs); - v += n * (d / ( n.norm() * gdt)); - } - } - } - } - return i; - } -}; - - -#if !defined(SOFA_COMPONENT_COLLISION_DISTANCEGRIDCOLLISIONMODEL_CPP) - -extern template class SOFA_SOFADISTANCEGRID_API response::mapper::ContactMapper; -extern template class SOFA_SOFADISTANCEGRID_API response::mapper::ContactMapper; - -# ifdef _MSC_VER -// Manual declaration of non-specialized members, to avoid warnings from MSVC. -extern template SOFA_SOFADISTANCEGRID_API void response::mapper::BarycentricContactMapper::cleanup(); -extern template SOFA_SOFADISTANCEGRID_API core::behavior::MechanicalState* response::mapper::BarycentricContactMapper::createMapping(const char*); -extern template SOFA_SOFADISTANCEGRID_API void response::mapper::RigidContactMapper::cleanup(); -extern template SOFA_SOFADISTANCEGRID_API core::behavior::MechanicalState* response::mapper::RigidContactMapper::createMapping(const char*); -# endif -#endif - - - -} // namespace collision - -} // namespace component - -} // namespace sofa - -#endif diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/FFDDistanceGridDiscreteIntersection.cpp b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/FFDDistanceGridDiscreteIntersection.cpp deleted file mode 100644 index 115c440e3fc..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/FFDDistanceGridDiscreteIntersection.cpp +++ /dev/null @@ -1,719 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#include -#include -#include -#include -#include -#include -#include "FFDDistanceGridDiscreteIntersection.inl" - -namespace sofa -{ - -namespace component -{ - -namespace collision -{ - -using namespace sofa::type; -using namespace sofa::defaulttype; -using namespace sofa::core::collision; -using namespace sofa::component::collision::geometry; -using namespace sofa::component::collision::detection::intersection; - -IntersectorCreator FFDDistanceGridDiscreteIntersectors("FFDDistanceGrid"); - -FFDDistanceGridDiscreteIntersection::FFDDistanceGridDiscreteIntersection(DiscreteIntersection* intersection) -{ - intersection->intersectors.add, FFDDistanceGridDiscreteIntersection> (this); - intersection->intersectors.add, FFDDistanceGridDiscreteIntersection> (this); - intersection->intersectors.add, FFDDistanceGridDiscreteIntersection> (this); - intersection->intersectors.add (this); - intersection->intersectors.add (this); - intersection->intersectors.add (this); -} - -bool FFDDistanceGridDiscreteIntersection::testIntersection(FFDDistanceGridCollisionElement&, RigidDistanceGridCollisionElement&, const core::collision::Intersection*) -{ - return true; -} - -int FFDDistanceGridDiscreteIntersection::computeIntersection(FFDDistanceGridCollisionElement& e1, RigidDistanceGridCollisionElement& e2, OutputVector* contacts, const core::collision::Intersection* intersection) -{ - int nc = 0; - const std::shared_ptr grid1 = e1.getGrid(); - const std::shared_ptr grid2 = e2.getGrid(); - FFDDistanceGridCollisionModel::DeformedCube& c1 = e1.getCollisionModel()->getDeformCube(e1.getIndex()); - bool useXForm = e2.isTransformed(); - //const type::Vec3& t1 = e1.getTranslation(); - //const type::Mat3x3& r1 = e1.getRotation(); - const type::Vec3& t2 = e2.getTranslation(); - const type::Mat3x3& r2 = e2.getRotation(); - - const double d0 = e1.getContactDistance() + e2.getContactDistance() + (intersection->getContactDistance() == 0.0 ? 0.001 : intersection->getContactDistance()); - //const SReal margin = 0.001f + (SReal)d0; - const SReal margin = (SReal)((e1.getContactDistance() + e2.getContactDistance() + (intersection->getAlarmDistance() == 0.0 ? 0.001 : intersection->getAlarmDistance()))/2); - const bool singleContact = e1.getCollisionModel()->singleContact.getValue(); - - // transform from grid1 to grid2 - Vec3 translation; - Mat3x3 rotation; - - if (useXForm) - { - translation = r2.multTranspose(-t2); - rotation = r2; rotation.transpose(); - } - else rotation.identity(); - - const DistanceGrid::Coord center2 = translation + rotation*c1.center; - const SReal radius2 = c1.radius*c1.radius; - const DistanceGrid::VecCoord& x2 = grid2->meshPts; - const int i0 = x2.size(); - // first points of e1 against distance field of e2 - if (e1.getCollisionModel()->usePoints.getValue()) // && !e2.getCollisionModel()->usePoints.getValue()) - { - if (grid2->inBBox( center2, margin + c1.radius )) - { - c1.updatePoints(); - const sofa::type::vector& x1 = c1.deformedPoints; - const sofa::type::vector& n1 = c1.deformedNormals; - bool first = true; - for (unsigned int i=0; iinBBox( p2, margin )) continue; - if (!grid2->inGrid( p2 )) - { - msg_error(intersection) << "Margin less than "<getName(); - continue; - } - - SReal d = grid2->interp(p2); - if (d >= margin) continue; - - type::Vec3 grad = grid2->grad(p2); // note that there are some redundant computations between interp() and grad() - grad.normalize(); - - //p2 -= grad * d; // push p2 back to the surface - if (!singleContact || first) - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - double value = d + margin - d0; - if (!singleContact || first || (value < detection->value)) - { - detection->point[0] = grid1->meshPts[c1.points[i].index]; - detection->point[1] = type::Vec3(p2) - grad * d; - detection->normal = r2 * -grad; // normal in global space from p1's surface - detection->value = value; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = i0 + c1.points[i].index; - ++nc; - first = false; - } - } - } - } - - // then points of e2 against distance field of e1 - - if (!x2.empty() && e2.getCollisionModel()->usePoints.getValue()) - { - const SReal cubesize = c1.invDP.norm(); - bool first = true; - for (unsigned int i=0; i= radius2) continue; - DistanceGrid::Coord p1 = rotation.multTranspose(p2-translation); - // try to find the point in the undeformed cube - { - c1.updateFaces(); - // estimate the barycentric coordinates - DistanceGrid::Coord b = c1.undeform0(p1); - - // refine the estimate until we are very close to the p1 or we are sure p1 cannot intersect with the object - int iter; - SReal err1 = 1000.0f; - for(iter=0; iter<5; ++iter) - { - DistanceGrid::Coord pdeform = c1.deform(b); - DistanceGrid::Coord diff = p1-pdeform; - SReal err = diff.norm(); - SReal berr = err*cubesize; if (berr>0.5f) berr=0.5f; - if (b[0] < -berr || b[0] > 1+berr - || b[1] < -berr || b[1] > 1+berr - || b[2] < -berr || b[2] > 1+berr) - break; // far from the cube - if (iter>3) - msg_info(intersection) << "Iter"< "< 0.001f && b[0] < 0.999f - && b[1] > 0.001f && b[1] < 0.999f - && b[2] > 0.001f && b[2] < 0.999f) - { - DistanceGrid::Coord pinit = c1.initpos(b); - SReal d = grid1->interp(pinit); - if (d < 2*margin) - { - DistanceGrid::Coord grad = grid1->grad(pinit); // note that there are some redundant computations between interp() and grad() - grad.normalize(); - pinit -= grad*d; - grad = c1.deformDir(c1.baryCoords(pinit),grad); - grad.normalize(); - - if (!singleContact || first) - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - double value = d - d0; - if (!singleContact || first || (value < detection->value)) - { - detection->point[0] = type::Vec3(pinit); - detection->point[1] = type::Vec3(p2); - detection->normal = type::Vec3(grad); // normal in global space from p1's surface - detection->value = value; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = i; - ++nc; - first = false; - } - } - } - break; - } - err1 = err; - SReal d = grid1->interp(c1.initpos(b)); - if (d*0.5f - err > 2*margin) - break; // the point is too far from the object - // we are solving for deform(b+db)-deform(b) = p1-deform(b) - // deform(b+db) ~= deform(b) + J db -> J db = p1-deform(b) -> db = J^-1 (p1-deform(b)) - b += c1.undeformDir( b, diff ); - } - if (iter == 5) - { - if (b[0] > 0.001f && b[0] < 0.999f - && b[1] > 0.001f && b[1] < 0.999f - && b[2] > 0.001f && b[2] < 0.999f) - msg_error(intersection) << "FFD-Rigid collision failed to converge to undeformed point: p1 = "< "< "< "< "< "<0.5f) berr=0.5f; - if (b[0] < -berr || b[0] > 1+berr - || b[1] < -berr || b[1] > 1+berr - || b[2] < -berr || b[2] > 1+berr) - break; // far from the cube - if (err < 0.001f) - { - // we found the corresponding point, but is is only valid if inside the current cube - if (b[0] > -0.1f && b[0] < 1.1f - && b[1] > -0.1f && b[1] < 1.1f - && b[2] > -0.1f && b[2] < 1.1f) - { - found = true; - } - break; - } - //err1 = err; - b += c1.undeformDir( b, diff ); - } - if (found) - { - SReal d = grid1->interp(c1.initpos(b)); - if (d < 0) - { - // intersection found - - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - - detection->point[0] = e2.origin() + e2.direction()*rayPos; - detection->point[1] = c1.initpos(b); - detection->normal = e2.direction(); // normal in global space from p1's surface - detection->value = d; - detection->elem.first = e2; - detection->elem.second = e1; - detection->id = e2.getIndex(); - return 1; - } - } - // else move along the ray - //if (dot(type::Vec3(grid1->grad(c1.initpos(b))),rayDirection) < 0) - // rayPos += 0.5*d; - //else - // rayPos -= 0.5*d; - } - return 0; -} - -} // namespace collision - -} // namespace component - -} // namespace sofa - diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/FFDDistanceGridDiscreteIntersection.h b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/FFDDistanceGridDiscreteIntersection.h deleted file mode 100644 index a35679a55bf..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/FFDDistanceGridDiscreteIntersection.h +++ /dev/null @@ -1,75 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_COMPONENT_COLLISION_FFDDISTANCEGRIDDISCRETEINTERSECTION_H -#define SOFA_COMPONENT_COLLISION_FFDDISTANCEGRIDDISCRETEINTERSECTION_H -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace sofa -{ - -namespace component -{ - -namespace collision -{ -class SOFA_SOFADISTANCEGRID_API FFDDistanceGridDiscreteIntersection : public core::collision::BaseIntersector -{ - - typedef detection::intersection::DiscreteIntersection::OutputVector OutputVector; - -public: - FFDDistanceGridDiscreteIntersection(detection::intersection::DiscreteIntersection* object); - - bool testIntersection(FFDDistanceGridCollisionElement&, RigidDistanceGridCollisionElement&, const core::collision::Intersection*); - bool testIntersection(FFDDistanceGridCollisionElement&, FFDDistanceGridCollisionElement&, const core::collision::Intersection*); - bool testIntersection(FFDDistanceGridCollisionElement&, geometry::Point&, const core::collision::Intersection*); - template bool testIntersection(FFDDistanceGridCollisionElement&, geometry::TSphere&, const core::collision::Intersection*); - bool testIntersection(FFDDistanceGridCollisionElement&, geometry::Triangle&, const core::collision::Intersection*); - bool testIntersection(geometry::Ray&, FFDDistanceGridCollisionElement&, const core::collision::Intersection*); - - int computeIntersection(FFDDistanceGridCollisionElement&, RigidDistanceGridCollisionElement&, OutputVector*, const core::collision::Intersection*); - int computeIntersection(FFDDistanceGridCollisionElement&, FFDDistanceGridCollisionElement&, OutputVector*, const core::collision::Intersection*); - int computeIntersection(FFDDistanceGridCollisionElement&, geometry::Point&, OutputVector*, const core::collision::Intersection*); - template int computeIntersection(FFDDistanceGridCollisionElement&, geometry::TSphere&, OutputVector*, const core::collision::Intersection*); - int computeIntersection(FFDDistanceGridCollisionElement&, geometry::Triangle&, OutputVector*, const core::collision::Intersection*); - int computeIntersection(geometry::Ray&, FFDDistanceGridCollisionElement&, OutputVector*, const core::collision::Intersection*); - -}; - -} // namespace collision - -} // namespace component - -} // namespace sofa - -#endif diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/FFDDistanceGridDiscreteIntersection.inl b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/FFDDistanceGridDiscreteIntersection.inl deleted file mode 100644 index 6cdc6478708..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/FFDDistanceGridDiscreteIntersection.inl +++ /dev/null @@ -1,140 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_COMPONENT_COLLISION_FFDDISTANCEGRIDDISCRETEINTERSECTION_INL -#define SOFA_COMPONENT_COLLISION_FFDDISTANCEGRIDDISCRETEINTERSECTION_INL -#include - -#include -#include - -#include -#include - -#include "FFDDistanceGridDiscreteIntersection.h" - -namespace sofa -{ - -namespace component -{ - -namespace collision -{ - - -template -bool FFDDistanceGridDiscreteIntersection::testIntersection(FFDDistanceGridCollisionElement&, geometry::TSphere&, const core::collision::Intersection*) -{ - return true; -} - -template -int FFDDistanceGridDiscreteIntersection::computeIntersection(FFDDistanceGridCollisionElement& e1, geometry::TSphere& e2, OutputVector* contacts, const core::collision::Intersection* intersection) -{ - - const std::shared_ptr grid1 = e1.getGrid(); - FFDDistanceGridCollisionModel::DeformedCube& c1 = e1.getCollisionModel()->getDeformCube(e1.getIndex()); - - const double d0 = e1.getContactDistance() + e2.getContactDistance() + intersection->getContactDistance() + e2.r(); - const SReal margin = 0.001f + (SReal)d0; - - c1.updateFaces(); - const SReal cubesize = c1.invDP.norm(); - type::Vec3 p2 = e2.center(); - DistanceGrid::Coord p1 = p2; - - // estimate the barycentric coordinates - DistanceGrid::Coord b = c1.undeform0(p1); - - // refine the estimate until we are very close to the p2 or we are sure p2 cannot intersect with the object - int iter; - SReal err1 = 1000.0f; - for(iter=0; iter<5; ++iter) - { - DistanceGrid::Coord pdeform = c1.deform(b); - DistanceGrid::Coord diff = p1-pdeform; - SReal err = diff.norm(); - if (iter>3) - msg_info(intersection) << "Iter"< "<0.5f) berr=0.5f; - if (b[0] < -berr || b[0] > 1+berr - || b[1] < -berr || b[1] > 1+berr - || b[2] < -berr || b[2] > 1+berr) - break; // far from the cube - if (err < 0.005f) - { - // we found the corresponding point, but is is only valid if inside the current cube - if (b[0] > 0.001f && b[0] < 0.999f - && b[1] > 0.001f && b[1] < 0.999f - && b[2] > 0.001f && b[2] < 0.999f) - { - DistanceGrid::Coord pinit = c1.initpos(b); - SReal d = grid1->interp(pinit); - if (d < margin) - { - DistanceGrid::Coord grad = grid1->grad(pinit); // note that there are some redundant computations between interp() and grad() - grad.normalize(); - pinit -= grad*d; - grad = c1.deformDir(c1.baryCoords(pinit),grad); - grad.normalize(); - - contacts->resize(contacts->size()+1); - sofa::core::collision::DetectionOutput *detection = &*(contacts->end()-1); - detection->normal = type::Vec3(grad); // normal in global space from p1's surface - detection->value = d - d0; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = e2.getIndex(); - detection->point[0] = type::Vec3(pinit); - detection->point[1] = e2.getContactPointWithSurfacePoint( pinit ); - return 1; - } - } - break; - } - err1 = err; - SReal d = grid1->interp(c1.initpos(b)); - if (d*0.5f - err > margin) - break; // the point is too far from the object - // we are solving for deform(b+db)-deform(b) = p1-deform(b) - // deform(b+db) ~= deform(b) + M db -> M db = p1-deform(b) -> db = M^-1 (p1-deform(b)) - b += c1.undeformDir( b, diff ); - } - if (iter == 5) - { - if (b[0] > 0.001f && b[0] < 0.999f - && b[1] > 0.001f && b[1] < 0.999f - && b[2] > 0.001f && b[2] < 0.999f) - msg_error(intersection) << "FFD-FFD collision failed to converge to undeformed point: p1 = "< > RayDistanceGridContactClass("RayContact",true); -Creator > RayFFDDistanceGridContactClass("RayContact",true); - -} // namespace collision - -} // namespace component - -} // namespace sofa diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/RigidDistanceGridDiscreteIntersection.cpp b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/RigidDistanceGridDiscreteIntersection.cpp deleted file mode 100644 index f1d8fe325f6..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/RigidDistanceGridDiscreteIntersection.cpp +++ /dev/null @@ -1,914 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#include -#include - -#include -#include -#include -#include -#include "RigidDistanceGridDiscreteIntersection.inl" - -namespace sofa -{ - -namespace component -{ - -namespace collision -{ - -using namespace sofa::type; -using namespace sofa::defaulttype; -using namespace sofa::core::collision; -using namespace sofa::component::collision::detection::intersection; -using namespace sofa::component::collision::geometry; - -IntersectorCreator RigidDistanceGridDiscreteIntersectors("RigidDistanceGrid"); - -RigidDistanceGridDiscreteIntersection::RigidDistanceGridDiscreteIntersection(DiscreteIntersection* intersection) -{ - intersection->intersectors.add, RigidDistanceGridDiscreteIntersection> (this); - intersection->intersectors.add, RigidDistanceGridDiscreteIntersection> (this); - intersection->intersectors.add, RigidDistanceGridDiscreteIntersection> (this); - intersection->intersectors.add, RigidDistanceGridDiscreteIntersection> (this); - intersection->intersectors.add (this); - intersection->intersectors.add (this); -} - -bool RigidDistanceGridDiscreteIntersection::testIntersection(RigidDistanceGridCollisionElement&, RigidDistanceGridCollisionElement&, const core::collision::Intersection*) -{ - return true; -} - -//#define DEBUG_XFORM - -int RigidDistanceGridDiscreteIntersection::computeIntersection(RigidDistanceGridCollisionElement& e1, RigidDistanceGridCollisionElement& e2, OutputVector* contacts, const core::collision::Intersection* intersection) -{ - int nc = 0; - const std::shared_ptr grid1 = e1.getGrid(); - const std::shared_ptr grid2 = e2.getGrid(); - bool useXForm = e1.isTransformed() || e2.isTransformed(); - const type::Vec3& t1 = e1.getTranslation(); - const Matrix3& r1 = e1.getRotation(); - const type::Vec3& t2 = e2.getTranslation(); - const Matrix3& r2 = e2.getRotation(); - - const double d0 = e1.getContactDistance() + e2.getContactDistance() + (intersection->getContactDistance() == 0.0 ? 0.001 : intersection->getContactDistance()); - //const SReal margin = 0.001f + (SReal)d0; - const SReal margin = (SReal)((e1.getContactDistance() + e2.getContactDistance() + (intersection->getAlarmDistance() == 0.0 ? 0.001 : intersection->getAlarmDistance()))/2); - - // transform from grid1 to grid2 - Vec3f translation; - Mat3x3f rotation; - - if (useXForm) - { - // p = t1+r1*p1 = t2+r2*p2 - // r2*p2 = t1-t2+r1*p1 - // p2 = r2t*(t1-p2) + r2t*r1*p1 - translation = r2.multTranspose(t1-t2); - rotation = r2.multTranspose ( r1 ); - } - else rotation.identity(); - - // For the cube-cube case, we need to detect cases where cubes are stacked - // One way is to find if a pair of faces exists that are nearly parallel and - // that are near each other - // if such pair is found, the corresponding face will be stored in these variables - - enum { FACE_NONE=-1,FACE_XNEG=0,FACE_XPOS,FACE_YNEG,FACE_YPOS,FACE_ZNEG,FACE_ZPOS }; - int face_e1 = FACE_NONE; - int face_e2 = FACE_NONE; - - if (grid2->isCube() && grid1->isCube()) - { - const SReal cubeDim1 = grid1->getCubeDim(); - const SReal cubeDim2 = grid2->getCubeDim(); - // current distance found - // we allow only 10% penetration - SReal dist = (SReal)((cubeDim1 + cubeDim2) * 0.1); - // a nearly perpendicular pair would be visible by an entry close to 1 in the rotation matrix - for (int f2 = 0; f2 < 3; f2++) - { - for (int f1 = 0; f1 < 3; f1++) - { - if (rotation[f2][f1] < -0.99 || rotation[f2][f1] > 0.99) - { - // found a match - // translation is the position of cube1 center in cube2 space - // so the pair of faces are close if |translation[f2]| is close dim1+dim2 - SReal d = sofa::helper::rabs(sofa::helper::rabs(translation[f2])-(cubeDim1+cubeDim2)); - // we should favor normals that are perpendicular to the relative velocity - // however we don't have this information currently, so for now we favor the horizontal face - if (sofa::helper::rabs(r2[f2][2]) > 0.99 && d < (cubeDim1 + cubeDim2) * 0.1) d = 0; - if (d < dist) - { - dist = d; - if (translation[f2] > 0) - { - // positive side on cube 2 - face_e2 = 2*f2+1; - if (rotation[f2][f1] > 0) - { - // cubes have same axis orientation -> negative side on cube 1 - face_e1 = 2*f1; - } - else - { - // cubes have same opposite orientation -> positive side on cube 1 - face_e1 = 2*f1+1; - } - } - else - { - // negative side on cube 2 - face_e2 = 2*f2; - if (rotation[f2][f1] > 0) - { - // cubes have same axis orientation -> positive side on cube 1 - face_e1 = 2*f1+1; - } - else - { - // cubes have same opposite orientation -> negative side on cube 1 - face_e1 = 2*f1; - } - } - } - } - } - } - } - - // first points of e1 against distance field of e2 - const DistanceGrid::VecCoord& x1 = grid1->meshPts; - if (!x1.empty() && e1.getCollisionModel()->usePoints.getValue()) - { - if (grid2->isCube() && grid1->isCube()) - { - const SReal cubeDim2 = grid2->getCubeDim(); - const SReal cubeDim2Margin = cubeDim2+margin; - - if (face_e2 != FACE_NONE) - { - // stacked cubes - DistanceGrid::Coord normal; - normal[face_e2/2] = (face_e2&1)?1.0f:-1.0f; - type::Vec3 gnormal = r2 * -normal; - // special case: if normal in global frame is nearly vertical or horizontal, make it so - if (gnormal[0] < -0.99f) gnormal = type::Vec3(-1.0f, 0.0f, 0.0f); - else if (gnormal[0] > 0.99f) gnormal = type::Vec3( 1.0f, 0.0f, 0.0f); - if (gnormal[1] < -0.99f) gnormal = type::Vec3( 0.0f,-1.0f, 0.0f); - else if (gnormal[1] > 0.99f) gnormal = type::Vec3( 0.0f, 1.0f, 0.0f); - if (gnormal[2] < -0.99f) gnormal = type::Vec3( 0.0f, 0.0f, -1.0f); - else if (gnormal[2] > 0.99f) gnormal = type::Vec3( 0.0f, 0.0f, 1.0f); - for (unsigned int i=0; i cubeDim2Margin || - p2[1] < -cubeDim2Margin || p2[1] > cubeDim2Margin || - p2[2] < -cubeDim2Margin || p2[2] > cubeDim2Margin) - continue; - double d = p2*normal - cubeDim2; - - p2 -= normal * d; // push p2 to the surface - - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - - detection->point[0] = type::Vec3(p1); - detection->point[1] = type::Vec3(p2); - detection->normal = gnormal; - detection->value = d - d0; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = i; - ++nc; - } - } - else - { - // general case - for (unsigned int i=0; i cubeDim2Margin || - p2[1] < -cubeDim2Margin || p2[1] > cubeDim2Margin || - p2[2] < -cubeDim2Margin || p2[2] > cubeDim2Margin) - continue; - //double d = p2*normal - cubeDim2; - - DistanceGrid::Coord normal; - normal[0] = sofa::helper::rabs(p2[0]) - cubeDim2; - normal[1] = sofa::helper::rabs(p2[1]) - cubeDim2; - normal[2] = sofa::helper::rabs(p2[2]) - cubeDim2; - - SReal d; - // find the smallest penetration - int axis; - if (normal[0] > normal[1]) - if (normal[0] > normal[2]) axis = 0; - else axis = 2; - else if (normal[1] > normal[2]) axis = 1; - else axis = 2; - - SReal sign = (p2[axis]<0)?-1.0f:1.0f; - d = normal[axis]; - p2[axis] = sign*cubeDim2; - type::Vec3 gnormal = r2.col(axis) * -sign; - - //p2 -= normal * d; // push p2 to the surface - - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - - detection->point[0] = type::Vec3(p1); - detection->point[1] = type::Vec3(p2); - detection->normal = gnormal; - detection->value = d - d0; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = i; - ++nc; - } - } - } - else - { - for (unsigned int i=0; igrad(p1); // note that there are some redundant computations between interp() and grad() - n1.normalize(); - DistanceGrid::Coord p2 = translation + rotation*(p1 + n1*margin); -#ifdef DEBUG_XFORM - DistanceGrid::Coord p1b = rotation.multTranspose(p2-translation); - DistanceGrid::Coord gp1 = t1+r1*p1; - DistanceGrid::Coord gp2 = t2+r2*p2; - if ((p1b-p1).norm2() > 0.0001f) - serr << "ERROR1a: " << p1 << " -> " << p2 << " -> " << p1b << sendl; - if ((gp1-gp2).norm2() > 0.0001f) - serr << "ERROR1b: " << p1 << " -> " << gp1 << " " << p2 << " -> " << gp2 << sendl; -#endif - - if (!grid2->inBBox( p2 /*, margin*/ )) continue; - if (!grid2->inGrid( p2 )) - { - msg_error(intersection) << "Margin less than "<getName(); - continue; - } - - SReal d = grid2->interp(p2); - if (d >= 0 /* margin */ ) continue; - - type::Vec3 grad = grid2->grad(p2); // note that there are some redundant computations between interp() and grad() - grad.normalize(); - - //p2 -= grad * d; // push p2 back to the surface - - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - - detection->point[0] = type::Vec3(p1); - detection->point[1] = type::Vec3(p2) - grad * d; - detection->normal = r2 * -grad; // normal in global space from p1's surface - detection->value = d + margin - d0; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = i; - ++nc; - } - } - } - - // then points of e2 against distance field of e1 - const DistanceGrid::VecCoord& x2 = grid2->meshPts; - const int i0 = x1.size(); - if (!x2.empty() && e2.getCollisionModel()->usePoints.getValue()) - { - if (grid1->isCube() && grid2->isCube()) - { - const SReal cubeDim1 = grid1->getCubeDim(); - const SReal cubeDim1Margin = cubeDim1+margin; - - if (face_e1 != FACE_NONE) - { - // stacked cubes - DistanceGrid::Coord normal; - normal[face_e1/2] = (face_e1&1)?1.0f:-1.0f; - type::Vec3 gnormal = r1 * normal; - // special case: if normal in global frame is nearly vertical or horizontal, make it so - if (gnormal[0] < -0.99f) gnormal = type::Vec3(-1.0f, 0.0f, 0.0f); - else if (gnormal[0] > 0.99f) gnormal = type::Vec3( 1.0f, 0.0f, 0.0f); - if (gnormal[1] < -0.99f) gnormal = type::Vec3( 0.0f,-1.0f, 0.0f); - else if (gnormal[1] > 0.99f) gnormal = type::Vec3( 0.0f, 1.0f, 0.0f); - if (gnormal[2] < -0.99f) gnormal = type::Vec3( 0.0f, 0.0f, -1.0f); - else if (gnormal[2] > 0.99f) gnormal = type::Vec3( 0.0f, 0.0f, 1.0f); - for (unsigned int i=0; i cubeDim1Margin || - p1[1] < -cubeDim1Margin || p1[1] > cubeDim1Margin || - p1[2] < -cubeDim1Margin || p1[2] > cubeDim1Margin) - continue; - double d = p1*normal - cubeDim1; - - p1 -= normal * d; // push p2 to the surface - - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - - detection->point[0] = type::Vec3(p1); - detection->point[1] = type::Vec3(p2); - detection->normal = gnormal; - detection->value = d - d0; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = i + i0; - ++nc; - } - } - else - { - // general case - for (unsigned int i=0; i cubeDim1Margin || - p1[1] < -cubeDim1Margin || p1[1] > cubeDim1Margin || - p1[2] < -cubeDim1Margin || p1[2] > cubeDim1Margin) - continue; - - DistanceGrid::Coord normal; - normal[0] = sofa::helper::rabs(p1[0]) - cubeDim1; - normal[1] = sofa::helper::rabs(p1[1]) - cubeDim1; - normal[2] = sofa::helper::rabs(p1[2]) - cubeDim1; - - SReal d; - // find the smallest penetration - int axis; - if (normal[0] > normal[1]) - if (normal[0] > normal[2]) axis = 0; - else axis = 2; - else if (normal[1] > normal[2]) axis = 1; - else axis = 2; - - SReal sign = (p1[axis]<0)?-1.0f:1.0f; - d = normal[axis]; - p1[axis] = sign*cubeDim1; - type::Vec3 gnormal = r1.col(axis) * sign; - - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - - detection->point[0] = type::Vec3(p1); - detection->point[1] = type::Vec3(p2); - detection->normal = gnormal; - detection->value = d - d0; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = i + i0; - ++nc; - } - } -#if 0 -#if 0 - // -rotationT*translation is the position of cube2 center in cube1 space - // we use its largest component as the dominant contact face normal - /// \TODO use the relative velocity as an additional factor - type::Vec3 normal = rotation.multTranspose(-translation); - //normal[2] *= 1.1f; // we like Z contact better ;) - if (rabs(normal[0]) > rabs(normal[1])) - { - if (rabs(normal[0]) > rabs(normal[2])) - normal = type::Vec3(normal[0]>0.0f?1.0f:-1.0f,0.0f,0.0f); - else - normal = type::Vec3(0.0f,0.0f,normal[2]>0.0f?1.0f:-1.0f); - } - else - { - if (rabs(normal[1]) > rabs(normal[2])) - normal = type::Vec3(0.0f,normal[1]>0.0f?1.0f:-1.0f,0.0f); - else - normal = type::Vec3(0.0f,0.0f,normal[2]>0.0f?1.0f:-1.0f); - } - - type::Vec3 gnormal = r1 * normal; // normal in global space from p1's surface - // special case: if normal in global frame is nearly vertical, make it so - if (gnormal[2] < -0.99f) gnormal = type::Vec3(0.0f, 0.0f, -1.0f); - else if (gnormal[2] > 0.99f) gnormal = type::Vec3(0.0f, 0.0f, 1.0f); -#endif - type::Vec3 gnormal[3]; // X/Y/Z normals from p1 in global space - for (int i=0; i<3; i++) - { - gnormal[i] = r1.col(i); - // special case: if normal in global frame is nearly vertical or horizontal, make it so - if (gnormal[i][0] < -0.99f) gnormal[i] = type::Vec3(-1.0f, 0.0f, 0.0f); - else if (gnormal[i][0] > 0.99f) gnormal[i] = type::Vec3( 1.0f, 0.0f, 0.0f); - if (gnormal[i][1] < -0.99f) gnormal[i] = type::Vec3( 0.0f,-1.0f, 0.0f); - else if (gnormal[i][1] > 0.99f) gnormal[i] = type::Vec3( 0.0f, 1.0f, 0.0f); - if (gnormal[i][2] < -0.99f) gnormal[i] = type::Vec3( 0.0f, 0.0f, -1.0f); - else if (gnormal[i][2] > 0.99f) gnormal[i] = type::Vec3( 0.0f, 0.0f, 1.0f); - } - for (unsigned int i=0; i cubeDim1Margin || - p1[1] < -cubeDim1Margin || p1[1] > cubeDim1Margin || - p1[2] < -cubeDim1Margin || p1[2] > cubeDim1Margin) - continue; - - DistanceGrid::Coord p2normal = rotation.multTranspose(grid2->grad(p2)); // normal of p2, in p1's space - - DistanceGrid::Coord p1normal; - - p1normal[0] = (cubeDim1Margin - rabs(p1[0]))/(0.000001+rabs(p2normal[0])); - p1normal[1] = (cubeDim1Margin - rabs(p1[1]))/(0.000001+rabs(p2normal[1])); - p1normal[2] = (cubeDim1Margin - rabs(p1[2]))/(0.000001+rabs(p2normal[2])); - - SReal d; - type::Vec3 normal; - // find the smallest penetration - int axis; - //if (p1normal[0]*p2normal[0] < p1normal[1]*p2normal[1]) - if (p1normal[0] < p1normal[1]) - { - if (p1normal[0] < p1normal[2]) - axis = 0; - else - axis = 2; - } - else - { - if (p1normal[1] < p1normal[2]) - axis = 1; - else - axis = 2; - } - if (p1[axis]<0) - { - d = -cubeDim1 - p1[axis]; // p2normal[axis]; - p1[axis] = -cubeDim1; - normal = -gnormal[axis]; - } - else - { - d = p1[axis] - cubeDim1; // -p2normal[axis]; - p1[axis] = cubeDim1; - normal = gnormal[axis]; - } - - - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - - detection->point[0] = type::Vec3(p1); // - normal * d; - detection->point[1] = type::Vec3(p2); - detection->normal = normal; - detection->value = d - d0; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = i0+i; - ++nc; - } -#endif - } - else - { - for (unsigned int i=0; igrad(p2); // note that there are some redundant computations between interp() and grad() - n2.normalize(); - - DistanceGrid::Coord p1 = rotation.multTranspose(p2 + n2*margin - translation); -#ifdef DEBUG_XFORM - DistanceGrid::Coord p2b = translation + rotation*p1; - DistanceGrid::Coord gp1 = t1+r1*p1; - DistanceGrid::Coord gp2 = t2+r2*p2; - if ((p2b-p2).norm2() > 0.0001f) - serr << "ERROR2a: " << p2 << " -> " << p1 << " -> " << p2b << sendl; - else if ((gp1-gp2).norm2() > 0.0001f) - serr << "ERROR2b: " << p1 << " -> " << gp1 << " " << p2 << " -> " << gp2 << sendl; -#endif - - if (!grid1->inBBox( p1 /*, margin*/ )) continue; - if (!grid1->inGrid( p1 )) - { - msg_error(intersection) << "Mmargin less than "<getName(); - continue; - } - - SReal d = grid1->interp(p1); - if (d >= 0 /* margin */ ) continue; - - type::Vec3 grad = grid1->grad(p1); // note that there are some redundant computations between interp() and grad() - grad.normalize(); - - //p1 -= grad * d; // push p1 back to the surface - - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - - detection->point[0] = type::Vec3(p1) - grad * d; - detection->point[1] = type::Vec3(p2); - detection->normal = r1 * grad; // normal in global space from p1's surface - detection->value = d + margin - d0; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = i0+i; - ++nc; - } - } - } - return nc; -} - -bool RigidDistanceGridDiscreteIntersection::testIntersection(RigidDistanceGridCollisionElement&, Point&, const core::collision::Intersection* ) -{ - return true; -} - -int RigidDistanceGridDiscreteIntersection::computeIntersection(RigidDistanceGridCollisionElement& e1, Point& e2, OutputVector* contacts, const core::collision::Intersection* intersection) -{ - const std::shared_ptr grid1 = e1.getGrid(); - bool useXForm = e1.isTransformed(); - const type::Vec3& t1 = e1.getTranslation(); - const Matrix3& r1 = e1.getRotation(); - const bool flipped = e1.isFlipped(); - - const double d0 = e1.getContactDistance() + e2.getContactDistance() + intersection->getContactDistance(); - const SReal margin = 0.001f + (SReal)d0; - - - type::Vec3 p2 = e2.p(); - DistanceGrid::Coord p1; - - if (useXForm) - { - p1 = r1.multTranspose(p2-t1); - } - else p1 = p2; - - if (flipped) - { - if (!grid1->inGrid( p1 )) return 0; - } - else - { - if (!grid1->inBBox( p1, margin )) return 0; - if (!grid1->inGrid( p1 )) - { - msg_error(intersection) << "Margin less than "<getName(); - return 0; - } - } - - SReal d = grid1->interp(p1); - if (flipped) d = -d; - if (d >= margin) return 0; - - type::Vec3 grad = grid1->grad(p1); // note that there are some redundant computations between interp() and grad() - if (flipped) grad = -grad; - grad.normalize(); - - //p1 -= grad * d; // push p1 back to the surface - - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - - detection->point[0] = type::Vec3(p1) - grad * d; - detection->point[1] = type::Vec3(p2); - detection->normal = (useXForm) ? r1 * grad : grad; // normal in global space from p1's surface - detection->value = d - d0; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = e2.getIndex(); - return 1; -} - -bool RigidDistanceGridDiscreteIntersection::testIntersection(RigidDistanceGridCollisionElement&, Triangle&, const core::collision::Intersection*) -{ - return true; -} - -int RigidDistanceGridDiscreteIntersection::computeIntersection(RigidDistanceGridCollisionElement& e1, Triangle& e2, OutputVector* contacts, const core::collision::Intersection* intersection) -{ - const int f2 = e2.flags(); - if (!(f2&(TriangleCollisionModel::FLAG_POINTS|TriangleCollisionModel::FLAG_BEDGES))) return 0; // no points associated with this triangle - const std::shared_ptr grid1 = e1.getGrid(); - const bool useXForm = e1.isTransformed(); - const type::Vec3& t1 = e1.getTranslation(); - const Matrix3& r1 = e1.getRotation(); - - const double d0 = e1.getContactDistance() + e2.getContactDistance() + intersection->getContactDistance(); - const SReal margin = 0.001f + (SReal)d0; - int nc = 0; - for (unsigned int iP = 0; iP < 3; ++iP) - { - if (!(f2&(TriangleCollisionModel::FLAG_P1 << iP))) continue; - - type::Vec3 p2 = e2.p(iP); - DistanceGrid::Coord p1; - - if (useXForm) - { - p1 = r1.multTranspose(p2-t1); - } - else p1 = p2; - - if (grid1->inBBox( p1, margin )) - { - if (!grid1->inGrid( p1 )) - { - msg_error(intersection) << "Margin less than "<getName(); - } - else - { - SReal d = grid1->interp(p1); - if (d >= margin) continue; - - type::Vec3 grad = grid1->grad(p1); // note that there are some redundant computations between interp() and grad() - grad.normalize(); - - //p1 -= grad * d; // push p1 back to the surface - - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - - detection->point[0] = type::Vec3(p1) - grad * d; - detection->point[1] = type::Vec3(p2); - detection->normal = (useXForm) ? r1 * grad : grad; // normal in global space from p1's surface - detection->value = d - d0; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = e2.getIndex()*6+iP; - ++nc; - } - } - } - for (unsigned int iE = 0; iE < 3; ++iE) - { - if (!(f2&(TriangleCollisionModel::FLAG_BE23 << iE))) continue; - unsigned int iP1 = (iE+1)%3; - unsigned int iP2 = (iE+2)%3; - type::Vec3 p2 = (e2.p(iP1)+e2.p(iP2))*0.5; - - DistanceGrid::Coord p1; - - if (useXForm) - { - p1 = r1.multTranspose(p2-t1); - } - else p1 = p2; - - if (grid1->inBBox( p1, margin )) - { - if (!grid1->inGrid( p1 )) - { - msg_error(intersection) << "Margin less than "<getName(); - } - else - { - SReal d = grid1->interp(p1); - if (d >= margin) continue; - - type::Vec3 grad = grid1->grad(p1); // note that there are some redundant computations between interp() and grad() - grad.normalize(); - - //p1 -= grad * d; // push p1 back to the surface - - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - - detection->point[0] = type::Vec3(p1) - grad * d; - detection->point[1] = type::Vec3(p2); - detection->normal = (useXForm) ? r1 * grad : grad; // normal in global space from p1's surface - detection->value = d - d0; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = e2.getIndex()*6+(3+iE); - ++nc; - } - } - } - - return nc; -} - -bool RigidDistanceGridDiscreteIntersection::testIntersection(RigidDistanceGridCollisionElement&, Line&, const core::collision::Intersection*) -{ - return true; -} - -int RigidDistanceGridDiscreteIntersection::computeIntersection(RigidDistanceGridCollisionElement& e1, Line& e2, OutputVector* contacts, const core::collision::Intersection* intersection) -{ - const int f2 = e2.flags(); - if (!(f2&LineCollisionModel::FLAG_POINTS)) return 0; // no points associated with this line - const std::shared_ptr grid1 = e1.getGrid(); - const bool useXForm = e1.isTransformed(); - const type::Vec3& t1 = e1.getTranslation(); - const Matrix3& r1 = e1.getRotation(); - - const double d0 = e1.getContactDistance() + e2.getContactDistance() + intersection->getContactDistance(); - const SReal margin = 0.001f + (SReal)d0; - int nresult = 0; - for (unsigned int iP = 0; iP < 2; ++iP) - { - if (!(f2&(LineCollisionModel::FLAG_P1 << iP))) continue; - - type::Vec3 p2 = e2.p(iP); - DistanceGrid::Coord p1; - - if (useXForm) - { - p1 = r1.multTranspose(p2-t1); - } - else p1 = p2; - - if (grid1->inBBox( p1, margin )) - { - if (!grid1->inGrid( p1 )) - { - msg_error(intersection) << "Margin less than "<getName(); - } - else - { - SReal d = grid1->interp(p1); - if (d >= margin) continue; - - type::Vec3 grad = grid1->grad(p1); // note that there are some redundant computations between interp() and grad() - grad.normalize(); - - //p1 -= grad * d; // push p1 back to the surface - - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - - detection->point[0] = type::Vec3(p1) - grad * d; - detection->point[1] = type::Vec3(p2); - detection->normal = (useXForm) ? r1 * grad : grad; // normal in global space from p1's surface - detection->value = d - d0; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = e2.getIndex()*2+iP; - ++nresult; - } - } - } - - return nresult; -} - -bool RigidDistanceGridDiscreteIntersection::testIntersection(Ray& /*e2*/, RigidDistanceGridCollisionElement& /*e1*/, const core::collision::Intersection*) -{ - return true; -} - -int RigidDistanceGridDiscreteIntersection::computeIntersection(Ray& e2, RigidDistanceGridCollisionElement& e1, OutputVector* contacts, const core::collision::Intersection* intersection) -{ - type::Vec3 rayOrigin(e2.origin()); - type::Vec3 rayDirection(e2.direction()); - const double rayLength = e2.l(); - - int nc = 0; - const std::shared_ptr grid1 = e1.getGrid(); - bool useXForm = e1.isTransformed(); - - if (useXForm) - { - const type::Vec3& t1 = e1.getTranslation(); - const Matrix3& r1 = e1.getRotation(); - rayOrigin = r1.multTranspose(rayOrigin-t1); - rayDirection = r1.multTranspose(rayDirection); - // now ray infos are in grid1 space - } - - double l0 = 0; - double l1 = rayLength; - type::Vec3 r0 = rayOrigin; - type::Vec3 r1 = rayOrigin + rayDirection*l1; - - DistanceGrid::Coord bbmin = grid1->getBBMin(), bbmax = grid1->getBBMax(); - // clip along each axis - for (int c=0; c<3 && l1>l0; c++) - { - if (rayDirection[c] > 0) - { - // test if the ray is inside - if (r1[c] < bbmin[c] || r0[c] > bbmax[c]) - { l1 = 0; break; } - if (r0[c] < bbmin[c]) - { - // intersect with p[c] == bbmin[c] plane - double l = (bbmin[c]-rayOrigin[c]) / rayDirection[c]; - if(l0 < l) - { - l0 = l; - r0 = rayOrigin + rayDirection*l0; - } - } - if (r1[c] > bbmax[c]) - { - // intersect with p[c] == bbmax[c] plane - double l = (bbmax[c]-rayOrigin[c]) / rayDirection[c]; - if(l1 > l) - { - l1 = l; - r1 = rayOrigin + rayDirection*l1; - } - } - } - else - { - // test if the ray is inside - if (r0[c] < bbmin[c] || r1[c] > bbmax[c]) - { l1 = 0; break; } - if (r0[c] > bbmax[c]) - { - // intersect with p[c] == bbmax[c] plane - double l = (bbmax[c]-rayOrigin[c]) / rayDirection[c]; - if(l0 < l) - { - l0 = l; - r0 = rayOrigin + rayDirection*l0; - } - } - if (r1[c] < bbmin[c]) - { - // intersect with p[c] == bbmin[c] plane - double l = (bbmin[c]-rayOrigin[c]) / rayDirection[c]; - if(l1 > l) - { - l1 = l; - r1 = rayOrigin + rayDirection*l1; - } - } - } - - } - - if (l0 < l1) - { - // some part of the ray is inside the grid - type::Vec3 p = rayOrigin + rayDirection*l0; - double dist = grid1->interp(p); - double epsilon = grid1->getCellWidth().norm()*0.1f; - while (l0 < l1 && (dist > epsilon || dist < -epsilon)) - { - l0 += dist; - p = rayOrigin + rayDirection*l0; - dist = grid1->interp(p); - } - if (dist < epsilon) - { - // intersection found - - contacts->resize(contacts->size()+1); - DetectionOutput *detection = &*(contacts->end()-1); - - detection->point[0] = e2.origin() + e2.direction()*l0; - detection->point[1] = p; - detection->normal = e2.direction(); // normal in global space from p1's surface - detection->value = dist; - detection->elem.first = e2; - detection->elem.second = e1; - detection->id = e2.getIndex(); - ++nc; - } - } - return nc; -} - - -} // namespace collision - -} // namespace component - -} // namespace sofa - diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/RigidDistanceGridDiscreteIntersection.h b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/RigidDistanceGridDiscreteIntersection.h deleted file mode 100644 index bc884815068..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/RigidDistanceGridDiscreteIntersection.h +++ /dev/null @@ -1,75 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_COMPONENT_COLLISION_RIGIDDISTANCEGRIDDISCRETEINTERSECTION_H -#define SOFA_COMPONENT_COLLISION_RIGIDDISTANCEGRIDDISCRETEINTERSECTION_H -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "DistanceGridCollisionModel.h" - -namespace sofa -{ - -namespace component -{ - -namespace collision -{ -class SOFA_SOFADISTANCEGRID_API RigidDistanceGridDiscreteIntersection : public core::collision::BaseIntersector -{ - - typedef detection::intersection::DiscreteIntersection::OutputVector OutputVector; - -public: - RigidDistanceGridDiscreteIntersection(detection::intersection::DiscreteIntersection* intersection); - - bool testIntersection(RigidDistanceGridCollisionElement&, RigidDistanceGridCollisionElement&, const core::collision::Intersection* ); - bool testIntersection(RigidDistanceGridCollisionElement&, geometry::Point&, const core::collision::Intersection* ); - template bool testIntersection(RigidDistanceGridCollisionElement&, geometry::TSphere&, const core::collision::Intersection* ); - bool testIntersection(RigidDistanceGridCollisionElement&, geometry::Line&, const core::collision::Intersection* ); - bool testIntersection(RigidDistanceGridCollisionElement&, geometry::Triangle&, const core::collision::Intersection* ); - bool testIntersection(geometry::Ray&, RigidDistanceGridCollisionElement&, const core::collision::Intersection*); - - int computeIntersection(RigidDistanceGridCollisionElement&, RigidDistanceGridCollisionElement&, OutputVector*, const core::collision::Intersection*); - int computeIntersection(RigidDistanceGridCollisionElement&, geometry::Point&, OutputVector*, const core::collision::Intersection*); - template int computeIntersection(RigidDistanceGridCollisionElement&, geometry::TSphere&, OutputVector*, const core::collision::Intersection*); - int computeIntersection(RigidDistanceGridCollisionElement&, geometry::Line&, OutputVector*, const core::collision::Intersection*); - int computeIntersection(RigidDistanceGridCollisionElement&, geometry::Triangle&, OutputVector*, const core::collision::Intersection*); - int computeIntersection(geometry::Ray&, RigidDistanceGridCollisionElement&, OutputVector*, const core::collision::Intersection*); - -}; - -} // namespace collision - -} // namespace component - -} // namespace sofa - -#endif diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/RigidDistanceGridDiscreteIntersection.inl b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/RigidDistanceGridDiscreteIntersection.inl deleted file mode 100644 index 6e321b1aa74..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/collision/RigidDistanceGridDiscreteIntersection.inl +++ /dev/null @@ -1,101 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_COMPONENT_COLLISION_RIGIDDISTANCEGRIDDISCRETEINTERSECTION_INL -#define SOFA_COMPONENT_COLLISION_RIGIDDISTANCEGRIDDISCRETEINTERSECTION_INL -#include -#include - -#include -#include -#include - -#include "RigidDistanceGridDiscreteIntersection.h" - -namespace sofa -{ - -namespace component -{ - -namespace collision -{ - -template -bool RigidDistanceGridDiscreteIntersection::testIntersection(RigidDistanceGridCollisionElement&, geometry::TSphere&, const core::collision::Intersection*) -{ - return true; -} - -template -int RigidDistanceGridDiscreteIntersection::computeIntersection(RigidDistanceGridCollisionElement& e1, geometry::TSphere& e2, OutputVector* contacts, const core::collision::Intersection* intersection) -{ - const std::shared_ptr grid1 = e1.getGrid(); - bool useXForm = e1.isTransformed(); - const type::Vec3& t1 = e1.getTranslation(); - const sofa::type::Matrix3& r1 = e1.getRotation(); - - const double d0 = e1.getContactDistance() + e2.getContactDistance() + intersection->getContactDistance() + e2.r(); - const SReal margin = 0.001f + (SReal)d0; - - type::Vec3 p2 = e2.center(); - DistanceGrid::Coord p1; - - if (useXForm) - { - p1 = r1.multTranspose(p2-t1); - } - else p1 = p2; - - if (!grid1->inBBox( p1, margin )) return 0; - if (!grid1->inGrid( p1 )) - { - msg_error(intersection) << "Margin less than "<getName(); - return 0; - } - - SReal d = grid1->interp(p1); - if (d >= margin) return 0; - - type::Vec3 grad = grid1->grad(p1); // note that there are some redundant computations between interp() and grad() - grad.normalize(); - - //p1 -= grad * d; // push p1 back to the surface - - contacts->resize(contacts->size()+1); - core::collision::DetectionOutput *detection = &*(contacts->end()-1); - detection->normal = (useXForm) ? r1 * grad : grad; // normal in global space from p1's surface - detection->value = d - d0; - detection->elem.first = e1; - detection->elem.second = e2; - detection->id = e2.getIndex(); - detection->point[0] = type::Vec3(p1) - grad * d; - detection->point[1] = e2.getContactPointByNormal( detection->normal ); - return 1; -} - -} // namespace collision - -} // namespace component - -} // namespace sofa - -#endif diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/forcefield/DistanceGridForceField.cpp b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/forcefield/DistanceGridForceField.cpp deleted file mode 100644 index 026b1d0560d..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/forcefield/DistanceGridForceField.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#define SOFA_COMPONENT_INTERACTIONFORCEFIELD_DISTANCEGRIDFORCEFIELD_CPP -#include -#include -#include -#include "DistanceGridForceField.inl" - -namespace sofa -{ - -namespace component -{ - -namespace forcefield -{ - -using namespace sofa::defaulttype; - - -void registerDistanceGridForceField(sofa::core::ObjectFactory* factory) -{ - factory->registerObjects(sofa::core::ObjectRegistrationData("Force applied by a distancegrid toward the exterior, the interior, or the surface.") - .add< DistanceGridForceField >()); -} - -template class SOFA_SOFADISTANCEGRID_API DistanceGridForceField; - - -} // namespace forcefield - -} // namespace component - -} // namespace sofa diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/forcefield/DistanceGridForceField.h b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/forcefield/DistanceGridForceField.h deleted file mode 100644 index d3376e003bf..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/forcefield/DistanceGridForceField.h +++ /dev/null @@ -1,241 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_COMPONENT_INTERACTIONFORCEFIELD_DISTANCEGRIDFORCEFIELD_H -#define SOFA_COMPONENT_INTERACTIONFORCEFIELD_DISTANCEGRIDFORCEFIELD_H -#include -#include -#include -#include -#include -#include -#include - -namespace sofa -{ - -namespace component -{ - -namespace forcefield -{ - -/// This class can be overridden if needed for additional storage within template specializations. -template -class DistanceGridForceFieldInternalData -{ -public: -}; - -template -class DistanceGridForceField : public core::behavior::ForceField -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(DistanceGridForceField, DataTypes), SOFA_TEMPLATE(core::behavior::ForceField, DataTypes)); - - typedef core::behavior::ForceField Inherit; - typedef typename DataTypes::Real Real; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef container::DistanceGrid DistanceGrid; - -protected: - std::shared_ptr grid; - - class Contact - { - public: - int index; - Coord normal; - Real fact; - Contact( int index=0, Coord normal=Coord(),Real fact=Real(0)) - : index(index),normal(normal),fact(fact) - { - } - - inline friend std::istream& operator >> ( std::istream& in, Contact& c ) - { - in>>c.index>>c.normal>>c.fact; - return in; - } - - inline friend std::ostream& operator << ( std::ostream& out, const Contact& c ) - { - out << c.index << " " << c.normal << " " << c.fact ; - return out; - } - - }; - - sofa::type::vector pOnBorder; - Data > contacts; - - - class TContact - { - public: - type::fixed_array index; - Coord normal,B,C; - Real fact; - - inline friend std::istream& operator >> ( std::istream& in, TContact& c ) - { - in>>c.index>>c.normal>>c.B>>c.C>>c.fact; - return in; - } - - inline friend std::ostream& operator << ( std::ostream& out, const TContact& c ) - { - out << c.index << " " << c.normal << " " << c.B << " " << c.C << " " << c.fact ; - return out; - } - - }; - - Data > tcontacts; - - class VContact - { - public: - type::fixed_array index; - Coord A,B,C; - Real fact; - - inline friend std::istream& operator >> ( std::istream& in, VContact& c ) - { - in>>c.index>>c.A>>c.B>>c.C>>c.fact; - return in; - } - - inline friend std::ostream& operator << ( std::ostream& out, const VContact& c ) - { - out << c.index << " " << c.A << " " << c.B << " " << c.C << " " << c.fact ; - return out; - } - - }; - - Data > vcontacts; - - DistanceGridForceFieldInternalData data; - -public: - - // Input data parameters - sofa::core::objectmodel::DataFileName fileDistanceGrid; ///< load distance grid from specified file - Data< double > scale; ///< scaling factor for input file - Data< type::fixed_array > box; ///< Field bounding box defined by xmin,ymin,zmin, xmax,ymax,zmax - Data< int > nx; ///< number of values on X axis - Data< int > ny; ///< number of values on Y axis - Data< int > nz; ///< number of values on Z axis - - Data stiffnessIn; ///< force stiffness when inside of the object - Data stiffnessOut; ///< force stiffness when outside of the object - Data damping; ///< force damping coefficient - Data maxDist; ///< max distance of the surface after which no more force is applied - Data minArea; ///< minimal area for each triangle, as seen from the direction of the local surface (i.e. a flipped triangle will have a negative area) - Data stiffnessArea; ///< force stiffness if a triangle have an area less than minArea - Data minVolume; ///< minimal volume for each tetrahedron (a flipped triangle will have a negative volume) - Data stiffnessVolume; ///< force stiffness if a tetrahedron have an volume less than minVolume - bool flipNormals; - - Data color; ///< display color.(default=[0.0,0.5,0.2,1.0]) - Data bDraw; ///< enable/disable drawing of distancegrid - Data drawPoints; ///< enable/disable drawing of distancegrid - Data drawSize; ///< display size if draw is enabled - - /// optional range of local DOF indices. Any computation involving only indices outside of this range are discarded (useful for parallelization using mesh partitioning) - Data< type::Vec<2,int> > localRange; -protected: - DistanceGridForceField() - : grid(nullptr) - , fileDistanceGrid( initData( &fileDistanceGrid, "filename", "load distance grid from specified file")) - , scale( initData( &scale, 1.0, "scale", "scaling factor for input file")) - , box( initData( &box, "box", "Field bounding box defined by xmin,ymin,zmin, xmax,ymax,zmax") ) - , nx( initData( &nx, 64, "nx", "number of values on X axis") ) - , ny( initData( &ny, 64, "ny", "number of values on Y axis") ) - , nz( initData( &nz, 64, "nz", "number of values on Z axis") ) - , stiffnessIn(initData(&stiffnessIn, (Real)500, "stiffnessIn", "force stiffness when inside of the object")) - , stiffnessOut(initData(&stiffnessOut, (Real)0, "stiffnessOut", "force stiffness when outside of the object")) - , damping(initData(&damping, (Real)0.01, "damping", "force damping coefficient")) - , maxDist(initData(&maxDist, (Real)1.0, "maxdist", "max distance of the surface after which no more force is applied")) - , minArea(initData(&minArea, (Real)0, "minArea", "minimal area for each triangle, as seen from the direction of the local surface (i.e. a flipped triangle will have a negative area)")) - , stiffnessArea(initData(&stiffnessArea, (Real)100, "stiffnessArea", "force stiffness if a triangle have an area less than minArea")) - , minVolume(initData(&minVolume, (Real)0, "minVolume", "minimal volume for each tetrahedron (a flipped triangle will have a negative volume)")) - , stiffnessVolume(initData(&stiffnessVolume, (Real)0, "stiffnessVolume", "force stiffness if a tetrahedron have an volume less than minVolume")) - , color(initData(&color, sofa::type::RGBAColor(0.0f,0.5f,0.2f,1.0f), "color", "display color.(default=[0.0,0.5,0.2,1.0])")) - , bDraw(initData(&bDraw, false, "draw", "enable/disable drawing of distancegrid")) - , drawPoints(initData(&drawPoints, false, "drawPoints", "enable/disable drawing of distancegrid")) - , drawSize(initData(&drawSize, (Real)10.0f, "drawSize", "display size if draw is enabled")) - , localRange( initData(&localRange, type::Vec<2,int>(-1,-1), "localRange", "optional range of local DOF indices. Any computation involving only indices outside of this range are discarded (useful for parallelization using mesh partitioning)" ) ) - { - this->addAlias(&stiffnessIn,"stiffness"); - this->addAlias(&stiffnessOut,"stiffness"); - this->addAlias(&fileDistanceGrid,"fileDistanceGrid"); - } -public: - void init() override; - - void setMState( core::behavior::MechanicalState* mstate ) { this->mstate = mstate; } - - void setStiffness(Real stiffIn, Real stiffOut) - { - stiffnessIn.setValue( stiffIn ); - stiffnessOut.setValue( stiffOut ); - } - - void setDamping(Real damp) - { - damping.setValue( damp ); - } - - void addForce(const sofa::core::MechanicalParams* /*mparams*/, DataVecDeriv & dataF, const DataVecCoord & dataX , const DataVecDeriv & dataV ) override; - void addDForce(const sofa::core::MechanicalParams* mparams, DataVecDeriv& datadF , const DataVecDeriv& datadX ) override; - void addKToMatrix(const sofa::core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - SReal getPotentialEnergy(const core::MechanicalParams* /*mparams*/, const DataVecCoord& /* x */) const override - { - msg_warning() << "Get potentialEnergy not implemented"; - return 0.0; - } - void draw(const core::visual::VisualParams* vparams) override; - void drawDistanceGrid(const core::visual::VisualParams*, float size=0.0f); - - -}; - -#if !defined(SOFA_COMPONENT_INTERACTIONFORCEFIELD_DISTANCEGRIDFORCEFIELD_CPP) -extern template class SOFA_SOFADISTANCEGRID_API DistanceGridForceField; -//extern template class SOFA_SOFADISTANCEGRID_API DistanceGridForceField; -//extern template class SOFA_SOFADISTANCEGRID_API DistanceGridForceField; - -#endif - -} // namespace forcefield - -} // namespace component - -} // namespace sofa - -#endif // SOFA_COMPONENT_INTERACTIONFORCEFIELD_DISTANCEGRIDFORCEFIELD_H diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/forcefield/DistanceGridForceField.inl b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/forcefield/DistanceGridForceField.inl deleted file mode 100644 index d41cfad73ab..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/components/forcefield/DistanceGridForceField.inl +++ /dev/null @@ -1,568 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_COMPONENT_INTERACTIONFORCEFIELD_DISTANCEGRIDFORCEFIELD_INL -#define SOFA_COMPONENT_INTERACTIONFORCEFIELD_DISTANCEGRIDFORCEFIELD_INL - -#include -#include "DistanceGridForceField.h" -#include -#include -#include -#include - -#include - - -namespace sofa -{ - -namespace component -{ - -namespace forcefield -{ - - -template -void DistanceGridForceField::init() -{ - Inherit::init(); - - - if (fileDistanceGrid.getValue().empty()) - { - if (grid == nullptr) - msg_error() << "DistanceGridForceField requires an input filename." ; - /// the grid has already been set - return; - } - msg_info() << " creating "<-<"<"; - - grid = DistanceGrid::loadShared(fileDistanceGrid.getFullPath(), scale.getValue(), 0.0, - nx.getValue(),ny.getValue(),nz.getValue(), - box.getValue()[0],box.getValue()[1]); - - if (grid == nullptr) - { - sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - msg_error() << "Failed to initialize: Invalid distance grid"; - return; - } - - if (this->stiffnessArea.getValue() != 0 && this->mstate) - { - core::topology::BaseMeshTopology* topology = this->getContext()->getMeshTopology(); - if (topology && topology->getNbTriangles() > 0) - { - const core::topology::BaseMeshTopology::SeqTriangles& triangles = topology->getTriangles(); - Real sumArea = 0; - Real sumSArea = 0; - const VecCoord& p1 = this->mstate->read(core::vec_id::read_access::restPosition)->getValue(); - pOnBorder.resize(p1.size(), false); - for (unsigned int ti = 0; ti < triangles.size(); ++ti) - { - const auto& t = triangles[ti]; - Coord B = p1[t[1]]-p1[t[0]]; - Coord C = p1[t[2]]-p1[t[0]]; - Coord tN = cross(B, C); - Real area = tN.norm()/2; - Coord sN = grid->grad((p1[t[0]]+p1[t[1]]+p1[t[2]])*(1.0/3.0)); - sumArea += area; - sumSArea = (sN*tN)*0.5f; - pOnBorder[t[0]] = true; - pOnBorder[t[1]] = true; - pOnBorder[t[2]] = true; - } - msg_info() << "Surface area : " << sumArea << " ( mean " << sumArea / triangles.size() << " per triangle )" ; - flipNormals = (sumSArea < 0); - } - else - { - msg_error() << "No triangles found in topology"; - } - } - - if (this->stiffnessVolume.getValue() != 0 && this->mstate) - { - core::topology::BaseMeshTopology* topology = this->mstate->getContext()->getMeshTopology(); - if (topology && topology->getNbTetrahedra() > 0) - { - const core::topology::BaseMeshTopology::SeqTetrahedra& tetrahedra = topology->getTetrahedra(); - Real sumVolume = 0; - const VecCoord& p1 = this->mstate->read(core::vec_id::read_access::restPosition)->getValue(); - for (unsigned int ti = 0; ti < tetrahedra.size(); ++ti) - { - const auto & t = tetrahedra[ti]; - Coord A = p1[t[1]]-p1[t[0]]; - Coord B = p1[t[2]]-p1[t[0]]; - Coord C = p1[t[3]]-p1[t[0]]; - Real volume = (A*cross(B, C))/6.0f; - sumVolume += volume; - } - msg_info() << "Volume : " << sumVolume << " ( mean " << sumVolume / tetrahedra.size() << " per tetra )" ; - } - else - { - msg_error() << "No tetrahedra found in topology"; - } - } - - sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); -} - -template -void DistanceGridForceField::addForce(const sofa::core::MechanicalParams* /*mparams*/, DataVecDeriv & dataF, const DataVecCoord & dataX , const DataVecDeriv & dataV ) -{ - VecDeriv& f1 = *(dataF.beginEdit()); - const VecCoord& p1=dataX.getValue(); - const VecDeriv& v1=dataV.getValue(); - - - if (!grid) return; - //this->dfdd.resize(p1.size()); - f1.resize(p1.size()); - - sofa::type::vector& contacts = *this->contacts.beginEdit(); - contacts.clear(); - - unsigned int ibegin = 0; - unsigned int iend = p1.size(); - - if (localRange.getValue()[0] >= 0) - ibegin = localRange.getValue()[0]; - - if (localRange.getValue()[1] >= 0 && (unsigned int)localRange.getValue()[1]+1 < iend) - iend = localRange.getValue()[1]+1; - - const Real stiffIn = stiffnessIn.getValue(); - const Real stiffOut = stiffnessOut.getValue(); - const Real damp = damping.getValue(); - const Real maxdist = maxDist.getValue(); - unsigned int nbIn = 0; - for (unsigned int i=ibegin; iteval(p1[i]); - if(d > 0) - { - if (d >= maxdist || stiffOut == 0) continue; - Deriv grad = grid->tgrad(p1[i]); - Real forceIntensity = -stiffOut * d; - Real dampingIntensity = forceIntensity * damp; - Deriv force = grad * forceIntensity - v1[i]*dampingIntensity; - f1[i]+=force; - Contact c; - c.index = i; - c.normal = grad; - c.fact = forceIntensity; - contacts.push_back(c); - } - else if (d < 0) - { - if (-d >= maxdist || stiffIn == 0) continue; - Deriv grad = grid->tgrad(p1[i]); - Real forceIntensity = -stiffIn * d; - Real dampingIntensity = forceIntensity * damp; - Deriv force = grad * forceIntensity - v1[i]*dampingIntensity; - f1[i]+=force; - Contact c; - c.index = i; - c.normal = grad; - c.fact = forceIntensity; - contacts.push_back(c); - nbIn++; - } - } - - dmsg_info() << " number of points " << nbIn ; - - this->contacts.endEdit(); - - sofa::type::vector& tcontacts = *this->tcontacts.beginEdit(); - tcontacts.clear(); - - const Real stiffA = stiffnessArea.getValue(); - const Real minA = minArea.getValue(); - if (stiffA != 0) - { - core::topology::BaseMeshTopology* topology = this->getContext()->getMeshTopology(); - if (topology && topology->getNbTriangles() > 0) - { - const core::topology::BaseMeshTopology::SeqTriangles& triangles = topology->getTriangles(); - for (unsigned int ti = 0; ti < triangles.size(); ++ti) - { - const auto& t = triangles[ti]; - Coord B = p1[t[1]]-p1[t[0]]; - Coord C = p1[t[2]]-p1[t[0]]; - Coord tN = cross(B, C); - Coord tcenter = (p1[t[0]]+p1[t[1]]+p1[t[2]])*(1.0/3.0); - Coord sN = grid->tgrad(tcenter); - if (flipNormals) sN = -sN; - sN.normalize(); - Real area = (tN * sN) * 0.5f; - - if (area < minA) - { - - // lets consider A = (0,0,0) - // area = 0.5*(sNx*tNx + sNy * tNy + sNz * tNz) - // = 0.5*(sNx*(By*Cz-Bz*Cy) + sNy * (Bz*Cx-Bx*Cz) + sNz * (Bx*Cy-By*Cx)) - - // d(area) / dBx = 0.5*(sNz*Cy-sNy*Cz) - // d(area) / dBy = 0.5*(sNx*Cz-sNz*Cx) - // d(area) / dBz = 0.5*(sNy*Cx-sNx*Cy) - // d(area) / dB = 0.5*cross(C,sN) - // d(area) / dC = 0.5*cross(sN,B) - - Real forceIntensity = (stiffA * (minA-area)); - Coord fB = cross(C,sN)*forceIntensity; f1[t[1]] += fB; - Coord fC = cross(sN,B)*forceIntensity; f1[t[2]] += fC; - Coord fA = -(fB+fC); f1[t[0]] += fA; - - TContact c; - c.index = t.array(); - c.fact = minA-area; - c.normal = sN; - c.B = B; - c.C = C; - tcontacts.push_back(c); - } - } - } - } - this->tcontacts.endEdit(); - - sofa::type::vector& vcontacts = *this->vcontacts.beginEdit(); - vcontacts.clear(); - - const Real stiffV = stiffnessVolume.getValue(); - const Real minV = minVolume.getValue(); - if (stiffV != 0) - { - core::topology::BaseMeshTopology* topology = this->mstate->getContext()->getMeshTopology(); - if (topology && topology->getNbTetrahedra() > 0) - { - const core::topology::BaseMeshTopology::SeqTetrahedra& tetrahedra = topology->getTetrahedra(); - const Real v1_6 = (Real)(1.0/6.0); - for (unsigned int ti = 0; ti < tetrahedra.size(); ++ti) - { - const auto& t = tetrahedra[ti]; - Coord A = p1[t[1]]-p1[t[0]]; - Coord B = p1[t[2]]-p1[t[0]]; - Coord C = p1[t[3]]-p1[t[0]]; - Real volume = (A*cross(B, C))*v1_6; - - if (volume < minV) - { - // vol = 1/6*(A(BxC)) - // = 1/6*(Ax*(By*Cz-Bz*Cy) + Ay * (Bz*Cx-Bx*Cz) + Az * (Bx*Cy-By*Cx)) - - // d(vol) / dBx = 1/6*(Az*Cy-Ay*Cz) - // d(vol) / dBy = 1/6*(Ax*Cz-Az*Cx) - // d(vol) / dBz = 1/6*(Ay*Cx-Ax*Cy) - // d(vol) / dA = 1/6*cross(B,C) - // d(vol) / dB = 1/6*cross(C,A) - // d(vol) / dC = 1/6*cross(A,B) - - Real forceIntensity = v1_6*(stiffV * (minV-volume)); - Coord fA = cross(B,C)*forceIntensity; f1[t[1]] += fA; - Coord fB = cross(C,A)*forceIntensity; f1[t[2]] += fB; - Coord fC = cross(A,B)*forceIntensity; f1[t[3]] += fC; - Coord f0 = -(fA+fB+fC); f1[t[0]] += f0; - - VContact c; - c.index = t.array(); - c.fact = minV-volume; - c.A = A; - c.B = B; - c.C = C; - vcontacts.push_back(c); - } - } - } - } - this->vcontacts.endEdit(); - - dataF.endEdit(); - -} - -template -void DistanceGridForceField::addDForce(const sofa::core::MechanicalParams* mparams, DataVecDeriv& datadF , const DataVecDeriv& datadX ) -{ - VecDeriv& df1 = *(datadF.beginEdit()); - const VecCoord& dx1= datadX.getValue() ; - Real kFactor = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); - - if (!grid) - return; - - const sofa::type::vector& contacts = this->contacts.getValue(); - const sofa::type::vector& tcontacts = this->tcontacts.getValue(); - const sofa::type::vector& vcontacts = this->vcontacts.getValue(); - - if (contacts.empty() && tcontacts.empty() && vcontacts.empty()) - return; - - df1.resize(dx1.size()); - const Real fact = (Real)(kFactor); - - for (unsigned int i=0; icontacts.getValue())[i]; - Coord du = dx1[c.index]; - - Real dd = du * c.normal; - Deriv dforce = c.normal * (dd * c.fact * fact); - df1[c.index] += dforce; - } - - const Real factA = (Real)( -this->stiffnessArea.getValue()* kFactor ); - for (unsigned int i=0; itcontacts.getValue())[i]; - const type::fixed_array& t = c.index; - Coord dB = dx1[t[1]]-dx1[t[0]]; - Coord dC = dx1[t[2]]-dx1[t[0]]; - - // d(area) = dot(0.5*cross(C,sN), dB) + dot(0.5*cross(sN,B), dC) - Real darea = 0.5f*(cross(c.C,c.normal)*dB + cross(c.normal,c.B)*dC); - - // fB = (C x sN) * (stiffA * (minA-area)); - // dfB = (C x sN) * (-stiffA * d(area)) + (dC x sN) * (stiffA * (minA-area)); - Coord dfB = cross(c.C, c.normal) * (factA * darea) - cross (dC, c.normal) * (factA*c.fact); - Coord dfC = cross(c.normal, c.B) * (factA * darea) - cross (c.normal, dB) * (factA*c.fact); - Coord dfA = -(dfB+dfC); - df1[t[0]] += dfA; - df1[t[1]] += dfB; - df1[t[2]] += dfC; - } - - const Real factV = (Real)( - this->stiffnessVolume.getValue()*(1.0/6.0)* kFactor ); - for (unsigned int i=0; ivcontacts.getValue())[i]; - const type::fixed_array& t = c.index; - Coord dA = dx1[t[1]]-dx1[t[0]]; - Coord dB = dx1[t[2]]-dx1[t[0]]; - Coord dC = dx1[t[3]]-dx1[t[0]]; - // d(vol) = 1/6*(dot(cross(B,C), dA) + dot(cross(C,A), dB) + dot(cross(A,B), dC)) - Real dvolume = v1_6*(dA*cross(c.B,c.C) + dB*cross(c.C,c.A) + dC*cross(c.A,c.B)); - - // fA = (1/6)*(B x C)*(stiffV * (minV-volume)) - // dfA = (stiffV*1/6) * ((dB x C + B x dC) * (minV-volume) - (B x C) * dvol) - // dfB = (stiffV*1/6) * ((dC x A + C x dA) * (minV-volume) - (C x A) * dvol) - // dfC = (stiffV*1/6) * ((dA x B + A x dB) * (minV-volume) - (A x B) * dvol) - Coord dfA = cross(c.B, c.C) * (factV * dvolume) - (cross(dB, c.C) + cross(c.B, dC)) * (factV*c.fact); - Coord dfB = cross(c.C, c.A) * (factV * dvolume) - (cross(dC, c.A) + cross(c.C, dA)) * (factV*c.fact); - Coord dfC = cross(c.A, c.B) * (factV * dvolume) - (cross(dA, c.B) + cross(c.A, dB)) * (factV*c.fact); - Coord df0 = -(dfA+dfB+dfC); - df1[t[1]] += dfA; - df1[t[2]] += dfB; - df1[t[3]] += dfC; - df1[t[0]] += df0; - } - - datadF.endEdit(); -} - -template -void DistanceGridForceField::addKToMatrix(const sofa::core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); - Real kFactor = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); - unsigned int &offset = r.offset; - sofa::linearalgebra::BaseMatrix* mat = r.matrix; - - if (r) - { - if (!grid) return; - const sofa::type::vector& contacts = this->contacts.getValue(); - if (contacts.empty()) return; - for (unsigned int i=0; iadd(offset + p*Deriv::total_size + l, offset + p*Deriv::total_size + c, coef); - } - } - } -} - -template -void DistanceGridForceField::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowForceFields()) return; - drawDistanceGrid(vparams); -} - -template -void DistanceGridForceField::drawDistanceGrid(const core::visual::VisualParams* vparams,float size) -{ - if (!grid) return; - if (size == 0.0f) size = (float)drawSize.getValue(); - - const VecCoord& p1 = this->mstate->read(core::vec_id::read_access::position)->getValue(); - - std::vector< type::Vec3 > pointsLineIn; - std::vector< type::Vec3 > pointsLineOut; - // lines for points penetrating the distancegrid - - unsigned int ibegin = 0; - unsigned int iend = p1.size(); - - if (localRange.getValue()[0] >= 0) - ibegin = localRange.getValue()[0]; - - if (localRange.getValue()[1] >= 0 && (unsigned int)localRange.getValue()[1]+1 < iend) - iend = localRange.getValue()[1]+1; - - const Real stiffIn = stiffnessIn.getValue(); - const Real stiffOut = stiffnessOut.getValue(); - const Real maxdist = maxDist.getValue(); - - type::Vec3 point1,point2; - for (unsigned int i=ibegin; iteval(p1[i]); - if (d > 0) - { - if (d >= maxdist || stiffOut == 0) continue; - } - else if (d < 0) - { - if (-d >= maxdist || stiffIn == 0) continue; - } - else continue; - Coord p2 = p1[i]; - Deriv normal = grid->tgrad(p1[i]); //normal.normalize(); - p2 += normal*(-d); - point1 = DataTypes::getCPos(p1[i]); - point2 = DataTypes::getCPos(p2); - if (d > 0) - { - pointsLineOut.push_back(point1); - pointsLineOut.push_back(point2); - } - else //if (d < 0) - { - pointsLineIn.push_back(point1); - pointsLineIn.push_back(point2); - } - } - if (!pointsLineIn.empty()) - vparams->drawTool()->drawLines(pointsLineIn, 1, sofa::type::RGBAColor(1.,0.,0.,1.)); - if (!pointsLineOut.empty()) - vparams->drawTool()->drawLines(pointsLineOut, 1, sofa::type::RGBAColor(1.,0.,1.,1.)); - - const sofa::type::vector& tcontacts = this->tcontacts.getValue(); - if (!tcontacts.empty()) - { - std::vector< type::Vec3 > pointsTri; - for (unsigned int i=0; itcontacts.getValue())[i]; - type::Vec3 p; - for (int j=0; j<3; ++j) - { - p = DataTypes::getCPos(p1[c.index[j]]); - pointsTri.push_back(p); - } - } - vparams->drawTool()->drawTriangles(pointsTri, sofa::type::RGBAColor{ 1.0f,0.2f,0.2f,0.5f }); - } - const sofa::type::vector& vcontacts = this->vcontacts.getValue(); - if (!vcontacts.empty()) - { - std::vector< type::Vec3 > pointsTet; - for (unsigned int i=0; ivcontacts.getValue())[i]; - const type::fixed_array& t = c.index; - type::Vec3 p[4]; - Coord pc = (p1[t[0]]+p1[t[1]]+p1[t[2]]+p1[t[3]])*0.25f; - for (int j=0; j<4; ++j) - { - Coord pj = p1[c.index[j]]; - pj += (pc-pj)*0.2f; - p[j] = DataTypes::getCPos(pj); - } - pointsTet.push_back(p[0]); - pointsTet.push_back(p[1]); - pointsTet.push_back(p[2]); - pointsTet.push_back(p[0]); - pointsTet.push_back(p[2]); - pointsTet.push_back(p[3]); - pointsTet.push_back(p[0]); - pointsTet.push_back(p[3]); - pointsTet.push_back(p[1]); - pointsTet.push_back(p[1]); - pointsTet.push_back(p[3]); - pointsTet.push_back(p[2]); - } - vparams->drawTool()->drawTriangles(pointsTet, sofa::type::RGBAColor{ 0.8f,0.8f,0.0f,0.25f }); - } - - if (drawPoints.getValue()) - { - std::vector< type::Vec3 > distancePointsIn; - std::vector< type::Vec3 > distancePointsOut; - - for (int i=0; i < grid->getNx(); i++) - for (int j=0; j < grid->getNy(); j++) - for (int k=0; k < grid->getNz(); k++) - { - Coord cellCoord = grid->coord(i,j,k); - if (grid->teval(cellCoord) < 0.0) - distancePointsIn.push_back(cellCoord); - else - distancePointsOut.push_back(cellCoord); - } - - if (distancePointsIn.size()) - vparams->drawTool()->drawPoints(distancePointsIn, (float)drawSize.getValue(), sofa::type::RGBAColor{ 0.8f,0.2f,0.2f,1.0f }); - if (distancePointsOut.size()) - vparams->drawTool()->drawPoints(distancePointsOut, (float)drawSize.getValue() * 1.2f, sofa::type::RGBAColor{ 0.2f,0.8f,0.2f,1.0f }); - } - -} -} // namespace forcefield - -} // namespace component - -} // namespace sofa - -#endif // SOFA_COMPONENT_INTERACTIONFORCEFIELD_DISTANCEGRIDFORCEFIELD_INL diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/config.h.in b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/config.h.in deleted file mode 100644 index 512419c34c3..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/config.h.in +++ /dev/null @@ -1,43 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_SOFADISTANCEGRID_CONFIG_H -#define SOFA_SOFADISTANCEGRID_CONFIG_H - -#include - -#cmakedefine01 SOFADISTANCEGRID_HAVE_MINIFLOWVR -#cmakedefine01 SOFADISTANCEGRID_HAVE_SOFA_GL - -#ifdef SOFA_BUILD_SOFADISTANCEGRID -# define SOFA_TARGET @PROJECT_NAME@ -# define SOFA_SOFADISTANCEGRID_API SOFA_EXPORT_DYNAMIC_LIBRARY -#else -# define SOFA_SOFADISTANCEGRID_API SOFA_IMPORT_DYNAMIC_LIBRARY -#endif - -namespace sofadistancegrid -{ - constexpr const char* MODULE_NAME = "@PROJECT_NAME@"; - constexpr const char* MODULE_VERSION = "@PROJECT_VERSION@"; -} - -#endif diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/initSofaDistanceGrid.cpp b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/initSofaDistanceGrid.cpp deleted file mode 100644 index a358bdbb1e0..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/initSofaDistanceGrid.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#include -#include "components/collision/DistanceGridCollisionModel.h" -#include "components/forcefield/DistanceGridForceField.h" -#include "RegisterModelToCollisionFactory.h" - -#include -#include - -namespace sofa::component::forcefield -{ - extern void registerDistanceGridForceField(sofa::core::ObjectFactory* factory); -} -namespace sofa::component::collision -{ - extern void registerRigidDistanceGridCollisionModel(sofa::core::ObjectFactory* factory); - extern void registerFFDDistanceGridCollisionModel(sofa::core::ObjectFactory* factory); -} - -namespace sofadistancegrid -{ -extern "C" { - SOFA_SOFADISTANCEGRID_API void initExternalModule(); - SOFA_SOFADISTANCEGRID_API const char* getModuleName(); - SOFA_SOFADISTANCEGRID_API const char* getModuleVersion(); - SOFA_SOFADISTANCEGRID_API const char* getModuleLicense(); - SOFA_SOFADISTANCEGRID_API const char* getModuleDescription(); - SOFA_SOFADISTANCEGRID_API void registerObjects(sofa::core::ObjectFactory* factory); -} - -void initExternalModule() -{ - initSofaDistanceGrid(); -} - -void initSofaDistanceGrid() -{ - static bool first = true; - if (first) - { - first = false; - } - // make sure that this plugin is registered into the PluginManager - sofa::helper::system::PluginManager::getInstance().registerPlugin(MODULE_NAME); - - sofa::component::collision::registerDistanceGridCollisionModel(); -} - -const char* getModuleName() -{ - return sofadistancegrid::MODULE_NAME; -} - -const char* getModuleVersion() -{ - return sofadistancegrid::MODULE_VERSION; -} - -const char* getModuleLicense() -{ - return "LGPL"; -} - -const char* getModuleDescription() -{ - return "A distance grid stores the distance to an object into a 3d regular grid. " - "This is an efficient data structure to get a distance approximation for " - "point in space. This is why it is often used to implement collisions. "; -} - -void registerObjects(sofa::core::ObjectFactory* factory) -{ - sofa::component::forcefield::registerDistanceGridForceField(factory); - sofa::component::collision::registerRigidDistanceGridCollisionModel(factory); - sofa::component::collision::registerFFDDistanceGridCollisionModel(factory); -} - -} /// namespace sofadistancegrid diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/initSofaDistanceGrid.h b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/initSofaDistanceGrid.h deleted file mode 100644 index 7d3fcf19084..00000000000 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/initSofaDistanceGrid.h +++ /dev/null @@ -1,31 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 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 Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#pragma once -#include - -namespace sofadistancegrid -{ - -void SOFA_SOFADISTANCEGRID_API initSofaDistanceGrid(); - -} // namespace sofadistancegrid -