├── .gitignore ├── LICENSE ├── Makefile ├── README.org ├── makefiles ├── Makefile ├── run.sh └── test_make.sh ├── scripts ├── .gitignore ├── README.org ├── docs │ └── .gitignore ├── plot_results.py └── requirements.txt └── source ├── ArgumentParser.h ├── ArithmeticPrecision.h ├── Elbow.cpp ├── Elbow.h ├── ExternalContact.h ├── FixedJoint.cpp ├── FixedJoint.h ├── Flagella.cpp ├── Flagella.h ├── GeometryFunctions.cpp ├── GeometryFunctions.h ├── HingeJoint.cpp ├── HingeJoint.h ├── InstabilityHelical.cpp ├── InstabilityHelical.h ├── Interaction.h ├── MRAGEnvironment.h ├── MRAGProfiler.cpp ├── MRAGProfiler.h ├── MathFunctions.cpp ├── MathFunctions.h ├── Matrix3.cpp ├── Matrix3.h ├── MuscularSnake.cpp ├── MuscularSnake.h ├── Polymer.cpp ├── Polymer.h ├── PolymerIntegrator.cpp ├── PolymerIntegrator.h ├── PositionVerlet2nd.cpp ├── PositionVerlet2nd.h ├── PullingMuscle.cpp ├── PullingMuscle.h ├── QuasistaticTimoshenkoBeam.cpp ├── QuasistaticTimoshenkoBeam.h ├── Rod.cpp ├── Rod.h ├── RodBoundaryConditions.h ├── RodExternalForces.h ├── RodInitialConfigurations.cpp ├── RodInitialConfigurations.h ├── SimpleConnection.h ├── Snake.cpp ├── Snake.h ├── SpeedFunctions.cpp ├── SpeedFunctions.h ├── SphericalJoint.cpp ├── SphericalJoint.h ├── SplineProfileZeroEnds.cpp ├── SplineProfileZeroEnds.h ├── Test.h ├── Tolerance.h ├── UsualHeaders.h ├── Vector3.cpp ├── Vector3.h ├── VectorFunctions.h ├── Walker.cpp ├── Walker.h ├── Wing.cpp ├── Wing.h └── main.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # General 2 | .DS_Store 3 | .AppleDouble 4 | .LSOverride 5 | .commit 6 | 7 | # Icon must end with two \r 8 | Icon 9 | 10 | 11 | # Thumbnails 12 | ._* 13 | 14 | # Files that might appear in the root of a volume 15 | .DocumentRevisions-V100 16 | .fseventsd 17 | .Spotlight-V100 18 | .TemporaryItems 19 | .Trashes 20 | .VolumeIcon.icns 21 | .com.apple.timemachine.donotpresent 22 | 23 | # Directories potentially created on remote AFP share 24 | .AppleDB 25 | .AppleDesktop 26 | Network Trash Folder 27 | Temporary Items 28 | .apdisk 29 | 30 | # Prerequisites 31 | *.d 32 | 33 | # Compiled Object files 34 | *.slo 35 | *.lo 36 | *.o 37 | *.obj 38 | 39 | # Precompiled Headers 40 | *.gch 41 | *.pch 42 | 43 | # Compiled Dynamic libraries 44 | *.so 45 | *.dylib 46 | *.dll 47 | 48 | # Fortran module files 49 | *.mod 50 | *.smod 51 | 52 | # Compiled Static libraries 53 | *.lai 54 | *.la 55 | *.a 56 | *.lib 57 | 58 | # Executables 59 | *.exe 60 | *.out 61 | *.app 62 | 63 | # run directory 64 | run_t/ 65 | run_*/ 66 | 67 | # executables 68 | makefiles/* 69 | !makefiles/test_make.sh 70 | !makefiles/run.sh 71 | !makefiles/Makefile 72 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 mattialab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | all: black fmt localcheck flake8 2 | 3 | black: 4 | @black --version 5 | @find . -maxdepth 3 -name '*.py'\ 6 | | while read -r src; do black "$$src"; done 7 | 8 | flake8: 9 | @flake8 --version 10 | @find . -maxdepth 3 -name '*.py'\ 11 | | while read -r src; do flake8 "$$src"; done 12 | 13 | pylint: 14 | @pylint --version 15 | @find . -maxdepth 3 -name '*.py'\ 16 | | while read -r src; do pylint -rn "$$src"; done 17 | 18 | # The last regexp in find matches bash and zsh files 19 | fmt: 20 | @shfmt --version 21 | @find . -type f -maxdepth 3 -name '*.sh' | while read -r src; do shfmt -w "$$src"; done 22 | 23 | localcheck: 24 | @shellcheck --version 25 | @find . -type f -maxdepth 3 -name '*.sh' | while read -r src; do shellcheck -s bash -e SC1071 "$$src"; done 26 | -------------------------------------------------------------------------------- /README.org: -------------------------------------------------------------------------------- 1 | #+TITLE: Elastica 2 | #+DATE:<2019-09-06 Fri> 3 | 4 | [[https://github.com/mattialab/elastica/blob/master/LICENSE.txt][https://img.shields.io/badge/license-MIT-blue.svg]] 5 | [[https://en.wikipedia.org/wiki/C%2B%2B#Standardization][https://img.shields.io/badge/c%2B%2B-11-blue.svg]] 6 | 7 | Elastica is a software to simulate the dynamics of filaments that, at every 8 | cross-section, can undergo all six possible modes of deformation, allowing the 9 | filament to bend, twist, stretch and shear, while interacting with complex 10 | environments via muscular activity, surface contact, friction and hydrodynamics. 11 | 12 | This repository provides a prototyping, non-parallel (single threaded, single node) 13 | code in ~C++~ that supplements the paper "Modeling and simulation of complex dynamic musculoskeletal architectures, Nature Communications, 2019." 14 | 15 | The code provided here is meant to be a tutorial into assembling and simulating active architectures similar to the ones in the above paper: 16 | 17 | 1. It presents a single Cosserat rod implementation according to the above paper and "Forward and inverse problems in the mechanics of soft filaments. Royal Society Open Science, 2018." 18 | 19 | 2. It illustrates the use of a single rod in the case of slithering snakes, and a comparison with the Timoshenko beam deflection to highlight the effect of shear. 20 | 21 | 3. It extends the use of single Cosserat rods to simple assemblies of rods, and presents three toy problems implementing the main joint connections employed in "Modeling and simulation of complex dynamic musculoskeletal architectures, Nat Comm, 2019." 22 | 23 | 4. It illustrates the assembly and simulation of dynamic musculoskeletal structures similar to those in the Nat Comm, 2019 (although not identical due to the use of a legacy code). An HPC version of Elastica will eventually replace this repository. 24 | 25 | The code has been tested with ~C++-11~-compliant ~gcc~ compilers 26 | from versions ~4.9.4~ and above on ~Linux~ and ~MacOS~. This version of the code is 27 | not optimized and non-parallel, so some examples are *slow* to run, we indicate 28 | this in the scripts provided. 29 | 30 | The code itself is distributed with a MIT License, but the authors retain 31 | copyright for the examples shown in this repository. 32 | 33 | * Quick-Use Guide 34 | To understand the code, starting from the fundamental rod model used, boundary 35 | conditions, connections between rods, we suggest taking the route below in running 36 | and understanding cases (for a complete explanation, see the /Commands/ section below). 37 | 38 | ** Basic tests to check the integrity of the Cosserat rod's core (individual rod) 39 | 1) timoshenkobeam in =QuasiStaticTimoshenkoBeam.cpp= 40 | 2) localized helical buckling in =InstabilityHelical.cpp= 41 | 3) snake in =Snake.cpp= 42 | 43 | ** Basic examples of rods' assembly (multiple rods) 44 | 1) sphericaljoint in =SphericalJoint.cpp= 45 | 2) hingejoint in =HingeJoint.cpp= 46 | 3) fixedjoint in =FixedJoint.cpp= 47 | 4) pullingmuscle in =PullingMuscle.cpp= 48 | 49 | When you reach this point, you should be able to assemble your own complex 50 | structures. The examples below rely on the same concepts as the first six. 51 | 52 | ** More complex architectures similar to the ones in the Nat Comm, 2019 paper 53 | 1) flagella in =Flagella.cpp= 54 | 2) muscularsnake in =MuscularSnake.cpp= 55 | 3) walker in =Walker.cpp= 56 | 4) elbow in =Elbow.cpp= 57 | 5) wing in =Wing.cpp= 58 | 59 | For processing and plotting results from these cases, use the 60 | ~plot_results.py~ script found in the ~scripts~ folder (see *Visualizing 61 | Results* section 62 | below and the ~README.org~ in the ~scripts~ folder for more information. 63 | 64 | * Usage 65 | While the repo contains code to simulate individual rods by themselves, we 66 | organized it to run several examples off-the-shelf. 67 | 68 | ** Prerequisites 69 | Our prerequisites are ~GNU Make~ and a ~C++-11~ compliant ~gcc~ compiler for 70 | running the code (there are known bugs with other compilers, see the section 71 | on /Bugs/ below). For visualizing the date, we use ~Python3~ (any version 72 | should be fine) using ~numpy,scipy~ and ~matplotlib~ as packages (see 73 | ~README.org~ file in the ~scripts~ folder for more details). 74 | 75 | ** Installation 76 | The Makefile provided can be used to compile one of many executables running 77 | a specific case, like so: 78 | #+begin_src sh 79 | cd makefiles 80 | make clean 81 | make -j8 # default target is elbow 82 | ./ 83 | #+end_src 84 | 85 | For ease of use however, we provide a handy bash script that makes and runs 86 | the provided examples: 87 | #+begin_src sh 88 | cd makefiles 89 | ./run.sh 90 | #+end_src 91 | The next section provides more details on using this script. 92 | 93 | ** Commands 94 | Upon running the ~run.sh~ with no arguments, it prints the following 95 | descriptive help message: 96 | 97 | #+begin_src sh 98 | usage 99 | ----- 100 | ./run.sh 101 | 102 | case_names and explanations 103 | --------------------------- 104 | help : Print this help message 105 | 106 | timoshenkobeam : Runs a cantilevered slender beam (clamped at the wall at one 107 | end, free at another) bending under the influence of a downward point force. 108 | We compare against analytical results using Timoshenko Beam theory. 109 | 110 | helicalbuckling : Runs the localized helical buckling benchmark which tests 111 | the mixed bending modes (two bending modes + twist mode). When an unstretchable 112 | rod is twisted at two ends, a characteristic instability arises which forms a 113 | localized helical shape at the center of the beam. There is an analytical solution 114 | (see Gazzola et. al, RSOS, 2018), which we compare against. 115 | 116 | snake : Runs an example of an snake, actuated by a continuum analytical bend- 117 | ing torque profile slithering on a plane-ground with anisotropic friction. 118 | This snake has been optimized for maximal forward speed. 119 | 120 | sphericaljoint : Runs an example of two rods connected by a spherical joint 121 | that allows for arbitrary rotations (see SphericalJoint.cpp) 122 | 123 | hingejoint : Runs an example of two rods connected by a hinge joint that 124 | allows for motion only along a constrained plane (see HingeJoint.cpp) 125 | 126 | fixedjoint : Runs an example of two rods connected by a fixed joint that 127 | does not allow any motion between the rods (see FixedJoint.cpp) 128 | 129 | pullingmuscle : Runs an example of two hanging vertical rods (connected to 130 | ground by a hinge) connected to each another's centers by a horizontal 131 | muscle. This muscle is actuated and pulls the rods together. 132 | (see PullingMuscle.cpp) 133 | 134 | elbow : Runs the injured elbow (50% Strength) with artificial muscles, 135 | as seen in Fig.7 in Supplementary Material. 136 | 137 | flagella : Runs the original biohybrid flagella design seen in Fig.2(a,b). 138 | 139 | walker : Runs the original biohybrid walker design at a frequency of 2 Hz, 140 | as seen in Fig.2(e,f). SLOW without parallelization. 141 | 142 | muscularsnake : Runs the muscular snake seen in Fig.3. SLOW without parallelization. 143 | 144 | wing : Runs the wing seen in Fig.4. EXTREMELY SLOW without parallelization. 145 | 146 | case_names 147 | ---------- 148 | timoshenkobeam, helicalbuckling, sphericaljoint, hingejoint, fixedjoint, pullingmuscle, snake 149 | flagella, muscularsnake, walker, elbow, wing 150 | 151 | All results are stored in folder 'run_' in the main directory 152 | #+end_src 153 | 154 | To run a specific example then (e.g. walker) type =./run.sh walker=. 155 | 156 | ** Visualizing Results 157 | To process the results using ~plot_results.py~, run 158 | #+begin_src sh 159 | ./plot_results.py -c 160 | #+end_src 161 | where the ~case_name~ is the same as before. This script visualizes 162 | the data and saves the files automatically. More details in the ~README.org~ 163 | file in the ~scripts~ folder. 164 | 165 | ** Known bugs 166 | The code, at the moment, only works with ~gcc~ compiler family (Apple 167 | ~clang~, source ~clang~, ~icc~ do not work). This will be corrected in the 168 | near future. All examples in the paper are run with ~g++ 8.2.0~ with ~-O3~ 169 | optimization and user warnings. 170 | 171 | ** Troubleshooting 172 | Because of the above bugs, we hard-coded ~g++~ in line 5 of our ~Makefile~. 173 | In case ~make~ throws an error during compile-time, the user is encouraged to 174 | change the path of the ~CC~ executable in the ~Makefile~. 175 | -------------------------------------------------------------------------------- /makefiles/Makefile: -------------------------------------------------------------------------------- 1 | hn = $(shell hostname) 2 | username = $(shell whoami) 3 | 4 | #CC=$(shell which g++) 5 | #CC=/usr/local/Cellar/gcc/8.2.0/bin/g++-8 6 | CPPSETTINGS+= -std=c++11 7 | CPPFLAGS+= -O3 -Wno-all -Wno-deprecated -w -Wno-reorder #-march=native 8 | 9 | COMMON_OBJS = \ 10 | main.o \ 11 | GeometryFunctions.o \ 12 | Polymer.o \ 13 | PolymerIntegrator.o \ 14 | PositionVerlet2nd.o \ 15 | Rod.o \ 16 | SplineProfileZeroEnds.o \ 17 | RodInitialConfigurations.o \ 18 | MRAGProfiler.o \ 19 | Vector3.o \ 20 | Matrix3.o \ 21 | SpeedFunctions.o \ 22 | MathFunctions.o 23 | 24 | VPATH := ../source/ 25 | .DEFAULT_GOAL := elbow 26 | 27 | #Linking stage 28 | elbow: CPPSETTINGS += -DFLAGELBOW 29 | elbow: $(COMMON_OBJS) Elbow.o 30 | $(CC) $(CPPSETTINGS) $(LIB) $^ -o $@ 31 | @echo done 32 | 33 | #Linking stage 34 | flagella: CPPSETTINGS += -DFLAGFLAGELLA 35 | flagella: $(COMMON_OBJS) Flagella.o 36 | $(CC) $(CPPSETTINGS) $(LIB) $^ -o $@ 37 | @echo done 38 | 39 | #Linking stage 40 | walker: CPPSETTINGS += -DFLAGWALKER 41 | walker: $(COMMON_OBJS) Walker.o 42 | $(CC) $(CPPSETTINGS) $(LIB) $^ -o $@ 43 | @echo done 44 | 45 | #Linking stage 46 | muscularsnake: CPPSETTINGS += -DFLAGMUSCULARSNAKE 47 | muscularsnake: $(COMMON_OBJS) MuscularSnake.o 48 | $(CC) $(CPPSETTINGS) $(LIB) $^ -o $@ 49 | @echo done 50 | 51 | #Linking stage 52 | wing: CPPSETTINGS += -DFLAGWING 53 | wing: $(COMMON_OBJS) Wing.o 54 | $(CC) $(CPPSETTINGS) $(LIB) $^ -o $@ 55 | @echo done 56 | 57 | #Linking stage 58 | sphericaljoint: CPPSETTINGS += -DFLAGSPHERICALJOINT 59 | sphericaljoint: $(COMMON_OBJS) SphericalJoint.o 60 | $(CC) $(CPPSETTINGS) $(LIB) $^ -o $@ 61 | @echo done 62 | 63 | #Linking stage 64 | hingejoint: CPPSETTINGS += -DFLAGHINGEJOINT 65 | hingejoint: $(COMMON_OBJS) HingeJoint.o 66 | $(CC) $(CPPSETTINGS) $(LIB) $^ -o $@ 67 | @echo done 68 | 69 | #Linking stage 70 | fixedjoint: CPPSETTINGS += -DFLAGFIXEDJOINT 71 | fixedjoint: $(COMMON_OBJS) FixedJoint.o 72 | $(CC) $(CPPSETTINGS) $(LIB) $^ -o $@ 73 | @echo done 74 | 75 | #Linking stage 76 | pullingmuscle: CPPSETTINGS += -DFLAGPULLINGMUSCLE 77 | pullingmuscle: $(COMMON_OBJS) PullingMuscle.o 78 | $(CC) $(CPPSETTINGS) $(LIB) $^ -o $@ 79 | @echo done 80 | 81 | #Linking stage 82 | timoshenkobeam: CPPSETTINGS += -DFLAGQUASISTATICTIMOSHENKOBEAM 83 | timoshenkobeam: $(COMMON_OBJS) QuasistaticTimoshenkoBeam.o 84 | $(CC) $(CPPSETTINGS) $(LIB) $^ -o $@ 85 | @echo done 86 | 87 | #Linking stage 88 | helicalbuckling: CPPSETTINGS += -DFLAGHELICALBUCKLING 89 | helicalbuckling: $(COMMON_OBJS) InstabilityHelical.o 90 | $(CC) $(CPPSETTINGS) $(LIB) $^ -o $@ 91 | @echo done 92 | 93 | #Linking stage 94 | snake: CPPSETTINGS += -DFLAGSNAKE 95 | snake: $(COMMON_OBJS) Snake.o 96 | $(CC) $(CPPSETTINGS) $(LIB) $^ -o $@ 97 | @echo done 98 | 99 | #Compiling stage 100 | %.o: %.cpp 101 | $(CC) $(CPPSETTINGS) $(CPPFLAGS) -c $^ -o $@ 102 | 103 | clean: 104 | rm -f *.o 105 | rm -f *.s 106 | rm -f timoshenkobeam 107 | rm -f helicalbuckling 108 | rm -f snake 109 | rm -f sphericaljoint 110 | rm -f hingejoint 111 | rm -f fixedjoint 112 | rm -f pullingmuscle 113 | rm -f elbow 114 | rm -f flagella 115 | rm -f walker 116 | rm -f muscularsnake 117 | rm -f wing 118 | -------------------------------------------------------------------------------- /makefiles/run.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # Not posix compliant 3 | 4 | read -rd '' globalhelp <<-EOF 5 | usage 6 | ----- 7 | ./run.sh 8 | 9 | case_names and explanations 10 | --------------------------- 11 | help : Print this help message 12 | 13 | timoshenkobeam : Runs a cantilevered slender beam (clamped at the wall at one 14 | end, free at another) bending under the influence of a downward point force. 15 | We compare against analytical results using Timoshenko Beam theory. 16 | 17 | helicalbuckling : Runs the localized helical buckling benchmark which tests 18 | the mixed bending modes (two bending modes + twist mode). When an unstretchable 19 | rod is twisted at two ends, a characteristic instability arises which forms a 20 | localized helical shape at the center of the beam. There is an analytical solution 21 | (see Gazzola et. al, RSOS, 2018), which we compare against. 22 | 23 | snake : Runs an example of an snake, actuated by a continuum analytical bend- 24 | ing torque profile slithering on a plane-ground with anisotropic friction. 25 | This snake has been optimized for maximal forward speed. 26 | 27 | sphericaljoint : Runs an example of two rods connected by a spherical joint 28 | that allows for arbitrary rotations (see SphericalJoint.cpp) 29 | 30 | hingejoint : Runs an example of two rods connected by a hinge joint that 31 | allows for motion only along a constrained plane (see HingeJoint.cpp) 32 | 33 | fixedjoint : Runs an example of two rods connected by a fixed joint that 34 | does not allow any motion between the rods (see FixedJoint.cpp) 35 | 36 | pullingmuscle : Runs an example of two hanging vertical rods (connected to 37 | ground by a hinge) connected to each another's centers by a horizontal 38 | muscle. This muscle is actuated and pulls the rods together. 39 | (see PullingMuscle.cpp) 40 | 41 | elbow : Runs the injured elbow (50% Strength) with artificial muscles, 42 | as seen in Fig.7 in Supplementary Material. 43 | 44 | flagella : Runs the original biohybrid flagella design seen in Fig.2(a,b). 45 | 46 | walker : Runs the original biohybrid walker design at a frequency of 2 Hz, 47 | as seen in Fig.2(e,f). SLOW without parallelization. 48 | 49 | muscularsnake : Runs the muscular snake seen in Fig.3. SLOW without parallelization. 50 | 51 | wing : Runs the wing seen in Fig.4. EXTREMELY SLOW without parallelization. 52 | 53 | case_names 54 | ---------- 55 | timoshenkobeam, helicalbuckling, sphericaljoint, hingejoint, fixedjoint, pullingmuscle, snake 56 | flagella, muscularsnake, walker, elbow, wing 57 | 58 | All results are stored in folder 'run_' in the main directory 59 | EOF 60 | 61 | if [[ $1 =~ ^([hH][eE][lL][pP]|[hH])$ || $# -eq 0 ]]; then 62 | echo "${globalhelp}" 63 | exit 0 64 | fi 65 | 66 | rm -fr "../run_$1" 67 | mkdir -p "../run_$1" 68 | if command -v make >/dev/null 2>&1; then 69 | 70 | # check gcc version starting from 9 on to 4 71 | CC_ver_arr=($(seq 9 -1 4)) 72 | CC_ver_arr=("${CC_ver_arr[@]/#/g++-}") 73 | # Try and detect GNU g++ from the shell, if not use default CC 74 | for cc_ver in "${CC_ver_arr[@]}"; do 75 | if command -v "${cc_ver}" >/dev/null 2>&1 && "${cc_ver}" --version | grep -q '[Gg][Cc][Cc]'; then 76 | CC="${cc_ver}" 77 | break 78 | fi 79 | done 80 | # Check if not set, else set it 81 | if [ -z "${CC}" ] && g++ --version | grep -q '[Gg][Cc][Cc]'; then 82 | CC="g++" 83 | fi 84 | if [ -z "${CC}" ]; then 85 | # We have no hope, fall back on XZ's g++ 86 | CC="/usr/local/Cellar/gcc/8.2.0/bin/g++-8" 87 | fi 88 | make clean 89 | make "$1" -j16 CC="${CC}" 90 | else 91 | echo "Make is a required dependency, please install Make" 92 | fi 93 | cp ../makefiles/"$1" ../run_"$1"/executable 94 | cd ../run_"$1" || { 95 | echo "cd failed, exiting now" 96 | exit 1 97 | } 98 | 99 | if [[ "$1" == "wing" ]]; then 100 | 101 | echo '#################################' 102 | echo ' WARNING : THIS CODE IS VERY SLOW' 103 | echo '#################################' 104 | 105 | elif [[ "$1" == "elbow" ]]; then 106 | echo "test" 107 | elif [[ "$1" == "timoshenkobeam" ]]; then 108 | 109 | echo '#################################################' 110 | echo ' WARNING : This simulates 5000 physical' 111 | echo ' seconds to reach equlibrium. This large' 112 | echo ' number is necessary because the damping rate nu' 113 | echo ' is very small' 114 | echo '#################################################' 115 | fi 116 | 117 | # Finally run the code 118 | echo "" 119 | echo '################################################' 120 | echo "Running code now...results in run_$1 folder" 121 | echo '################################################' 122 | echo "" 123 | ./executable 124 | -------------------------------------------------------------------------------- /makefiles/test_make.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | cases_arr=(timoshenkobeam sphericaljoint hingejoint fixedjoint pullingmuscle snake flagella muscularsnake walker elbow wing) 3 | make clean 4 | for my_case in ${cases_arr[@]}; do make -j8 "$my_case" CC="g++-8" && make clean; done 5 | -------------------------------------------------------------------------------- /scripts/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything 2 | * 3 | 4 | # But not these files... 5 | !.gitignore 6 | !plot_results.py 7 | !requirements.txt 8 | !docs/ 9 | !README.org 10 | # etc... 11 | -------------------------------------------------------------------------------- /scripts/README.org: -------------------------------------------------------------------------------- 1 | #+TITLE: Visualizing Results 2 | #+DATE:<2019-09-13 Fri> 3 | You can use the ~plot_results.py~ script provided in the folder. Default usage 4 | is 5 | #+begin_src sh 6 | ./plot_results.py -c 7 | #+end_src 8 | where ~case_name~ is one of ~timoshenkobeam, sphericaljoint, hingejoint, fixedjoint, 9 | pullingmuscle, snake, flagella, muscularsnake, walker, elbow, wing~, the same as 10 | ~run.sh~. For more details, see the *Usage* section below. 11 | 12 | For the simple and instructive cases involving O(1) rods, we provide 3D animated 13 | plots visualizing the rod trajectory. 14 | 15 | [[file:docs/demo.gif]] 16 | 17 | For the more complicated cases provided in our Nature Communications manuscript, 18 | we display dynamically updating 2D plots that reproduce those found in our paper. 19 | 20 | * Installing prerequisites 21 | To install prerequisites for running the script, do 22 | #+begin_src sh 23 | pip install -r requirements.txt 24 | #+end_src 25 | This installs the ~numpy,scipy~ and ~matplotlib~ scientific stack on your 26 | system. 27 | 28 | * Usage 29 | The script has more arguments that may be useful. Running the script without 30 | any arguments reproduces the following message too. Note that the default 31 | behavior of the script is to only plot new data (as it updates)! 32 | #+begin_src sh 33 | usage: plot_results.py [-h] 34 | [-c {timoshenkobeam,sphericaljoint,hingejoint,fixedjoint,pullingmuscle,snake,elbow,flagella,walker,muscularsnake,wing}] 35 | [-o OUTFILE] [-p OUTPATH] [-f] [-a] [-n] 36 | 37 | Plots data associated with Elastica simulations 38 | 39 | Used to reproduce various figures and renderings (in-part) 40 | from the paper: 41 | Modeling and simulation of complex dynamic musculoskeletal architectures 42 | Nature Communications, 2019 43 | 44 | optional arguments: 45 | -h, --help show this help message and exit 46 | -c {timoshenkobeam,sphericaljoint,hingejoint,fixedjoint,pullingmuscle,snake,elbow,flagella,walker,muscularsnake,wing}, --case {timoshenkobeam,sphericaljoint,hingejoint,fixedjoint,pullingmuscle,snake,elbow,flagella,walker,muscularsnake,wing} 47 | simulation case whose output needs to be seen 48 | (default: None) 49 | -o OUTFILE, --output OUTFILE 50 | if enabled, stores the images as 51 | OUTFILE_{1,2,...,N}.png/pdf, in the 52 | scripts/pyprocessed_ directory (default: out) 53 | -p OUTPATH, --path OUTPATH 54 | path to store output files, is default created to 55 | pyprocessed_case if not initialized (default: 56 | ./pyprocessed) 57 | -f, --force force rewrite of any previously saved images (default: 58 | False) 59 | -a, --animate force collate pngs (ffmpeg) after saving them 60 | (default: False) 61 | -n, --nodisp do not render images on the screen but only save 62 | (faster!) (default: False) 63 | 64 | #+end_src 65 | 66 | * Troubleshooting 67 | - If you get the following error: 68 | #+begin_src sh 69 | File "./plot_results.py", line 760, in __init__ 70 | self.files_to_be_processed[index] = temp.pop() 71 | IndexError: pop from empty list 72 | #+end_src 73 | you should wait for some time before running the script again as this 74 | indicates that the appropriate file that the script read is either empty or 75 | has not been created yet. 76 | - If you see an empty plot, try moving it to another part of the screen. 77 | Chances are that there is another plot behind that displays information. 78 | -------------------------------------------------------------------------------- /scripts/docs/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattialab/elastica/39e8445cec6fcb54763353a86117f57a380ff174/scripts/docs/.gitignore -------------------------------------------------------------------------------- /scripts/requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | matplotlib 3 | scipy 4 | -------------------------------------------------------------------------------- /source/ArgumentParser.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MRAG_IO_ArgumentParser.h 3 | * MRAG 4 | * 5 | * This argument parser assumes that all arguments are optional ie, each of 6 | *the argument names is preceded by a '-' all arguments are however NOT optional 7 | *to avoid a mess with default values and returned values when not found! 8 | * 9 | * More converter could be required: 10 | * add as needed 11 | * TypeName as{TypeName}() in Value 12 | * 13 | * Created by Christian Conti on 6/7/10. 14 | * Copyright 2010 ETH Zurich. All rights reserved. 15 | * 16 | */ 17 | 18 | #ifndef ARGUMENTPARSER_H_ 19 | #define ARGUMENTPARSER_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | using namespace std; 29 | 30 | namespace MRAG { 31 | 32 | class Value { 33 | private: 34 | string content; 35 | 36 | public: 37 | Value() : content("") {} 38 | 39 | Value(string content_) 40 | : content(content_) { /*printf("%s\n",content.c_str());*/ 41 | } 42 | 43 | double asDouble() const { 44 | if (content == "") return 0; 45 | return (double)atof(content.c_str()); 46 | } 47 | 48 | int asInt() const { 49 | if (content == "") return 0; 50 | return atoi(content.c_str()); 51 | } 52 | 53 | bool asBool() const { 54 | if (content == "") return false; 55 | if (content == "0") return false; 56 | if (content == "false") return false; 57 | 58 | return true; 59 | } 60 | 61 | string asString() const { return content; } 62 | }; 63 | 64 | class ArgumentParser { 65 | private: 66 | map mapArguments; 67 | 68 | const int iArgC; 69 | const char **vArgV; 70 | bool bStrictMode; 71 | 72 | public: 73 | Value operator()(const string arg) { 74 | if (bStrictMode) { 75 | map::const_iterator it = mapArguments.find(arg); 76 | 77 | if (it == mapArguments.end()) { 78 | printf("Runtime option NOT SPECIFIED! ABORTING! name: %s\n", 79 | arg.data()); 80 | abort(); 81 | } 82 | } 83 | 84 | printf("%s is %s\n", arg.data(), mapArguments[arg].asString().data()); 85 | return mapArguments[arg]; 86 | } 87 | 88 | ArgumentParser(const int argc, const char **argv) 89 | : mapArguments(), iArgC(argc), vArgV(argv), bStrictMode(false) { 90 | for (int i = 1; i < argc; i++) 91 | if (argv[i][0] == '-') { 92 | string values = ""; 93 | int itemCount = 0; 94 | 95 | for (int j = i + 1; j < argc; j++) 96 | if (argv[j][0] == '-') 97 | break; 98 | else { 99 | if (strcmp(values.c_str(), "")) values += ' '; 100 | 101 | values += argv[j]; 102 | itemCount++; 103 | } 104 | 105 | if (itemCount == 0) values += '1'; 106 | mapArguments[argv[i]] = Value(values); 107 | i += itemCount; 108 | } 109 | 110 | printf("found %ld arguments of %d\n", mapArguments.size(), argc); 111 | } 112 | 113 | int getargc() const { return iArgC; } 114 | 115 | const char **getargv() const { return vArgV; } 116 | 117 | void set_strict_mode() { bStrictMode = true; } 118 | 119 | void unset_strict_mode() { bStrictMode = false; } 120 | 121 | void save_options(string path = ".") { 122 | string options; 123 | for (map::const_iterator it = mapArguments.begin(); 124 | it != mapArguments.end(); it++) { 125 | options += it->first + " " + it->second.asString() + " "; 126 | } 127 | string filepath = (path + "/" + string("argumentparser.log")); 128 | FILE *f = fopen(filepath.data(), "a"); 129 | if (f == NULL) { 130 | printf("impossible to write %s.\n", filepath.data()); 131 | return; 132 | } 133 | fprintf(f, "%s\n", options.data()); 134 | fclose(f); 135 | } 136 | }; 137 | } // namespace MRAG 138 | 139 | #endif 140 | -------------------------------------------------------------------------------- /source/ArithmeticPrecision.h: -------------------------------------------------------------------------------- 1 | #ifndef ARITHMETICPRECISION_H_ 2 | #define ARITHMETICPRECISION_H_ 3 | 4 | typedef double REAL; // This must be double, this code is not designed for 5 | // floating point accuracy! 6 | 7 | #endif 8 | -------------------------------------------------------------------------------- /source/Elbow.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MuscularSnake.h 3 | * 4 | * Created on: Jun 22, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #ifndef ELBOW_H_ 9 | #define ELBOW_H_ 10 | 11 | #include "ArgumentParser.h" 12 | #include "Polymer.h" 13 | #include "PositionVerlet2nd.h" 14 | #include "RodInitialConfigurations.h" 15 | #include "Test.h" 16 | #include "UsualHeaders.h" 17 | 18 | using namespace std; 19 | 20 | class Elbow : public Test { 21 | protected: 22 | vector amp; 23 | double w, v; 24 | int NOR; 25 | 26 | vector _elbowRun(); 27 | 28 | #ifdef SNAKE_VIZ 29 | void _paint(){}; 30 | #endif 31 | 32 | public: 33 | Elbow(const int argc, const char **argv); 34 | ~Elbow(){}; 35 | 36 | void run(); 37 | void paint(){}; 38 | }; 39 | 40 | #endif /* ELBOW_H_ */ 41 | -------------------------------------------------------------------------------- /source/FixedJoint.cpp: -------------------------------------------------------------------------------- 1 | #include "FixedJoint.h" 2 | 3 | /* 4 | This example demonstrates how we connect two Cosserat rods together 5 | using a fixed joint like so, with a force at the tip 6 | 7 | // clang-format off 8 | 9 | * Force 10 | \ 11 | \ 12 | ============================= (f) ============================= 13 | Rod1 Fixed joint Rod2 14 | 15 | ^ z 16 | | 17 | | 18 | o — — —> y 19 | / 20 | / 21 | x 22 | 23 | // clang-format on 24 | 25 | No other forces (like gravity) are considered. 26 | 27 | The fixed joint disallows for relative rotations and displacements between the 28 | rods. This means that under the influence of a time-varying force (the same one 29 | applied in the Spherical Joint and Hinge Joint cases), the fixed joint 30 | counteracts all net force and torques resulting from this activation force. As a 31 | result 32 | Rod 2 does not exhibit any motion. However, since the rods and joints are 33 | minimally damped in this case you might observe oscillations in the Rod2 34 | position—which is seen as a "wobbling" motion when visualized with our Python 35 | script. 36 | */ 37 | 38 | FixedJoint::FixedJoint(const int argc, const char **argv) 39 | : amp(0.0), w(0.0), v(0.0) {} 40 | 41 | // Units in this case are mm/g/s 42 | 43 | vector FixedJoint::_fixedjointRun() { 44 | vector rodPtrs; 45 | 46 | // Dumping frequencies (number of frames/dumps per unit time) 47 | const REAL diagPerUnitTime = 120; 48 | const REAL povrayPerUnitTime = 50; 49 | const REAL dt = 0.5e-6; 50 | const REAL timeSimulation = (10.0); 51 | 52 | //----------------------------------------------------------------------------------------------------------------- 53 | // Define Links 54 | // Driving parameters 55 | // Link One shape 56 | const int n = 10; 57 | const REAL density = 1.75e-3; // 1.75g/cm^3 58 | const REAL L0 = 200.0; 59 | const REAL r0 = 7.0; 60 | const REAL E = 3e7; 61 | const REAL totalMass = density * M_PI * r0 * r0 * L0; 62 | 63 | const REAL dL0 = L0 / (double)n; // length of cross-section element 64 | const REAL poissonRatio = 0.5; // Incompressible 65 | const REAL G = E / (poissonRatio + 1.0); 66 | 67 | // Define rod 68 | const Vector3 Linkdirection = Vector3(0.0, -1.0, 0.0); 69 | const Vector3 Linknormal = Vector3(0.0, 0.0, 1.0); 70 | const Vector3 Linkoneorigin = Vector3(0.0, L0, L0); 71 | const Vector3 Linktwoorigin = Linkoneorigin + L0 * Linkdirection; 72 | 73 | // Second moment of area for disk cross section 74 | const REAL A0 = M_PI * r0 * r0; 75 | const REAL I0_1 = A0 * A0 / (4.0 * M_PI); 76 | const REAL I0_2 = I0_1; 77 | const REAL I0_3 = 2.0 * I0_1; 78 | const Matrix3 I0 = Matrix3(I0_1, 0.0, 0.0, 0.0, I0_2, 0.0, 0.0, 0.0, I0_3); 79 | // Mass inertia matrix for disk cross section 80 | const Matrix3 J0 = density * dL0 * I0; 81 | 82 | // Bending matrix 83 | Matrix3 B0 = 84 | Matrix3(E * I0_1, 0.0, 0.0, 0.0, E * I0_2, 0.0, 0.0, 0.0, G * I0_3); 85 | // Shear matrix 86 | Matrix3 S0 = Matrix3((4.0 / 3.0) * G * A0, 0.0, 0.0, 0.0, 87 | (4.0 / 3.0) * G * A0, 0.0, 0.0, 0.0, E * A0); 88 | 89 | // Initialize straight rod and pack it into a vector of pointers to rod 90 | const REAL initialTotalTwist = 0.0; 91 | const REAL nu = 0.1; 92 | const REAL relaxationNu = 0.0; 93 | const bool useSelfContact = false; 94 | 95 | Rod *rod1 = RodInitialConfigurations::straightRod( 96 | n, totalMass, r0, J0, B0, S0, L0, initialTotalTwist, Linkoneorigin, 97 | Linkdirection, Linknormal, nu, relaxationNu, useSelfContact); 98 | rodPtrs.push_back(rod1); 99 | rod1->update(0.0); 100 | Rod *rod2 = RodInitialConfigurations::straightRod( 101 | n, totalMass, r0, J0, B0, S0, L0, initialTotalTwist, Linktwoorigin, 102 | Linkdirection, Linknormal, nu, relaxationNu, useSelfContact); 103 | rodPtrs.push_back(rod2); 104 | rod2->update(0.0); 105 | 106 | //----------------------------------------------------------------------------------------------------------------- 107 | // Pack boundary conditions 108 | vector boundaryConditionsPtrs; 109 | // Link One 110 | FixedBC fixed = FixedBC(rodPtrs[0]); 111 | boundaryConditionsPtrs.push_back(&fixed); 112 | // Link Two 113 | FreeBC freeBC = FreeBC(); 114 | boundaryConditionsPtrs.push_back(&freeBC); 115 | 116 | // Pack all forces together (no forces applied) 117 | vector externalForcesPtrs; 118 | 119 | // Gravity 120 | MultipleForces multipleForces1; 121 | GravityForce gravity = GravityForce(Vector3(0.0, 0.0, 0.0)); 122 | multipleForces1.add(&gravity); 123 | MultipleForces *multipleForcesPtr1 = multipleForces1.get(); 124 | for (unsigned int i = 0; i < 2; i++) { 125 | externalForcesPtrs.push_back(multipleForcesPtr1); 126 | } 127 | 128 | vector substrateInteractionsPtrs; 129 | 130 | // Set up External Contact -- This is for the five cases in the paper, not 131 | // used in this case 132 | vector> attachpoint; 133 | vector externalcontactPtrs; 134 | /* The second and third argument are unimportant, but 135 | are preserved here for legacy purposes. Hence we simply 136 | set it to 0.0 137 | */ 138 | ExternalContact externalcontact = 139 | ExternalContact(rodPtrs, 0.0, 0.0, attachpoint); 140 | externalcontactPtrs.push_back(&externalcontact); 141 | 142 | // Set up Simple Connection 143 | vector simpleconnectionPtrs; 144 | SimpleConnection simpleconnection = SimpleConnection(rodPtrs); 145 | simpleconnectionPtrs.push_back(&simpleconnection); 146 | //----------------------------------------------------------------------------------------------------------------- 147 | // Set up integrator (define integration order) 148 | PolymerIntegrator *integrator = new PositionVerlet2nd( 149 | rodPtrs, externalForcesPtrs, boundaryConditionsPtrs, 150 | substrateInteractionsPtrs, externalcontactPtrs, simpleconnectionPtrs); 151 | 152 | // Instantiate simulator 153 | Polymer poly = Polymer(integrator); 154 | 155 | // I am goint go collect data over this time window 156 | poly.setWindowStats(1.0, 2.0); 157 | 158 | // Run simulation 159 | string outfileName = string("prova"); 160 | const bool goodRun = poly.simulate(timeSimulation, dt, diagPerUnitTime, 161 | povrayPerUnitTime, outfileName); 162 | 163 | // Throw exception if something went wrong 164 | if (!goodRun) 165 | throw "not good run in localized helical buckling, what is going on?"; 166 | 167 | const vector avgVel = poly.getAverageVelocity(); 168 | 169 | vector fwdAvgVel; 170 | for (unsigned int i = 0; i < 1; i++) { 171 | fwdAvgVel.push_back(avgVel[i] % Linkdirection); 172 | } 173 | const vector fitness = fwdAvgVel; 174 | 175 | return (fitness); 176 | } 177 | 178 | void FixedJoint::run() { 179 | const vector fitness = _fixedjointRun(); 180 | exit(0); 181 | } 182 | -------------------------------------------------------------------------------- /source/FixedJoint.h: -------------------------------------------------------------------------------- 1 | /* 2 | * FixedJoint.h 3 | * 4 | */ 5 | 6 | #ifndef FIXEDJOINT_H_ 7 | #define FIXEDJOINT_H_ 8 | 9 | #include "ArgumentParser.h" 10 | #include "Polymer.h" 11 | #include "PositionVerlet2nd.h" 12 | #include "RodInitialConfigurations.h" 13 | #include "Test.h" 14 | #include "UsualHeaders.h" 15 | 16 | using namespace std; 17 | 18 | class FixedJoint : public Test { 19 | protected: 20 | vector amp; 21 | double w, v; 22 | int NOR; 23 | 24 | vector _fixedjointRun(); 25 | 26 | #ifdef SNAKE_VIZ 27 | void _paint(){}; 28 | #endif 29 | 30 | public: 31 | FixedJoint(const int argc, const char **argv); 32 | ~FixedJoint(){}; 33 | 34 | void run(); 35 | void paint(){}; 36 | }; 37 | 38 | #endif /* FIXEDJOINT_H_ */ 39 | -------------------------------------------------------------------------------- /source/Flagella.cpp: -------------------------------------------------------------------------------- 1 | #include "Flagella.h" 2 | 3 | Flagella::Flagella(const int argc, const char **argv) 4 | : rsmall(0.0), rlarge(0.0), head(0.0), cell1(0.0), cell2(0.0) {} 5 | 6 | // Units in this case are mm/g/s 7 | 8 | REAL Flagella::_flagellaRun() { 9 | vector rodPtrs; 10 | 11 | // Dumping frequencies (number of frames/dumps per unit time) 12 | const REAL diagPerUnitTime = 120; 13 | const REAL povrayPerUnitTime = 50; 14 | 15 | REAL dt = 0.5e-7; // ms 16 | const REAL timeSimulation = (6.5); // ms 17 | 18 | //----------------------------------------------------------------------------------------------------------------- 19 | // Define PDMS Body 20 | 21 | // Driving parameters 22 | const int n = 18; 23 | const REAL density = 0.965e-3; 24 | // const REAL density = 0.965*1e-3; //(g/mm^-3) 25 | const REAL L0 = 1.927; //(mm) 26 | // const REAL totalMass = density*M_PI*r0*r0*L0; //(g) 27 | const REAL E = 3.86e6; //(g*mm^-1*ms^-2) 28 | const REAL poissonRatio = 0.5; // Incompressible 29 | const REAL G = E / (poissonRatio + 1.0); 30 | 31 | // fluid property 32 | const REAL T = 1.0 / 1.8; //(s) 33 | const REAL fluidDensity = 1.15e-3; //(g/mm^3) 34 | const REAL RE = 1.8e-2; 35 | const REAL dynamicViscosity = 1.2e-3; 36 | 37 | const REAL w = 2.0 * M_PI / T; 38 | 39 | const REAL dL0 = L0 / (double)n; // length of cross-section element 40 | 41 | // Define rod 42 | const Vector3 spermdirection = Vector3(1.0, 0.0, 0.0); 43 | const Vector3 spermnormal = Vector3(0.0, 0.0, 1.0); 44 | const Vector3 spermorigin = Vector3(0.0, 0.0, 0.1); 45 | 46 | // vector values 47 | vector r0_v2 = vector(n); //(mm) 48 | vector J0_v2 = vector(n); 49 | vector B0_v2 = vector(n - 1); 50 | vector S0_v2 = vector(n); 51 | 52 | // The actual radius for the head and tail 53 | rsmall = 0.007; //(mm) 54 | rlarge = 0.02; //(mm) 55 | 56 | // This is the virtual radius for matching the bending stiffness 57 | // of the tail. 58 | // See 59 | // Neuromuscular actuation of biohybrid motile bots. 60 | // by Aydin, O., Zhang, X., Nuethong, S., Pagan-Diaz, G. J., et al. 61 | // PNAS (2019).for more details. 62 | REAL requal = 0.0053; 63 | REAL Asmall = M_PI * requal * requal; 64 | REAL Alarge = M_PI * rlarge * rlarge; 65 | 66 | // Second moment of area for disk cross section 67 | REAL I0_1_s = Asmall * Asmall / (4.0 * M_PI); 68 | REAL I0_2_s = I0_1_s; 69 | REAL I0_3_s = 2.0 * I0_1_s; 70 | Matrix3 I0_s = Matrix3(I0_1_s, 0.0, 0.0, 0.0, I0_2_s, 0.0, 0.0, 0.0, I0_3_s); 71 | // Mass inertia matrix for disk cross section 72 | Matrix3 J0_s = density * dL0 * I0_s; 73 | 74 | // Bending matrix 75 | Matrix3 B0_s = 76 | Matrix3(E * I0_1_s, 0.0, 0.0, 0.0, E * I0_2_s, 0.0, 0.0, 0.0, G * I0_3_s); 77 | // Shear matrix 78 | Matrix3 S0_s = Matrix3((4.0 / 3.0) * G * Asmall, 0.0, 0.0, 0.0, 79 | (4.0 / 3.0) * G * Asmall, 0.0, 0.0, 0.0, E * Asmall); 80 | 81 | REAL I0_1_l = Alarge * Alarge / (4.0 * M_PI); 82 | REAL I0_2_l = I0_1_l; 83 | REAL I0_3_l = 2.0 * I0_1_l; 84 | Matrix3 I0_l = Matrix3(I0_1_l, 0.0, 0.0, 0.0, I0_2_l, 0.0, 0.0, 0.0, I0_3_l); 85 | // Mass inertia matrix for disk cross section 86 | Matrix3 J0_l = density * dL0 * I0_l; 87 | 88 | // Bending matrix 89 | Matrix3 B0_l = 90 | Matrix3(E * I0_1_l, 0.0, 0.0, 0.0, E * I0_2_l, 0.0, 0.0, 0.0, G * I0_3_l); 91 | // Shear matrix 92 | Matrix3 S0_l = Matrix3((4.0 / 3.0) * G * Alarge, 0.0, 0.0, 0.0, 93 | (4.0 / 3.0) * G * Alarge, 0.0, 0.0, 0.0, E * Alarge); 94 | 95 | int headindex = 4; 96 | // Set properties for the head section 97 | if (headindex != 0) { 98 | for (unsigned int i = 0; i < headindex; i++) { 99 | r0_v2[i] = rlarge; 100 | J0_v2[i] = J0_l; 101 | B0_v2[i] = B0_l; 102 | S0_v2[i] = S0_l; 103 | } 104 | } 105 | // Set properties for the tail section 106 | for (unsigned int i = headindex; i < n; i++) { 107 | r0_v2[i] = rsmall; 108 | J0_v2[i] = J0_s; 109 | if (i < (n - 1)) { 110 | B0_v2[i] = B0_s; 111 | } 112 | S0_v2[i] = S0_s; 113 | } 114 | 115 | // Initialize straight rod and pack it into a vector of pointers to rod 116 | const REAL initialTotalTwist = 0.0; 117 | const REAL nu = 0.0; 118 | const REAL relaxationNu = 0.0; 119 | const bool useSelfContact = false; 120 | 121 | Rod *rod1 = RodInitialConfigurations::straightRod_v( 122 | n, density, r0_v2, J0_v2, B0_v2, S0_v2, L0, initialTotalTwist, 123 | spermorigin, spermdirection, spermnormal, nu, relaxationNu, 124 | useSelfContact); 125 | rodPtrs.push_back(rod1); 126 | rod1->update(0.0); 127 | rod1->computeEnergies(); 128 | 129 | // This enables the computation of hydrodynamic forces on the 130 | // rod using Slender Body Theory (SBT). This vector of Rod* is 131 | // later on passed into the SlenderBodyTheoryEnvironment Class 132 | vector spermPtr; 133 | spermPtr.push_back(rodPtrs[0]); 134 | 135 | //----------------------------------------------------------------------------------------------------------------- 136 | // Define Cell 137 | 138 | // number of cells 139 | NOR = 1; 140 | vector> attachpoint; 141 | 142 | // Driving parameters 143 | const int nm = 2; 144 | const REAL densitym = 2.6e-4; 145 | const REAL Lm = 0.107056; 146 | const REAL rm = 0.01; 147 | const REAL totalMassm = densitym * M_PI * rm * rm * Lm; 148 | const REAL Em = 0.3e5; // 30Kpa 149 | 150 | const REAL dLm = Lm / (double)nm; // length of cross-section element 151 | const REAL Am = M_PI * rm * rm; 152 | const REAL poissonRatiom = 0.5; // Incompressible 153 | const REAL Gm = Em / (poissonRatiom + 1.0); 154 | 155 | vector directionmuscle; 156 | vector normalmuscle; 157 | vector originmuscle; 158 | 159 | pair attach; 160 | 161 | directionmuscle.push_back(Vector3(1.0, 0.0, 0.0)); 162 | normalmuscle.push_back(Vector3(0.0, 0.0, 1.0)); 163 | originmuscle.push_back(Vector3(4.5 * Lm, -0.0053, 0.1)); 164 | 165 | // The attaching index and direction for the cell 166 | // The first element is the index on the body 167 | // The second index represents the 'side' at which 168 | // the cell is attached to the body 169 | attach.first = 4; 170 | attach.second = -1; 171 | attachpoint.push_back(attach); 172 | 173 | // Second moment of area for disk cross section 174 | const REAL I0_1m = Am * Am / (4.0 * M_PI); 175 | const REAL I0_2m = I0_1m; 176 | const REAL I0_3m = 2.0 * I0_1m; 177 | const Matrix3 I0m = 178 | Matrix3(I0_1m, 0.0, 0.0, 0.0, I0_2m, 0.0, 0.0, 0.0, I0_3m); 179 | 180 | // Mass inertia matrix for disk cross section 181 | const Matrix3 Jm = densitym * dLm * I0m; 182 | 183 | // Bending matrix 184 | Matrix3 Bm = 185 | Matrix3(Em * I0_1m, 0.0, 0.0, 0.0, Em * I0_2m, 0.0, 0.0, 0.0, Gm * I0_3m); 186 | 187 | // Shear matrix 188 | Matrix3 Sm = Matrix3((4.0 / 3.0) * Gm * Am, 0.0, 0.0, 0.0, 189 | (4.0 / 3.0) * Gm * Am, 0.0, 0.0, 0.0, Em * Am); 190 | 191 | // Initialize straight rod and pack it into a vector of pointers to rod 192 | const REAL initialTotalTwistm = 0.0; 193 | const REAL num = 1e-6; 194 | const REAL relaxationNum = 0.0; 195 | const bool useSelfContactm = false; 196 | 197 | // Just one rod 198 | for (unsigned int i = 0; i < NOR; i++) { 199 | Rod *rod = RodInitialConfigurations::straightRod( 200 | nm, totalMassm, rm, Jm, Bm, Sm, Lm, initialTotalTwistm, originmuscle[i], 201 | directionmuscle[i], normalmuscle[i], num, relaxationNum, 202 | useSelfContactm); 203 | rodPtrs.push_back(rod); 204 | rod->update(0.0); 205 | } 206 | 207 | //----------------------------------------------------------------------------------------------------------------- 208 | // Pack boundary conditions 209 | FreeBC freeBC = FreeBC(); 210 | vector boundaryConditionsPtrs; 211 | // Both body rod and cell rod are free 212 | for (unsigned int i = 0; i < 2; i++) { 213 | boundaryConditionsPtrs.push_back(&freeBC); 214 | } 215 | 216 | // Pack all forces together 217 | vector externalForcesPtrs; 218 | 219 | // No force and torques on the body, all actuation comes from the cell 220 | // Hence all these coefficients are set to 0, and are required only for 221 | // legacy purposes 222 | int cellindex1 = 0; 223 | cellindex1 = cellindex1 + headindex; 224 | LocalTorque local1 = LocalTorque(Vector3(0.0, 0.0, 0.0e-3), w, 0); 225 | LocalTorque local2 = LocalTorque(Vector3(0.0, 0.0, 0.0e-3), w, 0); 226 | 227 | MultipleForces multipleForces1; 228 | multipleForces1.add(&local1); 229 | multipleForces1.add(&local2); 230 | 231 | MultipleForces *multipleForcesPtr1 = multipleForces1.get(); 232 | externalForcesPtrs.push_back(multipleForcesPtr1); 233 | 234 | // muscular activity of cell 235 | // define contracting groups 236 | // This represents a 12 microN force amplitude beating at w=3.6Hz 237 | LocalForce muscle0 = LocalForce(12, w); 238 | MultipleForces multipleForces2; 239 | multipleForces2.add(&muscle0); 240 | 241 | MultipleForces *multipleForcesPtr2 = multipleForces2.get(); 242 | externalForcesPtrs.push_back(multipleForcesPtr2); 243 | 244 | // Set up substrate properties and slenderbodytheory environment 245 | SlenderBodyTheoryEnvironment sbt = 246 | SlenderBodyTheoryEnvironment(spermPtr, dynamicViscosity); 247 | vector substrateInteractionsPtrs; 248 | substrateInteractionsPtrs.push_back(&sbt); 249 | 250 | // Set up External Contact 251 | vector externalcontactPtrs; 252 | /* The second and third argument are unimportant, but 253 | are preserved here for legacy purposes. Hence we simply 254 | set it to 0.0 255 | */ 256 | /* This function takes care of assembly of body and cell 257 | */ 258 | ExternalContact externalcontact = 259 | ExternalContact(rodPtrs, 0.0, 0.0, attachpoint); 260 | externalcontactPtrs.push_back(&externalcontact); 261 | 262 | // Set up Simple Connection -- Not used in this case, only in simple toy 263 | // examples 264 | vector simpleconnectionPtrs; 265 | SimpleConnection simpleconnection = SimpleConnection(rodPtrs); 266 | simpleconnectionPtrs.push_back(&simpleconnection); 267 | //----------------------------------------------------------------------------------------------------------------- 268 | // Set up integrator (define integration order) 269 | PolymerIntegrator *integrator = new PositionVerlet2nd( 270 | rodPtrs, externalForcesPtrs, boundaryConditionsPtrs, 271 | substrateInteractionsPtrs, externalcontactPtrs, simpleconnectionPtrs); 272 | 273 | // Instantiate simulator 274 | Polymer poly = Polymer(integrator); 275 | 276 | // This call is done for legacy purposes (for data collection). In this 277 | // case we don't collect any data, but it is dumped in the flagella.txt 278 | // file below. 279 | poly.setWindowStats(0.5, 1.5); 280 | 281 | // Run simulation 282 | string outfileName = string("prova"); 283 | const bool goodRun = poly.simulate(timeSimulation, dt, diagPerUnitTime, 284 | povrayPerUnitTime, outfileName); 285 | 286 | // Throw exception if something went wrong 287 | if (!goodRun) 288 | throw "not good run in localized helical buckling, what is going on?"; 289 | 290 | const vector avgVel = poly.getAverageVelocity(); 291 | 292 | vector fwdAvgVel; 293 | for (unsigned int i = 0; i < NOR; i++) { 294 | fwdAvgVel.push_back(avgVel[i] % spermdirection); 295 | } 296 | const REAL fitness = fwdAvgVel[0]; 297 | 298 | return (fitness); 299 | } 300 | 301 | void Flagella::run() { 302 | const REAL fitness = _flagellaRun(); 303 | exit(0); 304 | } 305 | -------------------------------------------------------------------------------- /source/Flagella.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Flagella.h 3 | * 4 | * Created on: Jun 22, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #ifndef FLAGELLA_H_ 9 | #define FLAGELLA_H_ 10 | 11 | #include "ArgumentParser.h" 12 | #include "Polymer.h" 13 | #include "PositionVerlet2nd.h" 14 | #include "RodInitialConfigurations.h" 15 | #include "Test.h" 16 | #include "UsualHeaders.h" 17 | 18 | using namespace std; 19 | 20 | class Flagella : public Test { 21 | protected: 22 | vector amp; 23 | double rsmall, rlarge; 24 | double head; 25 | double cell1; 26 | double cell2; 27 | double ncycles; 28 | int NOR; 29 | 30 | REAL _flagellaRun(); 31 | 32 | #ifdef SNAKE_VIZ 33 | void _paint(){}; 34 | #endif 35 | 36 | public: 37 | Flagella(const int argc, const char **argv); 38 | ~Flagella(){}; 39 | 40 | void run(); 41 | void paint(){}; 42 | }; 43 | 44 | #endif /* FLAGELLA_H_ */ 45 | -------------------------------------------------------------------------------- /source/GeometryFunctions.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GeometryFunctions.cpp 3 | * 4 | * Created on: Jun 20, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #include "GeometryFunctions.h" 9 | 10 | // Given a set of points x[3], it constructs a set of frames 11 | // that are aligned with the tangent of the curve expressed by x[3] 12 | std::vector alignFrames(const std::vector &x, 13 | const Vector3 &u0) { 14 | // Check that u0 is indeed a unit vector 15 | assert(fabs(u0 % u0 - 1.0) < Tolerance::tol()); 16 | 17 | // Compute tangent vectors 18 | std::vector t = vUnitize(vDiff(x)); 19 | 20 | // Check that u0 is orthogonal to tangent 21 | assert(fabs(t[0] % u0) < 1e5 * Tolerance::tol()); 22 | 23 | // Create first body frame 24 | std::vector Q; 25 | Q.push_back(Matrix3(u0, t[0] * u0, t[0])); 26 | 27 | // Index i starts from 1, because the first frame has been filled already 28 | for (unsigned int i = 1; i < t.size(); i++) { 29 | // Compute angle of rotation: in this case, this is a counterclockwise 30 | // rotation 31 | const REAL angleRotation = angle(t[i - 1], t[i]); 32 | 33 | // Use cross product to compute axis of rotation in the lab frame of 34 | // reference (LabFOR) 35 | const Vector3 axisRotationInLabFOR = (t[i - 1] * t[i]).unitize(); 36 | 37 | // Express the axis of rotation in LabFOR in the material frame of 38 | // reference: Formula: k_material = Q * (k_lab - x_material), where k_lab is 39 | // the vector to be transformed, x_material is its position in the material 40 | // frame of reference 41 | const Vector3 axisRotationInMaterialFOR = 42 | (Q[i - 1] * axisRotationInLabFOR).unitize(); 43 | 44 | // Apply rotation using Rodrigues' rotation formula: Q_rotated = exp(angle * 45 | // k) * Q. This formula is based on the definition of skew map S. In general 46 | // it applies a CLOCKWISE rotation, but here Andrew for no reason decided to 47 | // implement S_andrew = -S_rest_of_the_world and so in this code the formula 48 | // applies a COUNTERCLOCKWISE rotation by default, hence no minus in front 49 | // of 'angleRotation' below 50 | const Matrix3 rotatedQ = 51 | exp(angleRotation * axisRotationInMaterialFOR) * Q[i - 1]; 52 | 53 | // Store frame 54 | Q.push_back(rotatedQ); 55 | } 56 | 57 | return Q; 58 | } 59 | 60 | // Apply clockwise twist about the director d3 (the one paired to the tangent 61 | // vector of the rod) 62 | void applyTwists(std::vector &Q, const std::vector &twists) { 63 | // Check consistency in size 64 | assert(Q.size() == twists.size()); 65 | 66 | // Because Andrew had the brilliant idea to set S_andrew=-S_rest_of_the_world, 67 | // there is a minus in front of the axis of rotation 68 | for (unsigned int i = 0; i < Q.size(); i++) { 69 | const Vector3 axisRotationInMaterialFOR = Vector3(0, 0, twists[i]); 70 | Q[i] = exp(-axisRotationInMaterialFOR) * Q[i]; 71 | } 72 | } 73 | 74 | // finds the minimum distance vector between two line segments 75 | // with positions x_i and extent e_i, returning the point on the 76 | // first segment plus the distance vector 77 | std::vector findMinDistVectors(const Vector3 &x1, const Vector3 &e1, 78 | const Vector3 &x2, const Vector3 &e2, 79 | const REAL tol) { 80 | // we assume all e1 and e2 are non-0, problems occur if not 81 | // if they are within tol of parallel, we use another formula 82 | // note of course = for vectors so 83 | // we only compute one of them and use it for both 84 | const REAL e1e1 = e1 % e1; 85 | const REAL e1e2 = e1 % e2; 86 | const REAL e1x1 = e1 % x1; 87 | const REAL e1x2 = e1 % x2; 88 | const REAL e2e2 = e2 % e2; 89 | const REAL e2x1 = e2 % x1; 90 | const REAL e2x2 = e2 % x2; 91 | REAL s = 0.0; 92 | REAL t = 0.0; 93 | 94 | // check if lines are nearly parallel; if so use diff formula 95 | // than the skew case. 96 | if (fabs((1 - e1e2 * e1e2 / (e1e1 * e2e2))) < tol) { 97 | t = (e1x2 - e1x1) / e1e1; 98 | if (t < 0) t = 0; 99 | if (t > 1) t = 1; 100 | s = (e2 % (x1 + t * e1 - x2)) / e2e2; 101 | if (s < 0) s = 0; 102 | if (s > 1) s = 1; 103 | } else { 104 | // now for the skew case 105 | s = (e1e1 * e2x1 + e1e2 * e1x2 - e1e2 * e1x1 - e1e1 * e2x2) / 106 | (e1e1 * e2e2 - e1e2 * e1e2); 107 | t = (e1e2 * s + e1x2 - e1x1) / e1e1; 108 | 109 | // if CP given by s and t above is in [0,1]x[0,1] 110 | // then return it unchanged 111 | // as it is the minimum. Otherwise the minimum will occur on the border 112 | // of the unit square. 113 | if (s < 0 || s > 1 || t < 0 || t > 1) { 114 | REAL sTemp, tTemp, dTemp; 115 | REAL curMinDist = 1e20; 116 | 117 | // now check each edge of the square 118 | // s = 0 119 | tTemp = (e1x2 - e1x1) / e1e1; 120 | 121 | if (tTemp < 0) tTemp = 0; 122 | 123 | if (tTemp > 1) tTemp = 1; 124 | 125 | s = 0; 126 | t = tTemp; 127 | curMinDist = (x1 + e1 * tTemp - x2).length(); 128 | 129 | // s = 1 130 | tTemp = (e1x2 + e1e2 - e1x1) / e1e1; 131 | 132 | if (tTemp < 0) tTemp = 0; 133 | 134 | if (tTemp > 1) tTemp = 1; 135 | 136 | dTemp = (x1 + e1 * tTemp - x2 - e1 * 1).length(); 137 | 138 | if (dTemp < curMinDist) { 139 | curMinDist = dTemp; 140 | s = 1; 141 | t = tTemp; 142 | } 143 | 144 | // t = 0 145 | sTemp = (e2x1 - e2x2) / e2e2; 146 | 147 | if (sTemp < 0) sTemp = 0; 148 | 149 | if (sTemp > 1) sTemp = 1; 150 | 151 | dTemp = (x2 + e2 * sTemp - x1).length(); 152 | 153 | if (dTemp < curMinDist) { 154 | curMinDist = dTemp; 155 | s = sTemp; 156 | t = 0; 157 | } 158 | 159 | // t = 1 160 | sTemp = (e2x1 + e1e2 - e2x2) / e2e2; 161 | 162 | if (sTemp < 0) sTemp = 0; 163 | 164 | if (sTemp > 1) sTemp = 1; 165 | 166 | dTemp = (x2 + e2 * sTemp - x1 - e1).length(); 167 | 168 | if (dTemp < curMinDist) { 169 | curMinDist = dTemp; 170 | s = sTemp; 171 | t = 1; 172 | } 173 | } 174 | } 175 | // now construct the minimal distance vector using s and t 176 | std::vector result; 177 | result.push_back(x1 + t * e1); 178 | result.push_back(-x1 - t * e1 + x2 + s * e2); 179 | return result; 180 | } 181 | -------------------------------------------------------------------------------- /source/GeometryFunctions.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRYFUNCTIONS_H_ 2 | #define GEOMETRYFUNCTIONS_H_ 3 | 4 | #include 5 | #include "Matrix3.h" 6 | #include "Rod.h" 7 | #include "Tolerance.h" 8 | #include "UsualHeaders.h" 9 | #include "Vector3.h" 10 | #include "VectorFunctions.h" 11 | 12 | std::vector alignFrames(const std::vector &x, 13 | const Vector3 &u0); 14 | 15 | void applyTwists(std::vector &Q, const std::vector &twists); 16 | 17 | std::vector findMinDistVectors(const Vector3 &x1, const Vector3 &e1, 18 | const Vector3 &x2, const Vector3 &e2, 19 | const REAL tol = 1e-6); 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /source/HingeJoint.cpp: -------------------------------------------------------------------------------- 1 | #include "HingeJoint.h" 2 | 3 | /* 4 | 5 | This example demonstrates how we connect two Cosserat rods together using a 6 | hinge joint like so, with a force at the tip 7 | 8 | // clang-format off 9 | 10 | * Force 11 | \ 12 | \ 13 | ============================= (x) ============================= 14 | Rod1 Hinge joint Rod 2 15 | 16 | ^ z 17 | | 18 | | 19 | o — — —> y 20 | / 21 | / 22 | x 23 | 24 | // clang-format on 25 | 26 | No other forces (like gravity) are considered. 27 | 28 | The hinge joint allows for relative rod rotations about only one plane 29 | common to both the rods (in this case y-z plane), but prevents relative 30 | displacements. That means Rod2 in this case always lies on the y-z plane, still 31 | connected to Rod1. Initially the rods are aligned along the y-axis, and the same 32 | force as the Spherical Joint case with a time-varying direction (but fixed 33 | magnitude) acts along the x-z plane. The torque due to this force has components 34 | in all directions, but the hinge joint counteracts all torques in the 35 | x-direction (and all forces of course). Intuitively then, you would expect the 36 | rod to move up and down in the constrained y-z plane (due to this sinusoidal 37 | time-varying force), but this is NOT what should actually happen. In the plot 38 | provided by our python script, please pay attention to the applied force, its 39 | changes in direction and associated torques generated by the rod. From this 40 | visualization, it becomes apparent that the rod undergoes a complicated motion, 41 | eventually oscillating about an upright position (i.e. oriented along the z 42 | axis). 43 | 44 | */ 45 | 46 | HingeJoint::HingeJoint(const int argc, const char **argv) 47 | : amp(0.0), w(0.0), v(0.0) {} 48 | 49 | // Units in this case are mm/g/s 50 | 51 | vector HingeJoint::_hingejointRun() { 52 | vector rodPtrs; 53 | 54 | // Dumping frequencies (number of frames/dumps per unit time) 55 | const REAL diagPerUnitTime = 120; 56 | const REAL povrayPerUnitTime = 50; 57 | const REAL dt = 0.5e-6; 58 | const REAL timeSimulation = (10.0); 59 | 60 | //----------------------------------------------------------------------------------------------------------------- 61 | // Define Links 62 | // Driving parameters 63 | // Link One shape 64 | const int n = 10; 65 | const REAL density = 1.75e-3; // 1.75g/cm^3 66 | const REAL L0 = 200.0; 67 | const REAL r0 = 7.0; 68 | const REAL E = 3e7; 69 | const REAL totalMass = density * M_PI * r0 * r0 * L0; 70 | 71 | const REAL dL0 = L0 / (double)n; // length of cross-section element 72 | const REAL poissonRatio = 0.5; // Incompressible 73 | const REAL G = E / (poissonRatio + 1.0); 74 | 75 | // Define rod 76 | const Vector3 Linkdirection = Vector3(0.0, -1.0, 0.0); 77 | const Vector3 Linknormal = Vector3(0.0, 0.0, 1.0); 78 | const Vector3 Linkoneorigin = Vector3(0.0, L0, L0); 79 | const Vector3 Linktwoorigin = Linkoneorigin + L0 * Linkdirection; 80 | 81 | // Second moment of area for disk cross section 82 | const REAL A0 = M_PI * r0 * r0; 83 | const REAL I0_1 = A0 * A0 / (4.0 * M_PI); 84 | const REAL I0_2 = I0_1; 85 | const REAL I0_3 = 2.0 * I0_1; 86 | const Matrix3 I0 = Matrix3(I0_1, 0.0, 0.0, 0.0, I0_2, 0.0, 0.0, 0.0, I0_3); 87 | // Mass inertia matrix for disk cross section 88 | const Matrix3 J0 = density * dL0 * I0; 89 | 90 | // Bending matrix 91 | Matrix3 B0 = 92 | Matrix3(E * I0_1, 0.0, 0.0, 0.0, E * I0_2, 0.0, 0.0, 0.0, G * I0_3); 93 | // Shear matrix 94 | Matrix3 S0 = Matrix3((4.0 / 3.0) * G * A0, 0.0, 0.0, 0.0, 95 | (4.0 / 3.0) * G * A0, 0.0, 0.0, 0.0, E * A0); 96 | 97 | // Initialize straight rod and pack it into a vector of pointers to rod 98 | const REAL initialTotalTwist = 0.0; 99 | const REAL nu = 0.1; 100 | const REAL relaxationNu = 0.0; 101 | const bool useSelfContact = false; 102 | 103 | Rod *rod1 = RodInitialConfigurations::straightRod( 104 | n, totalMass, r0, J0, B0, S0, L0, initialTotalTwist, Linkoneorigin, 105 | Linkdirection, Linknormal, nu, relaxationNu, useSelfContact); 106 | rodPtrs.push_back(rod1); 107 | rod1->update(0.0); 108 | Rod *rod2 = RodInitialConfigurations::straightRod( 109 | n, totalMass, r0, J0, B0, S0, L0, initialTotalTwist, Linktwoorigin, 110 | Linkdirection, Linknormal, nu, relaxationNu, useSelfContact); 111 | rodPtrs.push_back(rod2); 112 | rod2->update(0.0); 113 | 114 | //----------------------------------------------------------------------------------------------------------------- 115 | // Pack boundary conditions 116 | vector boundaryConditionsPtrs; 117 | // Link One 118 | FixedBC fixed = FixedBC(rodPtrs[0]); 119 | boundaryConditionsPtrs.push_back(&fixed); 120 | // Link Two 121 | FreeBC freeBC = FreeBC(); 122 | boundaryConditionsPtrs.push_back(&freeBC); 123 | 124 | // Pack all forces together (no forces applied) 125 | vector externalForcesPtrs; 126 | 127 | // Gravity 128 | MultipleForces multipleForces1; 129 | GravityForce gravity = GravityForce(Vector3(0.0, 0.0, 0.0)); 130 | multipleForces1.add(&gravity); 131 | MultipleForces *multipleForcesPtr1 = multipleForces1.get(); 132 | for (unsigned int i = 0; i < 2; i++) { 133 | externalForcesPtrs.push_back(multipleForcesPtr1); 134 | } 135 | 136 | vector substrateInteractionsPtrs; 137 | 138 | // Set up External Contact -- This is for the five cases in the paper, not 139 | // used in this case 140 | vector> attachpoint; 141 | vector externalcontactPtrs; 142 | /* The second and third argument are unimportant, but 143 | are preserved here for legacy purposes. Hence we simply 144 | set it to 0.0 145 | */ 146 | ExternalContact externalcontact = 147 | ExternalContact(rodPtrs, 0.0, 0.0, attachpoint); 148 | externalcontactPtrs.push_back(&externalcontact); 149 | 150 | // Set up Simple Connection 151 | vector simpleconnectionPtrs; 152 | SimpleConnection simpleconnection = SimpleConnection(rodPtrs); 153 | simpleconnectionPtrs.push_back(&simpleconnection); 154 | //----------------------------------------------------------------------------------------------------------------- 155 | // Set up integrator (define integration order) 156 | PolymerIntegrator *integrator = new PositionVerlet2nd( 157 | rodPtrs, externalForcesPtrs, boundaryConditionsPtrs, 158 | substrateInteractionsPtrs, externalcontactPtrs, simpleconnectionPtrs); 159 | 160 | // Instantiate simulator 161 | Polymer poly = Polymer(integrator); 162 | 163 | // I am goint go collect data over this time window 164 | poly.setWindowStats(1.0, 2.0); 165 | 166 | // Run simulation 167 | string outfileName = string("prova"); 168 | const bool goodRun = poly.simulate(timeSimulation, dt, diagPerUnitTime, 169 | povrayPerUnitTime, outfileName); 170 | 171 | // Throw exception if something went wrong 172 | if (!goodRun) 173 | throw "not good run in localized helical buckling, what is going on?"; 174 | 175 | const vector avgVel = poly.getAverageVelocity(); 176 | 177 | vector fwdAvgVel; 178 | for (unsigned int i = 0; i < 1; i++) { 179 | fwdAvgVel.push_back(avgVel[i] % Linkdirection); 180 | } 181 | const vector fitness = fwdAvgVel; 182 | 183 | return (fitness); 184 | } 185 | 186 | void HingeJoint::run() { 187 | const vector fitness = _hingejointRun(); 188 | exit(0); 189 | } 190 | -------------------------------------------------------------------------------- /source/HingeJoint.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SphericalJoint.h 3 | * 4 | */ 5 | 6 | #ifndef HINGEJOINT_H_ 7 | #define HINGEJOINT_H_ 8 | 9 | #include "ArgumentParser.h" 10 | #include "Polymer.h" 11 | #include "PositionVerlet2nd.h" 12 | #include "RodInitialConfigurations.h" 13 | #include "Test.h" 14 | #include "UsualHeaders.h" 15 | 16 | using namespace std; 17 | 18 | class HingeJoint : public Test { 19 | protected: 20 | vector amp; 21 | double w, v; 22 | int NOR; 23 | 24 | vector _hingejointRun(); 25 | 26 | #ifdef SNAKE_VIZ 27 | void _paint(){}; 28 | #endif 29 | 30 | public: 31 | HingeJoint(const int argc, const char **argv); 32 | ~HingeJoint(){}; 33 | 34 | void run(); 35 | void paint(){}; 36 | }; 37 | 38 | #endif /* HINGEJOINT_H_ */ 39 | -------------------------------------------------------------------------------- /source/InstabilityHelical.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ValidationLocalizedHelicalBuckling_SpaceConvergence.cpp 3 | * 4 | * Created on: Aug 13, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #include "InstabilityHelical.h" 9 | 10 | InstabilityHelical::InstabilityHelical(const int argc, const char** argv) {} 11 | 12 | InstabilityHelical::~InstabilityHelical() {} 13 | 14 | bool InstabilityHelical::_testLocalizedHelicalBuckling(const int nEdges, 15 | const REAL _timeTwist, 16 | const REAL _timeRelax, 17 | const REAL _nu, 18 | string outfileName) { 19 | // Discretization parameters 20 | const int n = nEdges; // number of discretization edges (i.e. n+1 points) 21 | // along the entire rod 22 | const REAL timeTwist = _timeTwist; // twisting time 23 | const REAL timeRelax = _timeRelax; // relaxing time 24 | const REAL timeSimulation = timeTwist + timeRelax; // total simulation time 25 | 26 | // Dumping frequencies (number of frames/dumps per unit time) 27 | const unsigned int diagPerUnitTime = 1; 28 | const REAL povrayPerUnitTime = 1. / 200.; 29 | 30 | // Physical parameters 31 | const REAL massPerUnitLength = 1.0; // mass per unit length 32 | const REAL L0 = 100.0; // total length of rod 33 | const REAL alpha = 34 | 1.345; // flexural rigidity about d1 and d2 (often referred to as B - 35 | // note that here we use B for the bending matrix instead) 36 | const REAL beta = 37 | 0.789; // torsional rigidity about d3 (often referred to as C) 38 | const REAL R = 27.0; // Number of turns applied at the first extrema (see 39 | // "Writhing instabilities of twisted rods: from 40 | // infinite to finite length", 2001) 41 | const REAL D = 3.0; // Imposed displacement or slack at the first extrema 42 | // (see above reference) 43 | const REAL nu = _nu; // Numerical dumping viscosity 44 | const REAL nuRelaxation = 0.0; 45 | const REAL dL0 = L0 / (double)n; // length of cross-section element 46 | const REAL dt = 0.001 * dL0; // time step 47 | const REAL totalMass = massPerUnitLength * L0; // total mass of the rod 48 | const REAL r0 = 0.35; // 5*dL0; 49 | const REAL A0 = M_PI * r0 * r0; 50 | const REAL density = totalMass / (L0 * A0); 51 | const REAL totalInitialTwist = 0.0; 52 | const Vector3 originRod = Vector3(0.0, 0.0, -L0 / 2.0); 53 | const Vector3 directionRod = Vector3(0.0, 0.0, 1.0); 54 | const Vector3 normalRod = Vector3(1.0, 0.0, 0.0); 55 | 56 | // Second moment of area for disk cross section 57 | const REAL I0_1 = A0 * A0 / (4.0 * M_PI); 58 | const REAL I0_2 = I0_1; 59 | const REAL I0_3 = 2.0 * I0_1; 60 | const Matrix3 I0 = Matrix3(I0_1, 0.0, 0.0, 0.0, I0_2, 0.0, 0.0, 0.0, I0_3); 61 | 62 | // Mass inertia matrix for disk cross section 63 | const Matrix3 J0 = density * dL0 * I0; 64 | 65 | // Bending matrix 66 | Matrix3 B0 = Matrix3(alpha, 0.0, 0.0, 0.0, alpha, 0.0, 0.0, 0.0, beta); 67 | 68 | // Shear matrix --> the high value numerically enforce both unshearability and 69 | // unstreatchability! 70 | Matrix3 S0 = 1e5 * Matrix3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); 71 | 72 | // Initialize straight rod and pack it into a vector of pointers to rod. 73 | const bool useSelfContact = false; 74 | Rod* rod = RodInitialConfigurations::straightRod( 75 | n, totalMass, r0, J0, B0, S0, L0, totalInitialTwist, originRod, 76 | directionRod, normalRod, nu, nuRelaxation, useSelfContact); 77 | vector rodPtrs; 78 | rodPtrs.push_back(rod); 79 | 80 | // Perturb rod and update it 81 | rod->v[floor(n / 2)].x += 1e-6; 82 | rod->update(0.0); 83 | rod->computeEnergies(); 84 | 85 | // Pack boundary conditions 86 | HelicalBucklingBC endBC = HelicalBucklingBC(rodPtrs, timeTwist, D, R); 87 | vector boundaryConditionsPtrs; 88 | boundaryConditionsPtrs.push_back(&endBC); 89 | 90 | // Pack all forces together 91 | vector externalForcesPtrs; 92 | NoForces endpointsForce = NoForces(); 93 | MultipleForces multipleForces; 94 | multipleForces.add(&endpointsForce); 95 | MultipleForces* multipleForcesPtr = multipleForces.get(); 96 | externalForcesPtrs.push_back(multipleForcesPtr); 97 | 98 | // Empty interaction forces (no substrate in this case) 99 | vector substrateInteractionsPtrs; 100 | 101 | // No external contact function needed 102 | vector> attachpoint; 103 | vector externalcontactPtrs; 104 | ExternalContact externalcontact = 105 | ExternalContact(rodPtrs, 0.0, 0.0, attachpoint); 106 | externalcontactPtrs.push_back(&externalcontact); 107 | 108 | // No Simple Connection needed 109 | vector simpleconnectionPtrs; 110 | SimpleConnection simpleconnection = SimpleConnection(rodPtrs); 111 | simpleconnectionPtrs.push_back(&simpleconnection); 112 | 113 | // Set up time integrator 114 | PolymerIntegrator* integrator = new PositionVerlet2nd( 115 | rodPtrs, externalForcesPtrs, boundaryConditionsPtrs, 116 | substrateInteractionsPtrs, externalcontactPtrs, simpleconnectionPtrs); 117 | 118 | // Simulate 119 | Polymer poly = Polymer(integrator); 120 | const bool goodRun = poly.simulate(timeSimulation, dt, diagPerUnitTime, 121 | povrayPerUnitTime, outfileName); 122 | // const bool goodRun = poly.simulate(220.0, dt, diagPerUnitTime, 123 | // povrayPerUnitTime, outfileName); 124 | 125 | // Throw exception if something went wrong 126 | if (!goodRun) 127 | throw "not good run in localized helical buckling, what is going on?"; 128 | 129 | // Compute maximum envelope to be compared to analytical solution (See MATLAB 130 | // script "vanHejiden.m") 131 | VV3 tangents = vUnitize(vDiff(rod->x)); 132 | assert(tangents.size() == rod->l0.size()); 133 | REAL maxPhi = numeric_limits::min(); 134 | for (unsigned int i = 0; i < tangents.size(); i++) { 135 | const REAL argumentACos = tangents[i] % directionRod; 136 | assert(argumentACos >= -1.0 - Tolerance::tol() && 137 | argumentACos <= 1.0 + Tolerance::tol()); 138 | const REAL argumentACosClamped = 139 | max((REAL)-1.0, (REAL)min(argumentACos, (REAL)1.0)); 140 | const REAL phi = acos(argumentACosClamped); 141 | maxPhi = max(maxPhi, phi); 142 | } 143 | 144 | // Compute max velocity to make sure it is at quilibrium 145 | REAL maxVel = 0.0; 146 | for (unsigned int i = 0; i < rod->v.size(); i++) 147 | maxVel = max(maxVel, fabs(rod->v[i].length())); 148 | 149 | // Dump post buckling helical shape 150 | { 151 | string helixShape = outfileName + "_shape.txt"; 152 | FILE* fitnessFile = fopen(helixShape.c_str(), "w"); 153 | assert(fitnessFile != NULL); 154 | 155 | for (unsigned int i = 0; i < rod->x.size(); i++) 156 | fprintf(fitnessFile, "%1.15e %1.15e %1.15e\n", rod->x[i].x, rod->x[i].y, 157 | rod->x[i].z); 158 | 159 | fclose(fitnessFile); 160 | } 161 | 162 | cout << "TENSION AT THE ENDS" << endl; 163 | cout << rod->totalInternalForces.front() << endl; 164 | cout << rod->totalInternalForces.back() << endl; 165 | 166 | cout << "TORQUE AT THE ENDS" << endl; 167 | cout << rod->totalInternalTorques.front() << endl; 168 | cout << rod->totalInternalTorques.back() << endl; 169 | 170 | return false; 171 | } 172 | 173 | void InstabilityHelical::_test(const unsigned int nEdges, 174 | string outputfilename) { 175 | // const REAL timeTwist = 250.0; 176 | // const REAL timeRelax = 10000.0; 177 | const REAL timeTwist = 500.0; 178 | const REAL timeRelax = 10000.0; 179 | const REAL nu = 1e-2; 180 | _testLocalizedHelicalBuckling(nEdges, timeTwist, timeRelax, nu, 181 | outputfilename); 182 | } 183 | 184 | void InstabilityHelical::run() { 185 | { 186 | cout << "Localized helical buckling: time-space convergnece" << endl 187 | << endl; 188 | // _test(70, "helix_0100"); 189 | _test(100, "helix_0100"); 190 | // _test(200, "helix_0200"); 191 | // _test(400, "helix_0400"); 192 | // _test(800, "helix_0800"); 193 | //_test(1600, "helix_1600"); 194 | //_test(3200, "helix_3200"); 195 | cout << "study completed!" << endl << endl; 196 | } 197 | 198 | exit(0); 199 | } 200 | -------------------------------------------------------------------------------- /source/InstabilityHelical.h: -------------------------------------------------------------------------------- 1 | /* 2 | * InstabilityHelical.h 3 | * 4 | * Created on: Sep 8, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #ifndef INSTABILITYHELICAL_H_ 9 | #define INSTABILITYHELICAL_H_ 10 | 11 | #include "ArgumentParser.h" 12 | #include "Polymer.h" 13 | #include "PositionVerlet2nd.h" 14 | #include "RodBoundaryConditions.h" 15 | #include "RodInitialConfigurations.h" 16 | #include "Test.h" 17 | #include "Tolerance.h" 18 | #include "UsualHeaders.h" 19 | 20 | using namespace std; 21 | 22 | class InstabilityHelical : public Test { 23 | protected: 24 | static bool _testLocalizedHelicalBuckling(const int nEdges, 25 | const REAL _timeTwist, 26 | const REAL _timeRelax, 27 | const REAL _nu, string outfileName); 28 | static void _test(const unsigned int nEdges, string outputfilename); 29 | 30 | public: 31 | InstabilityHelical(const int argc, const char** argv); 32 | virtual ~InstabilityHelical(); 33 | 34 | void run(); 35 | void paint(){}; 36 | }; 37 | 38 | #endif /* INSTABILITYHELICAL_H_ */ 39 | -------------------------------------------------------------------------------- /source/MRAGEnvironment.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MRAGEnvironment.h 3 | * MRAG 4 | * 5 | * Created by Diego Rossinelli on 7/21/08. 6 | * Copyright 2008 CSE Lab, ETH Zurich. All rights reserved. 7 | * 8 | */ 9 | namespace MRAG { 10 | 11 | // ARCHITECTURE STUFF 12 | //#define _MRAG_TBB 13 | 14 | #ifdef _MRAG_TBB 15 | #ifndef _MRAG_TBB_NTHREADS_HINT 16 | #define _MRAG_TBB_NTHREADS_HINT 2 17 | #endif 18 | #endif 19 | } // namespace MRAG 20 | 21 | #ifdef _MRAG_TBB 22 | #include 23 | #include 24 | #endif 25 | #pragma once 26 | namespace MRAG { 27 | // ENVIRONMENT: RT SETUP 28 | namespace Environment { 29 | /** 30 | * General setup of MRAG Environment. Should be called before doing stuff. 31 | */ 32 | inline void setup(int threads = -1) { 33 | #ifdef _MRAG_TBB 34 | static tbb::task_scheduler_init *init = NULL; 35 | 36 | if (init == NULL) { 37 | const int nthreads = threads == -1 ? _MRAG_TBB_NTHREADS_HINT : threads; 38 | init = new tbb::task_scheduler_init(nthreads); 39 | printf("INITIALIZED THREADS=%d (_MRAG_TBB_NTHREADS_HINT is %d)\n", nthreads, 40 | _MRAG_TBB_NTHREADS_HINT); 41 | } 42 | #endif 43 | } 44 | } // namespace Environment 45 | } // namespace MRAG 46 | -------------------------------------------------------------------------------- /source/MRAGProfiler.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MRAGProfiler.cpp 3 | * MRAG 4 | * 5 | * Created by Diego Rossinelli on 9/13/08. 6 | * Copyright 2008 CSE Lab, ETH Zurich. All rights reserved. 7 | * 8 | */ 9 | 10 | #include "MRAGProfiler.h" 11 | #include "MRAGEnvironment.h" 12 | 13 | #ifdef _MRAG_TBB 14 | 15 | #include "tbb/tick_count.h" 16 | using namespace tbb; 17 | 18 | void MRAG::ProfileAgent::_getTime(tick_count &time) { 19 | time = tick_count::now(); 20 | } 21 | 22 | float MRAG::ProfileAgent::_getElapsedTime(const tick_count &tS, 23 | const tick_count &tE) { 24 | return (tE - tS).seconds(); 25 | } 26 | 27 | #else 28 | #include 29 | void MRAG::ProfileAgent::_getTime(clock_t &time) { time = clock(); } 30 | 31 | float MRAG::ProfileAgent::_getElapsedTime(const clock_t &tS, 32 | const clock_t &tE) { 33 | return (tE - tS) / (double)CLOCKS_PER_SEC; 34 | } 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /source/MRAGProfiler.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MRAGProfiler.h 3 | * MRAG 4 | * 5 | * Created by Diego Rossinelli on 9/13/08. 6 | * Copyright 2008 CSE Lab, ETH Zurich. All rights reserved. 7 | * 8 | */ 9 | 10 | #pragma once 11 | #include 12 | #undef min 13 | #undef max 14 | #include 15 | #undef min 16 | #undef max 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace std; 22 | 23 | #include "MRAGEnvironment.h" 24 | #ifdef _MRAG_TBB 25 | #include "tbb/tick_count.h" 26 | namespace tbb { 27 | class tick_count; 28 | } 29 | #else 30 | #include 31 | #endif 32 | 33 | namespace MRAG { 34 | /** Just for debugging (enables some printouts). */ 35 | const bool bVerboseProfiling = false; 36 | 37 | /** 38 | * Profiles one part of the code (parts collected in MRAG::Profiler). 39 | * The user should use ProfileAgent through MRAG::Profiler::getAgent(). 40 | * Stores (only accessible through MRAG::Profiler): 41 | * - Accumulated timings whenever profiler is started/stopped (in s) 42 | * - Number of measurements (i.e. once per start/stop) 43 | * - optional: "earned Money" for all the stuff done (added at each stop) 44 | * @see ProfileAgent::start(), ProfileAgent::stop() 45 | */ 46 | class ProfileAgent { 47 | #ifdef _MRAG_TBB 48 | typedef tbb::tick_count ClockTime; 49 | #else 50 | typedef clock_t ClockTime; 51 | #endif 52 | 53 | enum ProfileAgentState { 54 | ProfileAgentState_Created, 55 | ProfileAgentState_Started, 56 | ProfileAgentState_Stopped 57 | }; 58 | 59 | ClockTime m_tStart, m_tEnd; 60 | ProfileAgentState m_state; 61 | double m_dAccumulatedTime; 62 | int m_nMeasurements; 63 | int m_nMoney; 64 | 65 | static void _getTime(ClockTime &time); 66 | static float _getElapsedTime(const ClockTime &tS, const ClockTime &tE); 67 | 68 | void _reset() { 69 | m_tStart = ClockTime(); 70 | m_tEnd = ClockTime(); 71 | m_dAccumulatedTime = 0; 72 | m_nMeasurements = 0; 73 | m_nMoney = 0; 74 | m_state = ProfileAgentState_Created; 75 | } 76 | 77 | public: 78 | ProfileAgent() 79 | : m_tStart(), 80 | m_tEnd(), 81 | m_state(ProfileAgentState_Created), 82 | m_dAccumulatedTime(0), 83 | m_nMeasurements(0), 84 | m_nMoney(0) {} 85 | 86 | double getAccumulatedTime() const { return m_dAccumulatedTime; } 87 | 88 | /** 89 | * Start a time measurement. 90 | */ 91 | void start() { 92 | assert(m_state == ProfileAgentState_Created || 93 | m_state == ProfileAgentState_Stopped); 94 | 95 | if (bVerboseProfiling) { 96 | printf("start\n"); 97 | } 98 | 99 | _getTime(m_tStart); 100 | 101 | m_state = ProfileAgentState_Started; 102 | } 103 | 104 | /** 105 | * Stop a time measurement. 106 | * Time elapsed since ProfileAgent::start() was called is added to the 107 | * timings. 108 | * @param nMoney Defines how much "money" we earned for whatever we did 109 | * since calling ProfileAgent::start(). 110 | */ 111 | void stop(int nMoney = 0) { 112 | assert(m_state == ProfileAgentState_Started); 113 | 114 | if (bVerboseProfiling) { 115 | printf("stop\n"); 116 | } 117 | 118 | _getTime(m_tEnd); 119 | m_dAccumulatedTime += _getElapsedTime(m_tStart, m_tEnd); 120 | m_nMeasurements++; 121 | m_nMoney += nMoney; 122 | m_state = ProfileAgentState_Stopped; 123 | } 124 | 125 | friend class Profiler; 126 | }; 127 | 128 | /** 129 | * Helper to collect statistics on profiled stuff. 130 | */ 131 | struct ProfileSummaryItem { 132 | string sName; 133 | double dTime; 134 | int nMoney; 135 | int nSamples; 136 | double dAverageTime; 137 | 138 | ProfileSummaryItem(string sName_, double dTime_, int nMoney_, int nSamples_) 139 | : sName(sName_), 140 | dTime(dTime_), 141 | nMoney(nMoney_), 142 | nSamples(nSamples_), 143 | dAverageTime(dTime_ / nSamples_) {} 144 | }; 145 | 146 | /** 147 | * Profile different parts of your code (identified by a user-specified 148 | * string-ID). For each string-ID we get a MRAG::ProfileAgent, where we can 149 | * store timings. 150 | * @see Profiler::getAgent(), Profiler::printSummary() 151 | */ 152 | class Profiler { 153 | protected: 154 | map m_mapAgents; 155 | stack m_mapStoppedAgents; 156 | 157 | public: 158 | void push_start(string sAgentName) { 159 | if (m_mapStoppedAgents.size() > 0) 160 | getAgent(m_mapStoppedAgents.top()).stop(); 161 | 162 | m_mapStoppedAgents.push(sAgentName); 163 | getAgent(sAgentName).start(); 164 | } 165 | 166 | void pop_stop() { 167 | string sCurrentAgentName = m_mapStoppedAgents.top(); 168 | getAgent(sCurrentAgentName).stop(); 169 | m_mapStoppedAgents.pop(); 170 | 171 | if (m_mapStoppedAgents.size() == 0) return; 172 | 173 | getAgent(m_mapStoppedAgents.top()).start(); 174 | } 175 | 176 | void clear() { 177 | for (map::iterator it = m_mapAgents.begin(); 178 | it != m_mapAgents.end(); it++) { 179 | delete it->second; 180 | 181 | it->second = NULL; 182 | } 183 | 184 | m_mapAgents.clear(); 185 | } 186 | 187 | Profiler() : m_mapAgents() {} 188 | 189 | ~Profiler() { clear(); } 190 | 191 | void printSummary() const { 192 | vector v = createSummary(); 193 | 194 | double dTotalTime = 0; 195 | double dTotalTime2 = 0; 196 | for (vector::const_iterator it = v.begin(); 197 | it != v.end(); it++) 198 | dTotalTime += it->dTime; 199 | 200 | for (vector::const_iterator it = v.begin(); 201 | it != v.end(); it++) 202 | dTotalTime2 += it->dTime - it->nSamples * 1.30e-6; 203 | 204 | for (vector::const_iterator it = v.begin(); 205 | it != v.end(); it++) { 206 | const ProfileSummaryItem &item = *it; 207 | const double avgTime = item.dAverageTime; 208 | 209 | printf( 210 | "[%20s]: \t%02.0f-%02.0f%%\t%03.3e (%03.3e) s\t%03.3f (%03.3f) " 211 | "s\t(%d samples)\n", 212 | item.sName.c_str(), 100 * item.dTime / dTotalTime, 213 | 100 * (item.dTime - item.nSamples * 1.3e-6) / dTotalTime2, avgTime, 214 | avgTime - 1.30e-6, item.dTime, item.dTime - item.nSamples * 1.30e-6, 215 | item.nSamples); 216 | // if (outFile) fprintf(outFile,"[%30s]: \t%02.2f%%\t%03.3f s\t(%d 217 | // samples)\n", 218 | // 219 | // item.sName.data(), 220 | // 100*item.dTime/dTotalTime, avgTime, item.nSamples); 221 | } 222 | 223 | printf("[Total time]: \t%f\n", dTotalTime); 224 | // if (outFile) fprintf(outFile,"[Total time]: \t%f\n", dTotalTime); 225 | // if (outFile) fflush(outFile); 226 | // if (outFile) fclose(outFile); 227 | 228 | // return ;dTotalTime; 229 | } 230 | 231 | void printSummaryToFile(FILE *f) const { 232 | if (f == NULL) { 233 | printf( 234 | "Akamon now. you cannot say printSummaryToFile and f is NULL. " 235 | "aborting.\n"); 236 | abort(); 237 | } 238 | 239 | vector v = createSummary(); 240 | 241 | double dTotalTime = 0; 242 | double dTotalTime2 = 0; 243 | for (vector::const_iterator it = v.begin(); 244 | it != v.end(); it++) 245 | dTotalTime += it->dTime; 246 | 247 | for (vector::const_iterator it = v.begin(); 248 | it != v.end(); it++) 249 | dTotalTime2 += it->dTime - it->nSamples * 1.30e-6; 250 | 251 | for (vector::const_iterator it = v.begin(); 252 | it != v.end(); it++) { 253 | const ProfileSummaryItem &item = *it; 254 | const double avgTime = item.dAverageTime; 255 | 256 | fprintf(f, 257 | "[%20s]: \t%02.0f-%02.0f%%\t%03.3e (%03.3e) s\t%03.3f (%03.3f) " 258 | "s\t(%d samples)\n", 259 | item.sName.c_str(), 100 * item.dTime / dTotalTime, 260 | 100 * (item.dTime - item.nSamples * 1.3e-6) / dTotalTime2, 261 | avgTime, avgTime - 1.30e-6, item.dTime, 262 | item.dTime - item.nSamples * 1.30e-6, item.nSamples); 263 | } 264 | 265 | fprintf(f, "[Total time]: \t%f\n", dTotalTime); 266 | } 267 | 268 | vector createSummary() const { 269 | vector result; 270 | result.reserve(m_mapAgents.size()); 271 | 272 | for (map::const_iterator it = m_mapAgents.begin(); 273 | it != m_mapAgents.end(); it++) { 274 | const ProfileAgent &agent = *it->second; 275 | 276 | result.push_back(ProfileSummaryItem(it->first, agent.m_dAccumulatedTime, 277 | agent.m_nMoney, 278 | agent.m_nMeasurements)); 279 | } 280 | 281 | return result; 282 | } 283 | 284 | void reset() { 285 | // printf("reset\n"); 286 | for (map::const_iterator it = m_mapAgents.begin(); 287 | it != m_mapAgents.end(); it++) 288 | it->second->_reset(); 289 | } 290 | 291 | ProfileAgent &getAgent(string sName) { 292 | if (bVerboseProfiling) { 293 | printf("%s ", sName.c_str()); 294 | } 295 | 296 | map::const_iterator it = m_mapAgents.find(sName); 297 | 298 | const bool bFound = it != m_mapAgents.end(); 299 | 300 | if (bFound) return *it->second; 301 | 302 | ProfileAgent *agent = new ProfileAgent(); 303 | 304 | m_mapAgents[sName] = agent; 305 | 306 | return *agent; 307 | } 308 | 309 | friend class ProfileAgent; 310 | }; 311 | 312 | } // namespace MRAG 313 | -------------------------------------------------------------------------------- /source/MathFunctions.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MathFunctions.cpp 3 | * 4 | * Created on: Mar 4, 2015 5 | * Author: mgazzola 6 | */ 7 | 8 | #include "MathFunctions.h" 9 | 10 | // Returns fmod shifted into [0,r] 11 | REAL posMod(const REAL a, const REAL r) { 12 | REAL v = fmod(a, r); 13 | 14 | if (v < 0) v += r; 15 | 16 | return v; 17 | } 18 | 19 | // Calls the normal acos func, but if x>1 returns 0, and if x<-1 returns PI, 20 | // so as to get around precision errors making acos(cos(0)) = nan 21 | REAL arcCos(const REAL x) { 22 | const REAL argumentACosClamped = 23 | std::max((REAL)-1.0, (REAL)std::min(x, (REAL)1.0)); 24 | return acos(argumentACosClamped); 25 | } 26 | 27 | // Gaussian random normal generator without trigonometric calls using Box-Muller 28 | // transform 29 | double randn_notrig(const double mu, const double sigma) { 30 | double var1 = 0.0; 31 | double var2 = 0.0; 32 | double rsquared = 0.0; 33 | 34 | // If no deviate has been stored, the polar Box-Muller transformation is 35 | // performed, producing two independent normally-distributed random 36 | // deviates. One is stored for the next round, and one is returned. 37 | 38 | // Choose pairs of uniformly distributed deviates, discarding those 39 | // that don't fall within the unit circle 40 | do { 41 | var1 = 2.0 * (double(rand()) / double(RAND_MAX)) - 1.0; 42 | var2 = 2.0 * (double(rand()) / double(RAND_MAX)) - 1.0; 43 | rsquared = var1 * var1 + var2 * var2; 44 | } while (rsquared >= 1.0 || rsquared == 0.0); 45 | 46 | // Calculate polar tranformation for each deviate 47 | const double polar = sqrt(-2.0 * log(rsquared) / rsquared); 48 | 49 | // Return second deviate 50 | return var2 * polar * sigma + mu; 51 | } 52 | 53 | /* 54 | // PREVIOUS VERSION 55 | double randn_notrig(const double mu, const double sigma) 56 | { 57 | bool deviateAvailable = false; //flag 58 | float storedDeviate; //deviate from previous calculation 59 | double polar, rsquared, var1, var2; 60 | 61 | // If no deviate has been stored, the polar Box-Muller transformation is 62 | // performed, producing two independent normally-distributed random 63 | // deviates. One is stored for the next round, and one is returned. 64 | if (!deviateAvailable) 65 | { 66 | // Choose pairs of uniformly distributed deviates, discarding 67 | those 68 | // that don't fall within the unit circle 69 | do 70 | { 71 | var1=2.0*( double(rand())/double(RAND_MAX) ) - 1.0; 72 | var2=2.0*( double(rand())/double(RAND_MAX) ) - 1.0; 73 | rsquared=var1*var1+var2*var2; 74 | } while ( rsquared>=1.0 || rsquared == 0.0); 75 | 76 | // Calculate polar tranformation for each deviate 77 | polar=sqrt(-2.0*log(rsquared)/rsquared); 78 | 79 | // Store first deviate and set flag 80 | storedDeviate=var1*polar; 81 | deviateAvailable=true; 82 | 83 | // Return second deviate 84 | return var2*polar*sigma + mu; 85 | } 86 | else 87 | { 88 | // If a deviate is available from a previous call to this 89 | function, it is 90 | // returned, and the flag is set to false. 91 | deviateAvailable=false; 92 | return storedDeviate*sigma + mu; 93 | } 94 | } 95 | */ 96 | -------------------------------------------------------------------------------- /source/MathFunctions.h: -------------------------------------------------------------------------------- 1 | #ifndef MATHFUNCTIONS_H 2 | #define MATHFUNCTIONS_H 3 | 4 | #include "ArithmeticPrecision.h" 5 | #include "UsualHeaders.h" 6 | 7 | // Returns fmod shifted into [0,r] 8 | REAL posMod(const REAL a, const REAL r); 9 | 10 | // Calls the normal asin func, but if x>1 returns PI/2, and if x<-1 returns 11 | // -PI/2, so as to get around precision errors making asin(sin(PI/2)) = 12 | // nan 13 | REAL arcSin(const REAL x); 14 | 15 | // Calls the normal acos func, but if x>1 returns 0, and if x<-1 returns PI, 16 | // so as to get around precision errors making acos(cos(0)) = nan 17 | REAL arcCos(const REAL x); 18 | 19 | // Gaussian random normal generator without trigonometric calls using Box-Muller 20 | // transform 21 | double randn_notrig(const double mu = 0.0, const double sigma = 1.0); 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /source/Matrix3.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Matrix3.cpp 3 | * 4 | * Created on: Feb 26, 2015 5 | * Author: mgazzola 6 | */ 7 | 8 | #include "Matrix3.h" 9 | 10 | // constructors 11 | Matrix3::Matrix3() 12 | : r1c1(1.), 13 | r1c2(0.), 14 | r1c3(0.), 15 | r2c1(0.), 16 | r2c2(1.), 17 | r2c3(0.), 18 | r3c1(0.), 19 | r3c2(0.), 20 | r3c3(1.) {} 21 | 22 | Matrix3::Matrix3(REAL r1c1, REAL r1c2, REAL r1c3, REAL r2c1, REAL r2c2, 23 | REAL r2c3, REAL r3c1, REAL r3c2, REAL r3c3) 24 | : r1c1(r1c1), 25 | r1c2(r1c2), 26 | r1c3(r1c3), 27 | r2c1(r2c1), 28 | r2c2(r2c2), 29 | r2c3(r2c3), 30 | r3c1(r3c1), 31 | r3c2(r3c2), 32 | r3c3(r3c3) {} 33 | 34 | Matrix3::Matrix3(const Vector3 &S) 35 | : // skew matrix <-> vector mapping 36 | r1c1(0.), 37 | r1c2(S.z), 38 | r1c3(-S.y), 39 | r2c1(-S.z), 40 | r2c2(0.), 41 | r2c3(S.x), 42 | r3c1(S.y), 43 | r3c2(-S.x), 44 | r3c3(0.) {} 45 | 46 | Matrix3::Matrix3(const Vector3 &r1, const Vector3 &r2, const Vector3 &r3) 47 | : r1c1(r1.x), 48 | r1c2(r1.y), 49 | r1c3(r1.z), 50 | r2c1(r2.x), 51 | r2c2(r2.y), 52 | r2c3(r2.z), 53 | r3c1(r3.x), 54 | r3c2(r3.y), 55 | r3c3(r3.z) {} 56 | 57 | Matrix3::Matrix3(const Matrix3 &M) 58 | : r1c1(M.r1c1), 59 | r1c2(M.r1c2), 60 | r1c3(M.r1c3), 61 | r2c1(M.r2c1), 62 | r2c2(M.r2c2), 63 | r2c3(M.r2c3), 64 | r3c1(M.r3c1), 65 | r3c2(M.r3c2), 66 | r3c3(M.r3c3) {} 67 | 68 | // I/O 69 | std::ostream &operator<<(std::ostream &out, const Matrix3 &A) { 70 | out.setf(std::ios::fixed | std::ios::right); 71 | out.width(A.PRINTPRECISION + 3); 72 | out.precision(A.PRINTPRECISION); 73 | out << A.r1c1 << ", "; 74 | out.setf(std::ios::fixed | std::ios::right); 75 | out.width(A.PRINTPRECISION + 3); 76 | out.precision(A.PRINTPRECISION); 77 | out << A.r1c2 << ", "; 78 | out.setf(std::ios::fixed | std::ios::right); 79 | out.width(A.PRINTPRECISION + 3); 80 | out.precision(A.PRINTPRECISION); 81 | out << A.r1c3 << "\n"; 82 | out.setf(std::ios::fixed | std::ios::right); 83 | out.width(A.PRINTPRECISION + 3); 84 | out.precision(A.PRINTPRECISION); 85 | out << A.r2c1 << ", "; 86 | out.setf(std::ios::fixed | std::ios::right); 87 | out.width(A.PRINTPRECISION + 3); 88 | out.precision(A.PRINTPRECISION); 89 | out << A.r2c2 << ", "; 90 | out.setf(std::ios::fixed | std::ios::right); 91 | out.width(A.PRINTPRECISION + 3); 92 | out.precision(A.PRINTPRECISION); 93 | out << A.r2c3 << "\n"; 94 | out.setf(std::ios::fixed | std::ios::right); 95 | out.width(A.PRINTPRECISION + 3); 96 | out.precision(A.PRINTPRECISION); 97 | out << A.r3c1 << ", "; 98 | out.setf(std::ios::fixed | std::ios::right); 99 | out.width(A.PRINTPRECISION + 3); 100 | out.precision(A.PRINTPRECISION); 101 | out << A.r3c2 << ", "; 102 | out.setf(std::ios::fixed | std::ios::right); 103 | out.width(A.PRINTPRECISION + 3); 104 | out.precision(A.PRINTPRECISION); 105 | out << A.r3c3; 106 | 107 | return out; 108 | } 109 | 110 | ////////////////// operators not declarable inside Matrix3 ////////// 111 | 112 | Matrix3 operator*(REAL a, const Matrix3 &M) { return M * a; } 113 | 114 | // treating Vector3's as row vectors here: we need to be careful in using it! 115 | Vector3 operator*(const Vector3 &v, const Matrix3 &M) { 116 | return Vector3(v.x * M.r1c1 + v.y * M.r2c1 + v.z * M.r3c1, 117 | v.x * M.r1c2 + v.y * M.r2c2 + v.z * M.r3c2, 118 | v.x * M.r1c3 + v.y * M.r2c3 + v.z * M.r3c3); 119 | } 120 | 121 | // exponential of a vector (\in so(3)) maps to an orthogonal matrix (\in SO(3)) 122 | // using rodiguez formula. The size (angle) of the rotation is just the 123 | // magnitude of the vector. 124 | Matrix3 exp(const Vector3 &S, const REAL tol) { 125 | const REAL theta = S.length(); 126 | if (theta < tol) return Matrix3(); 127 | Matrix3 Sx = Matrix3(S.unitize()); 128 | return Matrix3() + Sx * sin(theta) + (1 - cos(theta)) * Sx * Sx; 129 | } 130 | 131 | void exp(const Vector3 &S, Matrix3 &B) { 132 | // Clear matrix container and initialize to identity matrix 133 | B.r1c1 = 1.0; 134 | B.r1c2 = 0.0; 135 | B.r1c3 = 0.0; 136 | B.r2c1 = 0.0; 137 | B.r2c2 = 1.0; 138 | B.r2c3 = 0.0; 139 | B.r3c1 = 0.0; 140 | B.r3c2 = 0.0; 141 | B.r3c3 = 1.0; 142 | 143 | // Fast in-place evaluation of the rodriguez formula 144 | const REAL theta = S.length(); 145 | const Matrix3 Sx = Matrix3(S.unitize()); 146 | const REAL sintheta = sin(theta); 147 | const REAL oneminuscostheta = 1.0 - cos(theta); 148 | 149 | B.r1c1 += oneminuscostheta * (Sx.r1c2 * Sx.r2c1 + Sx.r1c3 * Sx.r3c1); 150 | B.r1c2 += Sx.r1c2 * sintheta + oneminuscostheta * (Sx.r1c3 * Sx.r3c2); 151 | B.r1c3 += Sx.r1c3 * sintheta + oneminuscostheta * (Sx.r1c2 * Sx.r2c3); 152 | B.r2c1 += Sx.r2c1 * sintheta + oneminuscostheta * (Sx.r2c3 * Sx.r3c1); 153 | B.r2c2 += oneminuscostheta * (Sx.r2c1 * Sx.r1c2 + Sx.r2c3 * Sx.r3c2); 154 | B.r2c3 += Sx.r2c3 * sintheta + oneminuscostheta * (Sx.r2c1 * Sx.r1c3); 155 | B.r3c1 += Sx.r3c1 * sintheta + oneminuscostheta * (Sx.r3c2 * Sx.r2c1); 156 | B.r3c2 += Sx.r3c2 * sintheta + oneminuscostheta * (Sx.r3c1 * Sx.r1c2); 157 | B.r3c3 += oneminuscostheta * (Sx.r3c1 * Sx.r1c3 + Sx.r3c2 * Sx.r2c3); 158 | } 159 | 160 | // generates a uniform gaussian random variable in each element 161 | Matrix3 randMatrix3() { 162 | return Matrix3(randn_notrig(0, 1), randn_notrig(0, 1), randn_notrig(0, 1), 163 | randn_notrig(0, 1), randn_notrig(0, 1), randn_notrig(0, 1), 164 | randn_notrig(0, 1), randn_notrig(0, 1), randn_notrig(0, 1)); 165 | } 166 | -------------------------------------------------------------------------------- /source/Matrix3.h: -------------------------------------------------------------------------------- 1 | #ifndef MATRIX3_H 2 | #define MATRIX3_H 3 | 4 | #include "Tolerance.h" 5 | #include "UsualHeaders.h" 6 | #include "Vector3.h" 7 | 8 | using namespace std; 9 | 10 | class Matrix3 { 11 | protected: 12 | typedef std::vector VV3; 13 | typedef std::vector VREAL; 14 | typedef std::vector VBOOL; 15 | typedef std::vector VINT; 16 | 17 | static const unsigned int PRINTPRECISION = 6; 18 | 19 | public: 20 | // This shouldnt be here, super dangerous, wtf!! 21 | REAL r1c1, r1c2, r1c3, r2c1, r2c2, r2c3, r3c1, r3c2, r3c3; 22 | 23 | // constructors 24 | Matrix3(); 25 | Matrix3(REAL r1c1, REAL r1c2, REAL r1c3, REAL r2c1, REAL r2c2, REAL r2c3, 26 | REAL r3c1, REAL r3c2, REAL r3c3); 27 | Matrix3(const Vector3 &S); // skew matrix <-> vector mapping 28 | Matrix3(const Vector3 &r1, const Vector3 &r2, const Vector3 &r3); 29 | Matrix3(const Matrix3 &M); 30 | 31 | // matrix operators 32 | inline Matrix3 operator+(const Matrix3 &M) const { 33 | return Matrix3(r1c1 + M.r1c1, r1c2 + M.r1c2, r1c3 + M.r1c3, r2c1 + M.r2c1, 34 | r2c2 + M.r2c2, r2c3 + M.r2c3, r3c1 + M.r3c1, r3c2 + M.r3c2, 35 | r3c3 + M.r3c3); 36 | } 37 | 38 | inline Matrix3 operator-(const Matrix3 &M) const { 39 | return Matrix3(r1c1 - M.r1c1, r1c2 - M.r1c2, r1c3 - M.r1c3, r2c1 - M.r2c1, 40 | r2c2 - M.r2c2, r2c3 - M.r2c3, r3c1 - M.r3c1, r3c2 - M.r3c2, 41 | r3c3 - M.r3c3); 42 | } 43 | 44 | inline Matrix3 operator*(const Matrix3 &M) const // matrix multiplication 45 | { 46 | return Matrix3(r1c1 * M.r1c1 + r1c2 * M.r2c1 + r1c3 * M.r3c1, 47 | r1c1 * M.r1c2 + r1c2 * M.r2c2 + r1c3 * M.r3c2, 48 | r1c1 * M.r1c3 + r1c2 * M.r2c3 + r1c3 * M.r3c3, 49 | r2c1 * M.r1c1 + r2c2 * M.r2c1 + r2c3 * M.r3c1, 50 | r2c1 * M.r1c2 + r2c2 * M.r2c2 + r2c3 * M.r3c2, 51 | r2c1 * M.r1c3 + r2c2 * M.r2c3 + r2c3 * M.r3c3, 52 | r3c1 * M.r1c1 + r3c2 * M.r2c1 + r3c3 * M.r3c1, 53 | r3c1 * M.r1c2 + r3c2 * M.r2c2 + r3c3 * M.r3c2, 54 | r3c1 * M.r1c3 + r3c2 * M.r2c3 + r3c3 * M.r3c3); 55 | } 56 | 57 | // vector operators 58 | inline Vector3 operator*(const Vector3 &v) const { 59 | return Vector3(r1c1 * v.x + r1c2 * v.y + r1c3 * v.z, 60 | r2c1 * v.x + r2c2 * v.y + r2c3 * v.z, 61 | r3c1 * v.x + r3c2 * v.y + r3c3 * v.z); 62 | } 63 | 64 | // scalar operators 65 | inline Matrix3 operator*(const REAL a) const { 66 | return Matrix3(a * r1c1, a * r1c2, a * r1c3, a * r2c1, a * r2c2, a * r2c3, 67 | a * r3c1, a * r3c2, a * r3c3); 68 | } 69 | 70 | inline Matrix3 operator/(const REAL a) const { 71 | return Matrix3(r1c1 / a, r1c2 / a, r1c3 / a, r2c1 / a, r2c2 / a, r2c3 / a, 72 | r3c1 / a, r3c2 / a, r3c3 / a); 73 | } 74 | 75 | inline Matrix3 operator-() const { 76 | return Matrix3(-r1c1, -r1c2, -r1c3, -r2c1, -r2c2, -r2c3, -r3c1, -r3c2, 77 | -r3c3); 78 | } 79 | 80 | // in-place modifiers 81 | inline Matrix3 &operator+=(const Matrix3 &M) { 82 | r1c1 += M.r1c1; 83 | r1c2 += M.r1c2; 84 | r1c3 += M.r1c3; 85 | r2c1 += M.r2c1; 86 | r2c2 += M.r2c2; 87 | r2c3 += M.r2c3; 88 | r3c1 += M.r3c1; 89 | r3c2 += M.r3c2; 90 | r3c3 += M.r3c3; 91 | return *this; 92 | } 93 | 94 | inline Matrix3 &operator-=(const Matrix3 &M) { 95 | r1c1 -= M.r1c1; 96 | r1c2 -= M.r1c2; 97 | r1c3 -= M.r1c3; 98 | r2c1 -= M.r2c1; 99 | r2c2 -= M.r2c2; 100 | r2c3 -= M.r2c3; 101 | r3c1 -= M.r3c1; 102 | r3c2 -= M.r3c2; 103 | r3c3 -= M.r3c3; 104 | return *this; 105 | } 106 | 107 | inline Matrix3 &operator*=(const Matrix3 &M) { 108 | *this = *this * M; 109 | return *this; 110 | } 111 | 112 | inline Matrix3 &operator*=(const REAL a) { 113 | r1c1 *= a; 114 | r1c2 *= a; 115 | r1c3 *= a; 116 | r2c1 *= a; 117 | r2c2 *= a; 118 | r2c3 *= a; 119 | r3c1 *= a; 120 | r3c2 *= a; 121 | r3c3 *= a; 122 | return *this; 123 | } 124 | 125 | inline Matrix3 &operator/=(const REAL a) { 126 | r1c1 /= a; 127 | r1c2 /= a; 128 | r1c3 /= a; 129 | r2c1 /= a; 130 | r2c2 /= a; 131 | r2c3 /= a; 132 | r3c1 /= a; 133 | r3c2 /= a; 134 | r3c3 /= a; 135 | return *this; 136 | } 137 | 138 | // general functions 139 | inline Matrix3 T() const // transpose 140 | { 141 | return Matrix3(r1c1, r2c1, r3c1, r1c2, r2c2, r3c2, r1c3, r2c3, r3c3); 142 | } 143 | 144 | inline void T(Matrix3 &A) const // transpose 145 | { 146 | A.r1c1 = r1c1; 147 | A.r1c2 = r2c1; 148 | A.r1c3 = r3c1; 149 | A.r2c1 = r1c2; 150 | A.r2c2 = r2c2; 151 | A.r2c3 = r3c2; 152 | A.r3c1 = r1c3; 153 | A.r3c2 = r2c3; 154 | A.r3c3 = r3c3; 155 | } 156 | 157 | inline Matrix3 I() const // inverse 158 | { 159 | const REAL D = det(); 160 | assert(D >= Tolerance::tol()); 161 | 162 | const REAL _r1c1 = (r2c2 * r3c3 - r3c2 * r2c3) / D; 163 | const REAL _r1c2 = (r3c2 * r1c3 - r1c2 * r3c3) / D; 164 | const REAL _r1c3 = (r1c2 * r2c3 - r2c2 * r1c3) / D; 165 | 166 | const REAL _r2c1 = (r3c1 * r2c3 - r2c1 * r3c3) / D; 167 | const REAL _r2c2 = (r1c1 * r3c3 - r3c1 * r1c3) / D; 168 | const REAL _r2c3 = (r2c1 * r1c3 - r1c1 * r2c3) / D; 169 | 170 | const REAL _r3c1 = (r2c1 * r3c2 - r3c1 * r2c2) / D; 171 | const REAL _r3c2 = (r3c1 * r1c2 - r1c1 * r3c2) / D; 172 | const REAL _r3c3 = (r1c1 * r2c2 - r2c1 * r1c2) / D; 173 | 174 | return Matrix3(_r1c1, _r1c2, _r1c3, _r2c1, _r2c2, _r2c3, _r3c1, _r3c2, 175 | _r3c3); 176 | } 177 | 178 | inline void diagI(Matrix3 &b) const // inverse of a diagonal matrix in-place 179 | { 180 | assert(r2c1 == 0.0); 181 | assert(r3c1 == 0.0); 182 | assert(r1c2 == 0.0); 183 | assert(r3c2 == 0.0); 184 | assert(r1c3 == 0.0); 185 | assert(r2c3 == 0.0); 186 | assert(r1c1 >= Tolerance::tol()); 187 | assert(r2c2 >= Tolerance::tol()); 188 | assert(r3c3 >= Tolerance::tol()); 189 | 190 | assert(b.r2c1 == 0.0); 191 | assert(b.r3c1 == 0.0); 192 | assert(b.r1c2 == 0.0); 193 | assert(b.r3c2 == 0.0); 194 | assert(b.r1c3 == 0.0); 195 | assert(b.r2c3 == 0.0); 196 | 197 | b.r1c1 = 1.0 / r1c1; 198 | b.r2c2 = 1.0 / r2c2; 199 | b.r3c3 = 1.0 / r3c3; 200 | } 201 | 202 | inline REAL tr() const // trace 203 | { 204 | return r1c1 + r2c2 + r3c3; 205 | } 206 | 207 | inline REAL det() const // determinant 208 | { 209 | const REAL D = (+r1c1 * (r2c2 * r3c3 - r2c3 * r3c2) - 210 | r1c2 * (r2c1 * r3c3 - r2c3 * r3c1) + 211 | r1c3 * (r2c1 * r3c2 - r2c2 * r3c1)); 212 | 213 | return D; 214 | } 215 | 216 | inline Vector3 log(const REAL tol = Tolerance::tol()) 217 | const // matrix logarithm (\in so(3)) represented as vector 218 | { 219 | const REAL theta = arcCos((tr() - 1.0) / 2.0); 220 | return (theta < tol) 221 | ? Vector3() 222 | : (theta / (2.0 * sin(theta)) * 223 | Vector3(r2c3 - r3c2, r3c1 - r1c3, 224 | r1c2 - r2c1)); // if theta ~= 0 return zero vector 225 | } 226 | 227 | inline void log(Vector3 &A, const REAL tol = Tolerance::tol()) 228 | const // matrix logarithm (\in so(3)) represented as vector 229 | { 230 | // Clean vector 231 | A.x = 0.0; 232 | A.y = 0.0; 233 | A.z = 0.0; 234 | 235 | const REAL theta = arcCos((tr() - 1.0) / 2.0); 236 | 237 | const REAL factor = (theta <= tol) ? 0.0 : (theta / (2.0 * sin(theta))); 238 | A.x = factor * (r2c3 - r3c2); 239 | A.y = factor * (r3c1 - r1c3); 240 | A.z = factor * (r1c2 - r2c1); 241 | } 242 | 243 | // norms 244 | inline REAL F() const // frobenius 245 | { 246 | return sqrt(r1c1 * r1c1 + r1c2 * r1c2 + r1c3 * r1c3 + r2c1 * r2c1 + 247 | r2c2 * r2c2 + r2c3 * r2c3 + r3c1 * r3c1 + r3c2 * r3c2 + 248 | r3c3 * r3c3); 249 | } 250 | 251 | // utility functions 252 | inline Vector3 S() const // vector associated with skew matrix 253 | { 254 | return Vector3(r2c3, r3c1, r1c2); 255 | } 256 | 257 | inline Vector3 operator[](int rowNum) const // returns the ith row 258 | { 259 | switch (rowNum) { 260 | case 0: 261 | return Vector3(r1c1, r1c2, r1c3); 262 | case 1: 263 | return Vector3(r2c1, r2c2, r2c3); 264 | case 2: 265 | return Vector3(r3c1, r3c2, r3c3); 266 | default: 267 | throw "Bad [] access attemped on Matrix3.\n"; 268 | } 269 | } 270 | 271 | inline bool isOrthogonal(const REAL tol = Tolerance::tol()) 272 | const // returns true if orthogonal within tol 273 | { 274 | Matrix3 QQT = (*this) * (*this).T(); 275 | if ((QQT - Matrix3()).F() < tol) 276 | return true; 277 | else 278 | return false; 279 | } 280 | 281 | inline bool goodNumerics() const { 282 | if ((r1c1 != r1c1) || (r1c2 != r1c2) || (r1c3 != r1c3) || (r2c1 != r2c1) || 283 | (r2c2 != r2c2) || (r2c3 != r2c3) || (r3c1 != r3c1) || (r3c2 != r3c2) || 284 | (r3c3 != r3c3)) 285 | return false; 286 | else 287 | return true; 288 | } 289 | 290 | // speed functions 291 | static inline void matrix_times_vector(const Matrix3 &a, const Vector3 &b, 292 | Vector3 &c) { 293 | c.x = a.r1c1 * b.x + a.r1c2 * b.y + a.r1c3 * b.z; 294 | c.y = a.r2c1 * b.x + a.r2c2 * b.y + a.r2c3 * b.z; 295 | c.z = a.r3c1 * b.x + a.r3c2 * b.y + a.r3c3 * b.z; 296 | } 297 | 298 | static inline void matrix_times_transposed(const Matrix3 &a, const Matrix3 &b, 299 | Matrix3 &c) { 300 | c.r1c1 = a.r1c1 * b.r1c1 + a.r1c2 * b.r1c2 + a.r1c3 * b.r1c3; 301 | c.r1c2 = a.r1c1 * b.r2c1 + a.r1c2 * b.r2c2 + a.r1c3 * b.r2c3; 302 | c.r1c3 = a.r1c1 * b.r3c1 + a.r1c2 * b.r3c2 + a.r1c3 * b.r3c3; 303 | 304 | c.r2c1 = a.r2c1 * b.r1c1 + a.r2c2 * b.r1c2 + a.r2c3 * b.r1c3; 305 | c.r2c2 = a.r2c1 * b.r2c1 + a.r2c2 * b.r2c2 + a.r2c3 * b.r2c3; 306 | c.r2c3 = a.r2c1 * b.r3c1 + a.r2c2 * b.r3c2 + a.r2c3 * b.r3c3; 307 | 308 | c.r3c1 = a.r3c1 * b.r1c1 + a.r3c2 * b.r1c2 + a.r3c3 * b.r1c3; 309 | c.r3c2 = a.r3c1 * b.r2c1 + a.r3c2 * b.r2c2 + a.r3c3 * b.r2c3; 310 | c.r3c3 = a.r3c1 * b.r3c1 + a.r3c2 * b.r3c2 + a.r3c3 * b.r3c3; 311 | } 312 | 313 | // I/O 314 | friend std::ostream &operator<<(std::ostream &out, const Matrix3 &A); 315 | }; 316 | 317 | ////////////////// operators not declarable inside Matrix3 ////////// 318 | 319 | Matrix3 operator*(REAL a, const Matrix3 &M); 320 | 321 | // treating Vector3's as row vectors here: we need to be careful in using it! 322 | Vector3 operator*(const Vector3 &v, const Matrix3 &M); 323 | 324 | // exponential of a vector (\in so(3)) maps to an orthogonal matrix (\in SO(3)) 325 | // using rodiguez formula. The size (angle) of the rotation is just the 326 | // magnitude of the vector. 327 | Matrix3 exp(const Vector3 &S, const REAL tol = Tolerance::tol()); 328 | void exp(const Vector3 &S, Matrix3 &B); 329 | 330 | // generates a uniform gaussian random variable in each element 331 | Matrix3 randMatrix3(); 332 | 333 | #endif 334 | -------------------------------------------------------------------------------- /source/MuscularSnake.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MuscularSnake.h 3 | * 4 | * Created on: Jun 22, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #ifndef MUSCULARSNAKE_H_ 9 | #define MUSCULARSNAKE_H_ 10 | 11 | #include "ArgumentParser.h" 12 | #include "Polymer.h" 13 | #include "PositionVerlet2nd.h" 14 | #include "RodInitialConfigurations.h" 15 | #include "Test.h" 16 | #include "UsualHeaders.h" 17 | 18 | using namespace std; 19 | 20 | class MuscularSnake : public Test { 21 | protected: 22 | vector amp; 23 | double w, v; 24 | int NOR; 25 | 26 | vector _muscularsnakeRun(); 27 | 28 | #ifdef SNAKE_VIZ 29 | void _paint(){}; 30 | #endif 31 | 32 | public: 33 | MuscularSnake(const int argc, const char **argv); 34 | ~MuscularSnake(){}; 35 | 36 | void run(); 37 | void paint(){}; 38 | }; 39 | 40 | #endif /* MUSCULARSNAKE_H_ */ 41 | -------------------------------------------------------------------------------- /source/Polymer.h: -------------------------------------------------------------------------------- 1 | #ifndef POLYMER_H_ 2 | #define POLYMER_H_ 3 | 4 | #ifdef SNAKE_VIZ 5 | #ifdef __APPLE__ 6 | #include 7 | #include "GLUT/glut.h" 8 | #else 9 | #include 10 | #endif 11 | #endif 12 | 13 | #define SNAKE_POV 14 | 15 | #include "ArithmeticPrecision.h" 16 | #include "GeometryFunctions.h" 17 | #include "MRAGProfiler.h" 18 | #include "Matrix3.h" 19 | #include "PolymerIntegrator.h" 20 | #include "UsualHeaders.h" 21 | #include "Vector3.h" 22 | 23 | using namespace std; 24 | 25 | class Polymer { 26 | protected: 27 | static const unsigned int PRINTPRECISION = 6; 28 | 29 | int numRods; 30 | 31 | REAL startTimeStats; 32 | REAL endTimeStats; 33 | vector avgVel; 34 | 35 | vector rodptrs; 36 | vector efptrs; 37 | vector bcptrs; 38 | vector interptrs; 39 | PolymerIntegrator *pint; 40 | 41 | REAL bendingEnergy; 42 | REAL shearEnergy; 43 | REAL translationalEnergy; 44 | REAL rotationalEnergy; 45 | REAL totalInternalEnergy; 46 | 47 | #ifdef SNAKE_VIZ 48 | void _paint(Rod *snake); 49 | #endif 50 | 51 | public: 52 | Polymer(PolymerIntegrator *pint) 53 | : startTimeStats(0.0), 54 | endTimeStats(0.0), 55 | avgVel(vector()), 56 | pint(pint) { 57 | rodptrs = pint->getRods(); 58 | efptrs = pint->getExternalForces(); 59 | bcptrs = pint->getBoundaryConditions(); 60 | interptrs = pint->getInteractions(); 61 | 62 | numRods = (int)rodptrs.size(); 63 | avgVel = vector(numRods); 64 | // computeEnergies(); 65 | } 66 | 67 | bool simulate(const REAL simulationTime, const REAL dt0, 68 | const REAL diagPerUnitTime, const REAL povrayFramesPerUnitTime, 69 | const string diagnostics, 70 | const string integrationType = "VELOCITY_VERLET_2ND", 71 | const REAL CFL = 0.01); 72 | bool nanCheck(); 73 | void computeEnergies(); 74 | void printEnergies(const int step, const REAL time); 75 | void printX(const int step, const REAL time, const string outfilename); 76 | void printXV(const int step, const REAL time, const string outfilename); 77 | void print_s_internalTorques(const string outfilename); 78 | void print_s_coordinates(const string outfilename); 79 | void print_s_internalShears(const string outfilename); 80 | void print_s_curvatures(const string outfilename); 81 | void setWindowStats(const REAL _startTimeStats, const REAL _endTimeStats) { 82 | startTimeStats = _startTimeStats; 83 | endTimeStats = _endTimeStats; 84 | }; 85 | 86 | REAL getTotalBendingEnergy() { return bendingEnergy; } 87 | REAL getTotalTranslationalEnergy() { return translationalEnergy; } 88 | REAL getTotalEnergy() { return totalInternalEnergy; } 89 | REAL getTotalRotationalEnergy() { return rotationalEnergy; } 90 | vector getAverageVelocity() { return avgVel; } 91 | }; 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /source/PolymerIntegrator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PolymerIntegrator.cpp 3 | * 4 | * Created on: Jun 20, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #include "PolymerIntegrator.h" 9 | 10 | void PolymerIntegrator::_computeForces(const REAL time, const int step) { 11 | const unsigned int numRods = rodptrs.size(); 12 | 13 | for (unsigned int j = 0; j < numRods; j++) rodptrs[j]->reset(); 14 | 15 | for (unsigned int j = 0; j < numRods; j++) { 16 | rodptrs[j]->computeAllInternalResultingForcesAndTorques(); // (in-place) 17 | 18 | // if(time>0.2){ 19 | efptrs[j]->applyForces(*rodptrs[j], time); 20 | //} // make sure that external forces are in-place 21 | } 22 | 23 | // for(unsigned int j=0; japplyForces( 27 | time); // make sure that interaction forces are in-place 28 | #endif 29 | // Compute Rod-Rod connection and collision force 30 | #if defined FLAGELBOW || FLAGFLAGELLA || FLAGWALKER || FLAGMUSCULARSNAKE || \ 31 | FLAGWING 32 | ecptrs[0]->RodRodAttach(time); 33 | #endif 34 | // Walker case only rod collision check 35 | #ifdef FLAGWALKER 36 | ecptrs[0]->RodRodCollision_Simple(time); 37 | #endif 38 | // For simple toy example of rod connections 39 | #if defined FLAGSPHERICALJOINT || FLAGHINGEJOINT || FLAGFIXEDJOINT || \ 40 | FLAGPULLINGMUSCLE 41 | scptrs[0]->RodRodSimpleConnection(time); 42 | #endif 43 | 44 | for (unsigned int j = 0; j < numRods; j++) rodptrs[j]->applyStaticFrictions(); 45 | 46 | for (unsigned int j = 0; j < numRods; j++) { 47 | // Sum up internal and external forces 48 | // Next line is equivalent (but in-place) to: rodptrs[j]->totalForces = 49 | // rodptrs[j]->totalInternalForces + rodptrs[j]->externalForces; 50 | v_a_plus_b_equal_c(rodptrs[j]->totalInternalForces, 51 | rodptrs[j]->externalForces, rodptrs[j]->totalForces); 52 | 53 | // Sum up internal and external torques 54 | // Next line is equivalent (but in-place) to: rodptrs[j]->totalTorques = 55 | // rodptrs[j]->totalInternalTorques + rodptrs[j]->externalTorques; 56 | v_a_plus_b_equal_c(rodptrs[j]->totalInternalTorques, 57 | rodptrs[j]->externalTorques, rodptrs[j]->totalTorques); 58 | } 59 | } 60 | 61 | void PolymerIntegrator::_computeAccelerations(const REAL time, const int step) { 62 | const REAL dt = time - time_old; 63 | assert(dt > Tolerance::tol()); 64 | time_old = time; 65 | 66 | _computeForces(time, step); // in-place 67 | 68 | for (unsigned int j = 0; j < rodptrs.size(); j++) { 69 | // Compute linear accelerations given the forces (internal+ezternal) acting 70 | // on the single rod elements 71 | v_a_divide_b_equal_c( 72 | rodptrs[j]->totalForces, rodptrs[j]->m, 73 | rodptrs[j]->a); // rodptrs[j]->a = rodptrs[j]->totalForces / 74 | // rodptrs[j]->m; Note that m = rho*A0*dS = const 75 | 76 | // Compute angullar accelerations given the toques (internal+ezternal) 77 | // acting on the single rod elements 78 | rodptrs[j]->deldt = (rodptrs[j]->e - rodptrs[j]->e_old) / dt; 79 | rodptrs[j]->wDot = 80 | rodptrs[j]->e * (rodptrs[j]->J0inv * rodptrs[j]->totalTorques) + 81 | rodptrs[j]->w * rodptrs[j]->deldt / rodptrs[j]->e; 82 | 83 | // v_a_times_b_equal_c(rodptrs[j]->J0inv, rodptrs[j]->totalTorques, 84 | // rodptrs[j]->wDot); // rodptrs[j]->wDot = rodptrs[j]->I0inv * 85 | // rodptrs[j]->totalTorques; 86 | } 87 | } 88 | 89 | void PolymerIntegrator::_v_update_Q(const REAL coeffDt, Rod *rod) { 90 | // Next lines are equivalent (but in-place) to: rodptrs[j]->Q = vExp(0.5 * dt 91 | // * rodptrs[j]->w) * rodptrs[j]->Q; 92 | v_a_times_b_equal_c(rod->w, coeffDt, rod->tempVV3_n); // in-place 93 | vExp(rod->tempVV3_n, rod->tempVM3_n); // in-place 94 | v_timesequal(rod->tempVM3_n, rod->Q); // in-place 95 | // cout << "ReachQ"<< endl; 96 | } 97 | 98 | void PolymerIntegrator::_v_update_x(const REAL coeffDt, Rod *rod) { 99 | // Next lines are equivalent (but in-place) to: rodptrs[j]->x += coeffDt * 100 | // rodptrs[j]->v; 101 | 102 | v_plusequal_a_times_b(coeffDt, rod->v, rod->x); 103 | // cout << "Reach1"<< endl; 104 | } 105 | 106 | void PolymerIntegrator::_v_update_v(const REAL coeffDt, Rod *rod) { 107 | // Next line is equivalent (but in-place) to: rodptrs[j]->v += coeffDt * 108 | // rodptrs[j]->a; 109 | v_plusequal_a_times_b(coeffDt, rod->a, rod->v); 110 | // cout << "Reach2"<< endl; 111 | } 112 | 113 | void PolymerIntegrator::_v_update_w(const REAL coeffDt, Rod *rod) { 114 | // Next line is equivalent (but in-place) to: rodptrs[j]->w += coeffDt * 115 | // rodptrs[j]->wDot; 116 | v_plusequal_a_times_b(coeffDt, rod->wDot, rod->w); 117 | // cout << "Reach3"<< endl; 118 | } 119 | -------------------------------------------------------------------------------- /source/PolymerIntegrator.h: -------------------------------------------------------------------------------- 1 | #ifndef POLYMERINTEGRATOR_H_ 2 | #define POLYMERINTEGRATOR_H_ 3 | 4 | #include "ExternalContact.h" 5 | #include "Interaction.h" 6 | #include "MRAGProfiler.h" 7 | #include "Matrix3.h" 8 | #include "Rod.h" 9 | #include "RodBoundaryConditions.h" 10 | #include "RodExternalForces.h" 11 | #include "SimpleConnection.h" 12 | #include "SpeedFunctions.h" 13 | #include "Vector3.h" 14 | 15 | // Symplectic integrator for a Polymer 16 | class PolymerIntegrator { 17 | protected: 18 | typedef std::vector VV3; 19 | typedef std::vector VM3; 20 | typedef std::vector VREAL; 21 | typedef std::vector VBOOL; 22 | typedef std::vector VINT; 23 | typedef std::vector Vrodptr; 24 | typedef std::vector Vefptr; 25 | typedef std::vector Vbcptr; 26 | typedef std::vector Vinterptr; 27 | typedef std::vector Vecptr; 28 | typedef std::vector Vscptr; 29 | 30 | REAL time_old; 31 | 32 | Vrodptr rodptrs; 33 | Vefptr efptrs; 34 | Vbcptr bcptrs; 35 | Vinterptr interptrs; 36 | Vecptr ecptrs; 37 | Vscptr scptrs; 38 | 39 | void _computeForces(const REAL time, const int step); 40 | void _computeAccelerations(const REAL time, const int step); 41 | void _v_update_Q(const REAL coeffDt, Rod *rod); 42 | void _v_update_x(const REAL coeffDt, Rod *rod); 43 | void _v_update_v(const REAL coeffDt, Rod *rod); 44 | void _v_update_w(const REAL coeffDt, Rod *rod); 45 | 46 | bool startforce = false; 47 | 48 | public: 49 | PolymerIntegrator(Vrodptr &_rodptrs, Vefptr &_efptrs, Vbcptr &_bcptrs, 50 | Vinterptr &_interptrs, Vecptr &_ecptrs, Vscptr &_scptrs) 51 | : time_old(0.0), 52 | rodptrs(_rodptrs), 53 | efptrs(_efptrs), 54 | bcptrs(_bcptrs), 55 | interptrs(_interptrs), 56 | ecptrs(_ecptrs), 57 | scptrs(_scptrs) {} 58 | virtual ~PolymerIntegrator() {} 59 | 60 | inline Vrodptr &getRods() { return rodptrs; } 61 | inline Vefptr &getExternalForces() { return efptrs; } 62 | inline Vbcptr &getBoundaryConditions() { return bcptrs; } 63 | inline Vinterptr &getInteractions() { return interptrs; } 64 | 65 | virtual REAL integrate(const REAL time, const REAL dt, const int step) = 0; 66 | }; 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /source/PositionVerlet2nd.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PositionVerlet2nd.cpp 3 | * 4 | * Created on: Feb 5, 2015 5 | * Author: mgazzola 6 | */ 7 | 8 | #include "PositionVerlet2nd.h" 9 | 10 | REAL PositionVerlet2nd::integrate(const REAL time, const REAL dt, 11 | const int step) { 12 | // Position Verlet 13 | // 1. x(t+dt/2) = x(t) + 0.5*dt*v(t) 14 | // 2. v(t+dt) = v(t) + dt*a(t+dt/2) 15 | // 3. x(t+dt) = x(t+dt/2) + 0.5*dt*v(t+dt) 16 | 17 | // Get the number of rods in the system 18 | const unsigned int numRods = rodptrs.size(); 19 | 20 | // Set dt 21 | for (unsigned int j = 0; j < numRods; j++) (*rodptrs[j]).setDt(dt); 22 | 23 | // Enforce boundary conditions (position+velocity) 24 | if (time == 0.0) { 25 | for (unsigned int j = 0; j < numRods; j++) { 26 | (*rodptrs[j]).setTime(time); 27 | (*bcptrs[j]).neumann(*rodptrs[j], step, dt, time); 28 | (*bcptrs[j]).dirichlet(*rodptrs[j], step, dt, time); 29 | } 30 | } 31 | 32 | // Compute half step position: x(t+dt/2) = x(t) + 0.5*dt*v(t) 33 | for (unsigned int j = 0; j < numRods; j++) { 34 | _v_update_x( 35 | 0.5 * dt, 36 | rodptrs[j]); // (in-place) rodptrs[j]->x += 0.5 * dt * rodptrs[j]->v; 37 | _v_update_Q(0.5 * dt, 38 | rodptrs[j]); // (in-place) rodptrs[j]->Q = vExp(0.5 * dt * 39 | // rodptrs[j]->w) * rodptrs[j]->Q; 40 | 41 | // Enforce boundary conditions for position 42 | (*rodptrs[j]).setTime(time + 0.5 * dt); 43 | (*bcptrs[j]).dirichlet(*rodptrs[j], step, dt, time + 0.5 * dt); 44 | } 45 | 46 | // Compute half step acceleration: a(t+dt/2) 47 | { 48 | for (unsigned int j = 0; j < numRods; j++) rodptrs[j]->update(time); 49 | 50 | _computeAccelerations(time + 0.5 * dt, step); 51 | } 52 | 53 | // Compute final velocity: v(t+dt) = v(t) + dt*a(t+dt/2) 54 | for (unsigned int j = 0; j < numRods; j++) { 55 | _v_update_v(dt, 56 | rodptrs[j]); // (in-place) rodptrs[j]->v += dt * rodptrs[j]->a; 57 | _v_update_w( 58 | dt, rodptrs[j]); // (in-place) rodptrs[j]->w += dt * rodptrs[j]->wDot; 59 | 60 | // Enforce boundary conditions for velocity 61 | (*rodptrs[j]).setTime(time + dt); 62 | (*bcptrs[j]).neumann(*rodptrs[j], step, dt, time + dt); 63 | } 64 | // cout << "Reach4"<< endl; 65 | // Compute final positions: x(t+dt) = x(t+dt/2) + 0.5*dt*v(t+dt) 66 | for (unsigned int j = 0; j < numRods; j++) { 67 | _v_update_x( 68 | 0.5 * dt, 69 | rodptrs[j]); // (in-place) rodptrs[j]->x += 0.5 * dt * rodptrs[j]->v; 70 | _v_update_Q(0.5 * dt, 71 | rodptrs[j]); // (in-place) rodptrs[j]->Q = vExp(0.5 * dt * 72 | // rodptrs[j]->w) * rodptrs[j]->Q; 73 | 74 | // Enforce boundary conditions for position 75 | (*bcptrs[j]).dirichlet(*rodptrs[j], step, dt, time + dt); 76 | } 77 | 78 | return dt; 79 | } 80 | -------------------------------------------------------------------------------- /source/PositionVerlet2nd.h: -------------------------------------------------------------------------------- 1 | /* 2 | * PositionVerlet2nd.h 3 | * 4 | * Created on: Feb 5, 2015 5 | * Author: mgazzola 6 | */ 7 | 8 | #ifndef POSITIONVERLET2ND_H_ 9 | #define POSITIONVERLET2ND_H_ 10 | 11 | #include "PolymerIntegrator.h" 12 | 13 | class PositionVerlet2nd : public PolymerIntegrator { 14 | public: 15 | PositionVerlet2nd(Vrodptr &_rodptrs, Vefptr &_efptrs, Vbcptr &_bcptrs, 16 | Vinterptr &_interptrs, Vecptr &_ecptrs, Vscptr &_scptrs) 17 | : PolymerIntegrator(_rodptrs, _efptrs, _bcptrs, _interptrs, _ecptrs, 18 | _scptrs){}; 19 | 20 | virtual ~PositionVerlet2nd(){}; 21 | 22 | virtual REAL integrate(const REAL time, const REAL dt, const int step); 23 | }; 24 | 25 | #endif /* POSITIONVERLET2ND_H_ */ 26 | -------------------------------------------------------------------------------- /source/PullingMuscle.cpp: -------------------------------------------------------------------------------- 1 | #include "PullingMuscle.h" 2 | 3 | /* 4 | This example demonstrates how we connect two Cosserat rods together using an 5 | active muscle along their centers like so: 6 | 7 | // clang-format off 8 | 9 | Hinge joint Hinge joint 10 | (x) (x) 11 | || || 12 | || Muscle || 13 | || ++++++++ || 14 | Rod1||++++++++++++|| Rod2 15 | || ++++++++ || 16 | || || 17 | || || 18 | 19 | 20 | ^ z 21 | | 22 | | 23 | o — — —> y 24 | / 25 | / 26 | x 27 | 28 | // clang-format on 29 | 30 | No other forces (like gravity) are considered. 31 | 32 | The rods are attached to the ground via a hinge joint, which only allows 33 | relative orientation changes in the x-z plane (no displacement of the rod 34 | connected point is allowed). The muscle connecting them is initially in a 35 | relaxed configuration. The muscle is then activated and so it starts 36 | contracting, exerting forces on the rod COM. This muscle force causes a torque 37 | about the hinge joint, which causes the rods to rotate in the y-z plane. After 38 | reaching maximal contraction, the muscle starts relaxing and reaches its 39 | original configuration, as a result of which the rods are returned to their 40 | initial configuration as well. This contraction-relaxation cycles is repeated 41 | sinusoidally. This example is instructive in showing how muscles can be 42 | implemented using our Cosserat rod model. 43 | 44 | 45 | */ 46 | PullingMuscle::PullingMuscle(const int argc, const char **argv) 47 | : amp(0.0), w(0.0), v(0.0) {} 48 | 49 | // Units in this case are mm/g/s 50 | 51 | vector PullingMuscle::_pullingmuscleRun() { 52 | vector rodPtrs; 53 | 54 | // Dumping frequencies (number of frames/dumps per unit time) 55 | const REAL diagPerUnitTime = 120; 56 | const REAL povrayPerUnitTime = 50; 57 | const REAL dt = 1e-6; 58 | const REAL timeSimulation = (10.0); 59 | 60 | //----------------------------------------------------------------------------------------------------------------- 61 | // Define Links 62 | // Driving parameters 63 | // Link One shape 64 | const int n = 10; 65 | const REAL density = 1.75e-3; // 1.75g/cm^3 66 | const REAL L0 = 200.0; 67 | const REAL r0 = 7.0; 68 | const REAL E = 3e7; 69 | const REAL totalMass = density * M_PI * r0 * r0 * L0; 70 | 71 | const REAL dL0 = L0 / (double)n; // length of cross-section element 72 | const REAL poissonRatio = 0.5; // Incompressible 73 | const REAL G = E / (poissonRatio + 1.0); 74 | 75 | // Define rod 76 | const Vector3 Linkdirection = Vector3(0.0, 0.0, -1.0); 77 | const Vector3 Linknormal = Vector3(1.0, 0.0, 0.0); 78 | const Vector3 Linkoneorigin = Vector3(0.0, L0, 1.5 * L0); 79 | const Vector3 Linktwoorigin = Linkoneorigin + L0 * Vector3(0.0, -1.0, 0.0); 80 | 81 | // Second moment of area for disk cross section 82 | const REAL A0 = M_PI * r0 * r0; 83 | const REAL I0_1 = A0 * A0 / (4.0 * M_PI); 84 | const REAL I0_2 = I0_1; 85 | const REAL I0_3 = 2.0 * I0_1; 86 | const Matrix3 I0 = Matrix3(I0_1, 0.0, 0.0, 0.0, I0_2, 0.0, 0.0, 0.0, I0_3); 87 | // Mass inertia matrix for disk cross section 88 | const Matrix3 J0 = density * dL0 * I0; 89 | 90 | // Bending matrix 91 | Matrix3 B0 = 92 | Matrix3(E * I0_1, 0.0, 0.0, 0.0, E * I0_2, 0.0, 0.0, 0.0, G * I0_3); 93 | // Shear matrix 94 | Matrix3 S0 = Matrix3((4.0 / 3.0) * G * A0, 0.0, 0.0, 0.0, 95 | (4.0 / 3.0) * G * A0, 0.0, 0.0, 0.0, E * A0); 96 | 97 | // Initialize straight rod and pack it into a vector of pointers to rod 98 | const REAL initialTotalTwist = 0.0; 99 | const REAL nu = 0.1; 100 | const REAL relaxationNu = 0.0; 101 | const bool useSelfContact = false; 102 | 103 | Rod *rod1 = RodInitialConfigurations::straightRod( 104 | n, totalMass, r0, J0, B0, S0, L0, initialTotalTwist, Linkoneorigin, 105 | Linkdirection, Linknormal, nu, relaxationNu, useSelfContact); 106 | rodPtrs.push_back(rod1); 107 | rod1->update(0.0); 108 | Rod *rod2 = RodInitialConfigurations::straightRod( 109 | n, totalMass, r0, J0, B0, S0, L0, initialTotalTwist, Linktwoorigin, 110 | Linkdirection, Linknormal, nu, relaxationNu, useSelfContact); 111 | rodPtrs.push_back(rod2); 112 | rod2->update(0.0); 113 | 114 | //----------------------------------------------------------------------------------------------------------------- 115 | // Define Soft Contracting Element 116 | // two ends are stiff --- representing tendons 117 | // middle elements are soft --- representing muscle 118 | // Driving parameters 119 | const int nm = 10; 120 | const REAL densitym = 1.06e-3; 121 | const REAL Em = 1e4; 122 | const REAL Et = 5e8; 123 | 124 | const REAL poissonRatiom = 0.5; // Incompressible 125 | const REAL Gm = Em / (poissonRatiom + 1.0); 126 | const REAL Gt = Et / (poissonRatiom + 1.0); 127 | 128 | Vector3 originmuscle = rodPtrs[0]->x[6]; 129 | Vector3 directionmuscle = (rodPtrs[1]->x[6] - rodPtrs[0]->x[6]).unitize(); 130 | Vector3 normalmuscle = Vector3(1.0, 0.0, 0.0); 131 | 132 | REAL Lm = (rodPtrs[1]->x[6] - rodPtrs[0]->x[6]).length(); 133 | REAL dLm = Lm / (double)nm; 134 | 135 | // geometrical properties of tendons 136 | // Second moment of area for disk cross section for tendons 137 | const REAL rm_t = 6.0; 138 | const REAL Am_t = M_PI * rm_t * rm_t; 139 | const REAL I0_1m_t = Am_t * Am_t / (4.0 * M_PI); 140 | const REAL I0_2m_t = I0_1m_t; 141 | const REAL I0_3m_t = 2.0 * I0_1m_t; 142 | const Matrix3 I0m_t = 143 | Matrix3(I0_1m_t, 0.0, 0.0, 0.0, I0_2m_t, 0.0, 0.0, 0.0, I0_3m_t); 144 | 145 | // Mass inertia matrix for disk cross section 146 | const Matrix3 Jm_t = densitym * dLm * I0m_t; 147 | 148 | // Bending matrix 149 | const Matrix3 Bm_t = Matrix3(Et * I0_1m_t, 0.0, 0.0, 0.0, Et * I0_2m_t, 0.0, 150 | 0.0, 0.0, Gt * I0_3m_t); 151 | 152 | // Shear matrix 153 | const Matrix3 Sm_t = 154 | Matrix3((4.0 / 3.0) * Gt * Am_t, 0.0, 0.0, 0.0, (4.0 / 3.0) * Gt * Am_t, 155 | 0.0, 0.0, 0.0, Et * Am_t); 156 | 157 | // geometrical properties of muscle 158 | // Second moment of area for disk cross section for muscle 159 | const REAL rm = 12.0; 160 | const REAL Am = M_PI * rm * rm; 161 | const REAL I0_1m = Am * Am / (4.0 * M_PI); 162 | const REAL I0_2m = I0_1m; 163 | const REAL I0_3m = 2.0 * I0_1m; 164 | const Matrix3 I0m = 165 | Matrix3(I0_1m, 0.0, 0.0, 0.0, I0_2m, 0.0, 0.0, 0.0, I0_3m); 166 | // Mass inertia matrix for disk cross section 167 | const Matrix3 Jm = densitym * dLm * I0m; 168 | 169 | // Bending matrix 170 | const Matrix3 Bm = 171 | Matrix3(Em * I0_1m, 0.0, 0.0, 0.0, Em * I0_2m, 0.0, 0.0, 0.0, Gm * I0_3m); 172 | // Shear matrix 173 | const Matrix3 Sm = Matrix3((4.0 / 3.0) * Gm * Am, 0.0, 0.0, 0.0, 174 | (4.0 / 3.0) * Gm * Am, 0.0, 0.0, 0.0, Em * Am); 175 | 176 | vector rm_v = vector(nm); //(mm) 177 | vector Jm_v = vector(nm); 178 | vector Bm_v = vector(nm - 1); 179 | vector Sm_v = vector(nm); 180 | 181 | // tendon 182 | for (unsigned int i = 0; i < 3; i++) { 183 | rm_v[i] = rm_t; 184 | Jm_v[i] = Jm_t; 185 | Bm_v[i] = Bm_t; 186 | Sm_v[i] = Sm_t; 187 | } 188 | Bm_v[2] = (Bm_t + Bm) / 2; 189 | 190 | for (unsigned int i = 3; i < 7; i++) { 191 | rm_v[i] = rm; 192 | Jm_v[i] = Jm; 193 | Bm_v[i] = Bm; 194 | Sm_v[i] = Sm; 195 | } 196 | Bm_v[6] = (Bm_t + Bm) / 2; 197 | // muscle SP-SP 198 | for (unsigned int i = 7; i < 10; i++) { 199 | rm_v[i] = rm_t; 200 | Jm_v[i] = Jm_t; 201 | if (i < 9) Bm_v[i] = Bm_t; 202 | Sm_v[i] = Sm_t; 203 | } 204 | 205 | // Initialize straight rod and pack it into a vector of pointers to rod 206 | const REAL initialTotalTwist_m = 0.0; 207 | const REAL nu_m = 10.0; 208 | const REAL relaxationNu_m = 0.0; 209 | const bool useSelfContact_m = false; 210 | 211 | Rod *rod_m = RodInitialConfigurations::straightRod_v( 212 | nm, densitym, rm_v, Jm_v, Bm_v, Sm_v, Lm, initialTotalTwist_m, 213 | originmuscle, directionmuscle, normalmuscle, nu_m, relaxationNu_m, 214 | useSelfContact_m); 215 | rodPtrs.push_back(rod_m); 216 | rod_m->update(0.0); 217 | //----------------------------------------------------------------------------------------------------------------- 218 | // Pack boundary conditions 219 | vector boundaryConditionsPtrs; 220 | // Link One -- first node pinned 221 | HingeBC hinge1 = HingeBC(rodPtrs[0]); 222 | boundaryConditionsPtrs.push_back(&hinge1); 223 | // Link Two -- first node pinned 224 | HingeBC hinge2 = HingeBC(rodPtrs[1]); 225 | boundaryConditionsPtrs.push_back(&hinge2); 226 | // Contracting element 227 | FreeBC freeBC = FreeBC(); 228 | boundaryConditionsPtrs.push_back(&freeBC); 229 | 230 | // Pack all forces together (no forces applied) 231 | vector externalForcesPtrs; 232 | 233 | // Gravity 234 | MultipleForces multipleForces1; 235 | GravityForce gravity = GravityForce(Vector3(0.0, 0.0, 0.0)); 236 | multipleForces1.add(&gravity); 237 | MultipleForces *multipleForcesPtr1 = multipleForces1.get(); 238 | for (unsigned int i = 0; i < 3; i++) { 239 | externalForcesPtrs.push_back(multipleForcesPtr1); 240 | } 241 | 242 | vector substrateInteractionsPtrs; 243 | 244 | // Set up External Contact -- This is for the five cases in the paper, not 245 | // used in this case 246 | vector> attachpoint; 247 | vector externalcontactPtrs; 248 | /* The second and third argument are unimportant, but 249 | are preserved here for legacy purposes. Hence we simply 250 | set it to 0.0 251 | */ 252 | ExternalContact externalcontact = 253 | ExternalContact(rodPtrs, 0.0, 0.0, attachpoint); 254 | externalcontactPtrs.push_back(&externalcontact); 255 | 256 | // Set up Simple Connection 257 | vector simpleconnectionPtrs; 258 | SimpleConnection simpleconnection = SimpleConnection(rodPtrs); 259 | simpleconnectionPtrs.push_back(&simpleconnection); 260 | //----------------------------------------------------------------------------------------------------------------- 261 | // Set up integrator (define integration order) 262 | PolymerIntegrator *integrator = new PositionVerlet2nd( 263 | rodPtrs, externalForcesPtrs, boundaryConditionsPtrs, 264 | substrateInteractionsPtrs, externalcontactPtrs, simpleconnectionPtrs); 265 | 266 | // Instantiate simulator 267 | Polymer poly = Polymer(integrator); 268 | 269 | // I am goint go collect data over this time window 270 | poly.setWindowStats(1.0, 2.0); 271 | 272 | // Run simulation 273 | string outfileName = string("prova"); 274 | const bool goodRun = poly.simulate(timeSimulation, dt, diagPerUnitTime, 275 | povrayPerUnitTime, outfileName); 276 | 277 | // Throw exception if something went wrong 278 | if (!goodRun) 279 | throw "not good run in localized helical buckling, what is going on?"; 280 | 281 | const vector avgVel = poly.getAverageVelocity(); 282 | 283 | vector fwdAvgVel; 284 | for (unsigned int i = 0; i < 1; i++) { 285 | fwdAvgVel.push_back(avgVel[i] % Linkdirection); 286 | } 287 | const vector fitness = fwdAvgVel; 288 | 289 | return (fitness); 290 | } 291 | 292 | void PullingMuscle::run() { 293 | const vector fitness = _pullingmuscleRun(); 294 | exit(0); 295 | } 296 | -------------------------------------------------------------------------------- /source/PullingMuscle.h: -------------------------------------------------------------------------------- 1 | /* 2 | * PullingMuscle.h 3 | * 4 | */ 5 | 6 | #ifndef PULLINGMUSCLE_H_ 7 | #define PULLINGMUSCLE_H_ 8 | 9 | #include "ArgumentParser.h" 10 | #include "Polymer.h" 11 | #include "PositionVerlet2nd.h" 12 | #include "RodInitialConfigurations.h" 13 | #include "Test.h" 14 | #include "UsualHeaders.h" 15 | 16 | using namespace std; 17 | 18 | class PullingMuscle : public Test { 19 | protected: 20 | vector amp; 21 | double w, v; 22 | int NOR; 23 | 24 | vector _pullingmuscleRun(); 25 | 26 | #ifdef SNAKE_VIZ 27 | void _paint(){}; 28 | #endif 29 | 30 | public: 31 | PullingMuscle(const int argc, const char **argv); 32 | ~PullingMuscle(){}; 33 | 34 | void run(); 35 | void paint(){}; 36 | }; 37 | 38 | #endif /* PULLINGMUSCLE_H_ */ 39 | -------------------------------------------------------------------------------- /source/QuasistaticTimoshenkoBeam.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "QuasistaticTimoshenkoBeam.h" 3 | 4 | /* 5 | This code simulates a cantilevered slender beam (clamped at the wall at one end, 6 | free at another) bending under the influence of a downward point force like so: 7 | 8 | // clang-format off 9 | 10 | Wall 11 | /| | Force 12 | /| | 13 | /| v 14 | /|================================== Rod/Beam 15 | /| 16 | /| 17 | /| 18 | 19 | // clang-format on 20 | 21 | Any other effects like gravity are ignored. In our case, where the shear 22 | modulus (G) is comparable to the Young's modulus (E), the static deflection of 23 | the beam centerline is given by the Timoshenko bending solution (see Gazzola et 24 | al, 2018). 25 | 26 | In the code below, we consider a Cosserat rod and clamp it at the end 27 | using FixedBC (applies forces and torques). We time-step the code until it 28 | reaches a steady-state (or in this case a quasi-steady state). A comparison of 29 | the obtained solution with the theoretical results show a good match. For more 30 | details the reader is referred to "Forward and inverse problems in the mechanics 31 | of soft filaments" by Gazzola et.al, RSoS, 2018. This example is instructive in 32 | showing how a rod, along with simple boundary conditions can be coded up in the 33 | framework of Elastica. 34 | 35 | */ 36 | 37 | QuasistaticTimoshenkoBeam::QuasistaticTimoshenkoBeam(const int argc, 38 | const char **argv) {} 39 | 40 | QuasistaticTimoshenkoBeam::~QuasistaticTimoshenkoBeam() {} 41 | 42 | bool QuasistaticTimoshenkoBeam::_test(const int nEdges, const REAL _dt, 43 | const REAL _L, const REAL _r, 44 | const REAL _P, const REAL _timeSimulation, 45 | const REAL _E, const REAL _G, 46 | const REAL _rho, const REAL _nu, 47 | const REAL _relaxationNu, 48 | const string outfileName) { 49 | // Input parameters 50 | const int n = nEdges; // number of discretization edges (i.e. n+1 points) 51 | // along the entire rod 52 | const REAL timeSimulation = _timeSimulation; // total simulation time 53 | const REAL dt = _dt; // time step 54 | const REAL P = _P; 55 | const REAL L0 = _L; // total length of rod [m] 56 | const REAL r0 = _r; // radius [m] 57 | const REAL density = _rho; // [kg/m^3] 58 | const REAL E = _E; // GPa --> rubber~0.01-0.1Gpa, iron~200Gpa 59 | const REAL G = _G; // GPa --> rubber~0.01-0.1Gpa, iron~200Gpa 60 | const REAL nu = _nu; // Numerical damping viscosity [m^2/s] 61 | const REAL relaxationNu = 62 | _relaxationNu; // relaxation time for exponential decay of nu 63 | 64 | // Dumping frequencies (number of frames/dumps per unit time) 65 | const unsigned int diagPerUnitTime = 5; 66 | const unsigned int povrayPerUnitTime = 0; 67 | 68 | // Physical parameters 69 | const REAL dL0 = L0 / (double)n; // length of cross-section element 70 | const REAL A0 = M_PI * r0 * r0; 71 | const REAL Vol = A0 * L0; 72 | const REAL totalMass = Vol * density; 73 | const REAL initialTotalTwist = 0.0; 74 | const Vector3 originRod = Vector3(0.0, 0.0, 0.0); 75 | const Vector3 directionRod = Vector3(1.0, 0.0, 0.0); 76 | const Vector3 normalRod = Vector3(0.0, 0.0, 1.0); 77 | 78 | // Second moment of area for disk cross section 79 | const REAL I0_1 = A0 * A0 / (4.0 * M_PI); 80 | const REAL I0_2 = I0_1; 81 | const REAL I0_3 = 2.0 * I0_1; 82 | const Matrix3 I0 = Matrix3(I0_1, 0.0, 0.0, 0.0, I0_2, 0.0, 0.0, 0.0, I0_3); 83 | 84 | // Mass inertia matrix for disk cross section 85 | const Matrix3 J0 = density * dL0 * I0; 86 | 87 | // Bending matrix (TOD: change this is wrong!!) 88 | Matrix3 B0 = 89 | Matrix3(E * I0_1, 0.0, 0.0, 0.0, E * I0_2, 0.0, 0.0, 0.0, G * I0_3); 90 | 91 | // Shear matrix 92 | Matrix3 S0 = Matrix3((4.0 / 3.0) * G * A0, 0.0, 0.0, 0.0, 93 | (4.0 / 3.0) * G * A0, 0.0, 0.0, 0.0, E * A0); 94 | 95 | // Initialize straight rod and pack it into a vector of pointers to rod --> 96 | // Use linear load-strain (hence the true flag at the end)!!! 97 | const bool useSelfContact = false; 98 | Rod *rod = RodInitialConfigurations::straightRod( 99 | n, totalMass, r0, J0, B0, S0, L0, initialTotalTwist, originRod, 100 | directionRod, normalRod, nu, relaxationNu, useSelfContact); 101 | vector rodPtrs; 102 | rodPtrs.push_back(rod); 103 | rod->update(0.0); 104 | rod->computeEnergies(); 105 | 106 | // Pack boundary conditions 107 | TimoshenkoBeamBC endBC = TimoshenkoBeamBC(rodPtrs); 108 | vector boundaryConditionsPtrs; 109 | boundaryConditionsPtrs.push_back(&endBC); 110 | 111 | // Pack all forces together 112 | vector externalForcesPtrs; 113 | GradualEndpointForces endpointsForce = 114 | GradualEndpointForces(Vector3(), Vector3(0, -P, 0), timeSimulation / 2); 115 | MultipleForces multipleForces; 116 | multipleForces.add(&endpointsForce); 117 | MultipleForces *multipleForcesPtr = multipleForces.get(); 118 | externalForcesPtrs.push_back(multipleForcesPtr); 119 | 120 | // Empty interaction forces (no substrate in this case) 121 | vector substrateInteractionsPtrs; 122 | 123 | // No external contact function needed 124 | vector> attachpoint; 125 | vector externalcontactPtrs; 126 | ExternalContact externalcontact = 127 | ExternalContact(rodPtrs, 0.0, 0.0, attachpoint); 128 | externalcontactPtrs.push_back(&externalcontact); 129 | 130 | // No Simple Connection needed 131 | vector simpleconnectionPtrs; 132 | SimpleConnection simpleconnection = SimpleConnection(rodPtrs); 133 | simpleconnectionPtrs.push_back(&simpleconnection); 134 | //----------------------------------------------------------------------------------------------------------------- 135 | // Set up time integrator 136 | PolymerIntegrator *integrator = new PositionVerlet2nd( 137 | rodPtrs, externalForcesPtrs, boundaryConditionsPtrs, 138 | substrateInteractionsPtrs, externalcontactPtrs, simpleconnectionPtrs); 139 | 140 | // Simulate 141 | Polymer poly = Polymer(integrator); 142 | const bool goodRun = poly.simulate(timeSimulation, dt, diagPerUnitTime, 143 | povrayPerUnitTime, outfileName); 144 | 145 | // Throw exception if something went wrong 146 | if (!goodRun) 147 | throw "not good run in localized helical buckling, what is going on?"; 148 | 149 | // Dump post buckling helical shape 150 | { 151 | string rodShape = outfileName + "_shape.txt"; 152 | FILE *fitnessFile = fopen(rodShape.c_str(), "w"); 153 | assert(fitnessFile != NULL); 154 | 155 | for (unsigned int i = 0; i < rod->x.size(); i++) { 156 | const REAL s_hat = rod->x[i].x; 157 | /* Timoshenko theory analytical solution for shear hardened rod 158 | (set in _longWaveTest below) 159 | */ 160 | const REAL yposition = -P / (4.0 / 3.0 * A0 * G) * s_hat - 161 | P * L0 / (2 * E * I0_1) * s_hat * s_hat + 162 | P / (6 * E * I0_1) * s_hat * s_hat * s_hat; 163 | /* Euler-Bernoulli analytical solution for shear hardened rod 164 | (set in _longWaveTest below) 165 | */ 166 | // const REAL yposition = -P * L0 / (2 * E * I0_1) * s_hat * s_hat + 167 | // P / (6 * E * I0_1) * s_hat * s_hat * s_hat; 168 | fprintf(fitnessFile, "%1.15e %1.15e %1.15e %1.15e %1.15e %1.15e\n", 169 | rod->x[i].x, rod->x[i].y, rod->x[i].z, rod->x[i].x, yposition, 170 | rod->x[i].z); 171 | } 172 | 173 | fclose(fitnessFile); 174 | } 175 | 176 | cout << "total internal energy = " << poly.getTotalEnergy() << endl; 177 | cout << "total translational energy = " << poly.getTotalTranslationalEnergy() 178 | << endl; 179 | cout << "total rotational energy = " << poly.getTotalRotationalEnergy() 180 | << endl; 181 | 182 | return false; 183 | } 184 | 185 | void QuasistaticTimoshenkoBeam::_longWaveTest(const int nEdges, 186 | const string outputdata) { 187 | /* 188 | const REAL rho = 1000; 189 | const REAL L = 1.0; 190 | const REAL poissonRatio = 0.5; // incompressible material 191 | const REAL E = 1e9; 192 | const REAL G = E/(2.0*(1.0+poissonRatio)); 193 | const REAL nu = 5e-2; 194 | const REAL P = 0.0005; 195 | const REAL dL = L / nEdges; 196 | const REAL r = 0.01; 197 | const REAL dt = 0.001*dL; 198 | const REAL simTime = 10.0; 199 | const REAL relaxationNu = 0.0; 200 | _test(nEdges, dt, L, r, P, simTime, E, G, rho, nu, relaxationNu, outputdata); 201 | */ 202 | 203 | /* 204 | const REAL rho = 5000; 205 | const REAL L = 2.0; 206 | const REAL E = 1e6; 207 | const REAL G = 1e3; 208 | const REAL nu = 1e-1; 209 | const REAL minNu = 0.0; 210 | const REAL P = 1.0; 211 | const REAL dL = L / nEdges; 212 | const REAL r = 0.2; 213 | const REAL dt = 0.03*dL; 214 | const REAL simTime = 20000; 215 | const REAL halfLife = 10.0; 216 | const REAL relaxationNu = 0.0; 217 | _test(nEdges, dt, L, r, P, simTime, E, G, rho, nu, relaxationNu, minNu, 218 | outputdata); 219 | */ 220 | 221 | const REAL rho = 5000; 222 | const REAL L = 3.0; 223 | const REAL E = 1e6; 224 | 225 | /* 226 | Here we make a choice for the variable G, the shear modulus. 227 | 228 | If G is set low such that (E \cdot I)/\alpha \cdot L^2 \cdot A \cdot G not 229 | << 1, then the deflection of the beam under gravity is governed by the 230 | Timoshenko theorem of beams, which include the effect of shear. 231 | 232 | However, if we harden G or equivalently if (E \cdot I)/\alpha \cdot L^2 233 | \cdot A \cdot G << 1, then shear is unimportant in the deflection. Then we 234 | can calculate the final position of the beam using the (simplified) Euler- 235 | Bernoulli beam theory. 236 | 237 | Two values of G are specified by default. The choice governs which model 238 | is used to compare with Elastica's rod solution. 239 | */ 240 | 241 | const REAL G = 1e4; // Calculate using Timoshenko beam theory 242 | // const REAL G = 1e7; // Calculate using Euler-Bernoulli theory 243 | const REAL nu = 1e-1; 244 | const REAL P = 15; 245 | const REAL dL = L / nEdges; 246 | const REAL r = 0.25; 247 | const REAL dt = 0.01 * dL; 248 | const REAL simTime = 5000; 249 | const REAL relaxationNu = 0.0; 250 | _test(nEdges, dt, L, r, P, simTime, E, G, rho, nu, relaxationNu, outputdata); 251 | } 252 | 253 | void QuasistaticTimoshenkoBeam::run() { 254 | // Teja : replaced names here for ease in plotting script 255 | /* 256 | _longWaveTest( 5, "timoshenko_final"); 257 | _longWaveTest( 6, "timoshenko_final"); 258 | _longWaveTest( 7, "timoshenko_final"); 259 | _longWaveTest( 8, "timoshenko_final"); 260 | _longWaveTest( 9, "timoshenko_final"); 261 | _longWaveTest( 10, "timoshenko_final"); 262 | _longWaveTest( 20, "timoshenko_final"); 263 | _longWaveTest( 30, "timoshenko_final"); 264 | _longWaveTest( 40, "timoshenko_final"); 265 | _longWaveTest(50, "timoshenko_final"); 266 | */ 267 | /* 268 | _longWaveTest( 60, "timoshenko_final"); 269 | _longWaveTest( 70, "timoshenko_final"); 270 | _longWaveTest( 80, "timoshenko_final"); 271 | _longWaveTest( 90, "timoshenko_final"); 272 | */ 273 | _longWaveTest(100, "timoshenko_final"); 274 | /* 275 | _longWaveTest( 200, "timoshenko_final"); 276 | _longWaveTest( 400, "timoshenko_final"); 277 | */ 278 | exit(0); 279 | } 280 | -------------------------------------------------------------------------------- /source/QuasistaticTimoshenkoBeam.h: -------------------------------------------------------------------------------- 1 | /* 2 | * BendingWavesCouple.h 3 | * 4 | * Created on: Jun 21, 2015 5 | * Author: mgazzola 6 | */ 7 | 8 | #ifndef SOURCE_QUASISTATICTIMOSHENKOBEAM_H_ 9 | #define SOURCE_QUASISTATICTIMOSHENKOBEAM_H_ 10 | 11 | #include "ArgumentParser.h" 12 | #include "Polymer.h" 13 | #include "PositionVerlet2nd.h" 14 | #include "RodBoundaryConditions.h" 15 | #include "RodInitialConfigurations.h" 16 | #include "Test.h" 17 | #include "Tolerance.h" 18 | #include "UsualHeaders.h" 19 | 20 | using namespace std; 21 | 22 | class QuasistaticTimoshenkoBeam : public Test { 23 | protected: 24 | static bool _test(const int nEdges, const REAL _dt, const REAL _L, 25 | const REAL _r, const REAL _P, const REAL _timeSimulation, 26 | const REAL _E, const REAL _G, const REAL _rho, 27 | const REAL _nu, const REAL _relaxationNu, 28 | const string outfileName); 29 | static void _longWaveTest(const int nEdges, const string outputdata); 30 | 31 | public: 32 | QuasistaticTimoshenkoBeam(const int argc, const char **argv); 33 | virtual ~QuasistaticTimoshenkoBeam(); 34 | 35 | void run(); 36 | void paint(){}; 37 | }; 38 | 39 | #endif /* SOURCE_QUASISTATICTIMOSHENKOBEAM_H_ */ 40 | -------------------------------------------------------------------------------- /source/Rod.h: -------------------------------------------------------------------------------- 1 | #ifndef ROD_H_ 2 | #define ROD_H_ 3 | 4 | #ifdef SNAKE_VIZ 5 | #ifdef __APPLE__ 6 | #include 7 | #include "GLUT/glut.h" 8 | #else 9 | #include 10 | #endif 11 | #endif 12 | 13 | #include "GeometryFunctions.h" 14 | #include "Matrix3.h" 15 | #include "SpeedFunctions.h" 16 | #include "UsualHeaders.h" 17 | #include "Vector3.h" 18 | #include "VectorFunctions.h" 19 | 20 | class Rod { 21 | protected: 22 | static const unsigned int PRINTPRECISION = 6; 23 | 24 | public: 25 | // Flags 26 | const bool useSelfContact; 27 | 28 | // Some utility variable (just for prototyping purposes) 29 | long unsigned int COUNTER; 30 | REAL time; 31 | REAL dt; 32 | 33 | // Number of edges 34 | const int n; // number of edges, so there are n+1 vertices 35 | 36 | //========================================Added Parameters 37 | REAL MaxHeight; 38 | REAL Tforce; 39 | 40 | vector True_State; 41 | vector Observe_State; 42 | vector LC_State; 43 | vector LC_State_rate; 44 | 45 | vector Point_Rod[213 + 1]; 46 | vector Point_Rod_Direction[213 + 1]; 47 | vector Point_Rod_Angles[213 + 1]; 48 | 49 | REAL L0; 50 | //========================================Added Parameters 51 | 52 | // Material properties 53 | REAL density; // rod density --> [kg/m3] 54 | 55 | // Self-contact 56 | const REAL zetaSoft; // self-contact stiffness, soft core 57 | const REAL zetaHard; // self-contact stiffness, hard core 58 | 59 | // Muscle index; 60 | int mindex; 61 | 62 | // Masses 63 | vector m; // --> [kg], (n+1) 64 | 65 | // Volumes 66 | const vector 67 | V; // edge volume, so for round rod V = PI*r*r*l --> [m3], (n) 68 | 69 | // Position, velocity, acceleration (in lab coordinates) 70 | vector x; // centerline position in lab coords --> [m], (n+1) 71 | vector v; // centerline velocity in lab coords --> [m/s], (n+1) 72 | vector a; // centerline acceleration in lab coords -->[m/s2] (n+1) 73 | 74 | // Frame, angular velocity, angular acceleration (in material frame 75 | // coordinates) 76 | vector 77 | Q; // material frame attached to the edge mapping local coords 78 | // to lab coords: x_material ƒ= Q * x_lab --> [-], (n) 79 | vector w; // frame angular velocity in material frame --> [1/s], (n) 80 | vector 81 | wDot; // frame angular acceleration in material frame --> [1/s2], (n) 82 | 83 | // Inertia, bending, shear matrices (in reference configuration, they embedd 84 | // rotatory inertia and constitutive laws) 85 | const vector J0; // mass moment of inertia matrix in reference 86 | // configuration --> [kg*m2], (n) 87 | vector J0inv; // inverse moment of inertia matrix in reference 88 | // configuration --> [1/m4], (n) 89 | const vector 90 | B0; // bending matrix in reference configuration --> [Nm2], (n-1) 91 | const vector 92 | S0; // shear matrix in reference configuration --> [N], (n) 93 | const vector DB_0; // curvature damping matrix in reference 94 | // configuration --> [Nm2], (n-1) 95 | const vector 96 | DS_0; // shear damping matrix in reference configuration --> [N], (n) 97 | 98 | // Radius, length, voronaoi domain in reference configuration 99 | const vector l0; // lengths in reference configuration--> [m], (n) 100 | vector d0; // voronoi domain in reference configuration --> [m], (n-1) 101 | 102 | // Current radius, length, voronaoi domain 103 | vector r; // current radii --> [m], (n) 104 | vector l; // current lengths --> [m], (n) 105 | vector d; // current voronoi domain --> [m], (n-1) 106 | 107 | // Dilatations 108 | vector e; // dilatation -> l/l0 --> [-], (n) 109 | vector e_old; // dilatation -> l/l0 --> [-], (n) 110 | vector ed; // voronoi dilatation -> d/d0 --> [-], (n-1) 111 | 112 | // Rate of dilatation 113 | vector deldt; // dilatation -> d/dt{l/l0} --> [-], (n) 114 | 115 | // Edges 116 | vector edge; // edges, ie e_i = x_(i+1) - x_i, expressed in lab 117 | // frame --> [m], (n-1) 118 | vector edge_old; // edges, ie e_i = x_(i+1) - x_i, expressed in lab 119 | // frame --> [m], (n-1) 120 | 121 | // Bending 122 | vector 123 | k0; // curvature vector in the reference configuration, derivative taken 124 | // dividing by [d0]!!! Defined at interior vertices --> [1/m], (n-1) 125 | vector k0_old; // curvature vector in the previous step 126 | vector intrinsic_k0; // intrinsic curvature vector --> [1/m], (n-1) 127 | vector kDiff0; // difference between k and intrinsic_k0 128 | // (k0-intrinsic_k0) --> [1/m], (n-1) 129 | vector k0_rate; // curvature rate for calculating internal damping 130 | vector bendingInternalTorques0; // bending internal torques in 131 | // reference configuration, 132 | // bendingInternalTorques0=B0*kDiff0 133 | // --> [Nm], (n-1) 134 | vector 135 | bendingTorques; // resulting bending torques at edges in the current 136 | // cofiguration (obtain by rescaling appropriately 137 | // bendingInternalTorques0!) --> [Nm], (n) 138 | 139 | // Shear 140 | vector 141 | shearStrain0; // shear strain shearStrain0 = Q*d/dS{r} - d3 142 | // (expressed in the body frame and with respect 143 | // to reference configuration) --> [-], (n) 144 | vector shearStrain0_old; // shear strain in the previous step; 145 | const vector 146 | intrinsicShearStrain0; // intrinsic shear vector with respect to 147 | // reference configuration--> [-], (n) 148 | vector shearStrainDiff0; // difference between shearStrain0 and 149 | // intrinsicShearStrain0 --> [-], (n) 150 | vector shearStrain_rate; // shear strain rate 151 | vector shearInternalForces0; // internat shear forces with respect 152 | // to the reference configuration 153 | // S0*shearStrainDiff0 --> [N], (n) 154 | vector shearForces; // resulting shear forces with respect to actual 155 | // configuration at the vertices (obtained by 156 | // appropriately rescaling)! --> [N], (n+1) 157 | vector shearTorques; // resulting shear torques with respect to 158 | // actual configuration at edges (obtained by 159 | // appropriately rescaling)! --> [Nm], (n) 160 | 161 | // Damping 162 | const REAL nu; // linear drag term, ie fdrag = - nu * v 163 | const REAL relaxationNu; 164 | vector dampingForces; // --> [N], (n+1) 165 | vector dampingTorques; // --> [Nm], (n) 166 | Matrix3 D0; 167 | 168 | // Total forces and torques 169 | vector totalForces; // --> [N], (n+1) 170 | vector totalTorques; // --> [Nm], (n) 171 | 172 | // Total internal forces and torques 173 | vector totalInternalForces; // --> [N], (n+1) 174 | vector totalInternalTorques; // --> [Nm], (n) 175 | 176 | // External forces and torques 177 | vector externalForces; // --> [N], (n+1) 178 | vector externalTorques; // --> [Nm], (n) 179 | 180 | // Self collision forces 181 | vector selfCollisionForces; // --> [N], (n+1) 182 | 183 | // Term relative to the transport of angular velocity in the angular momentum 184 | // balance due to the use of material frame of reference (J0*w/e) x w 185 | vector transportW; 186 | 187 | // Energies 188 | REAL translationalEnergy; // we mean the energy associated with velocity of 189 | // centerline only here --> [J] 190 | REAL rotationalEnergy; // --> [J] 191 | REAL bendingEnergy; // --> [J] 192 | REAL shearEnergy; // --> [J] 193 | REAL totalInternalEnergy; // --> [J] 194 | 195 | const REAL selfKineticMu; 196 | const REAL selfStaticMu; 197 | const REAL selfKineticThreshold; 198 | vector collided; // whether the segment was in contact with substrate 199 | // the last time step 200 | 201 | // Friction with substrate 202 | vector staticFrictionsAxialForceForward; 203 | vector staticFrictionsAxialForceBackward; 204 | vector staticFrictionsRollingForce; 205 | vector staticFrictionsNormalPlane; 206 | vector kineticFrictionsForce; 207 | vector kineticFrictionsTorque; 208 | 209 | // Friction between rods 210 | vector staticFrictionsAxialForceForward_R; 211 | vector staticFrictionsAxialForceBackward_R; 212 | vector staticFrictionsRollingForce_R; 213 | vector staticFrictionsNormalPlane_R; 214 | 215 | vector globalcheck; 216 | vector rollingf; 217 | 218 | // Temporary vectors 219 | vector tempVV3_n; // temporary vector of vector3 for speed (size 220 | // equal to number of edges) 221 | vector tempVV3_n2; 222 | vector tempVV3_nplus; // temporary vector of vector3 for speed (size 223 | // equal to number of nodes) 224 | vector tempVV3_nminus; // temporary vector of vector3 for speed 225 | // (size equal to number of edges minus one) 226 | vector tempVV3_nminus2; 227 | vector tempVREAL_nminus; // temporary vector of reals for speed (size 228 | // equal to number of edges minus one) 229 | vector tempVREAL_n; // temporary vector of reals for speed (size equal 230 | // to number of edges) 231 | vector tempVREAL_nplus; // temporary vector of reals for speed (size 232 | // equal to number of nodes) 233 | vector tempVM3_n; // temporary vector of vector3 for speed (size 234 | // equal to number of edges) 235 | vector tempVM3_nminus; // temporary vector of vector3 for speed 236 | // (size equal to number of edges minus one) 237 | 238 | Rod(const int _n, vector _x, vector _v, vector _Q, 239 | vector _w, vector _l0, vector _intrinsic_k0, 240 | vector _intrinsicShearStrain0, const vector _m, 241 | vector _V, const REAL _density, vector _J0, 242 | vector _B, vector _S, const REAL _nu, 243 | const REAL _relaxationNu, const bool _useSelfContact = false); 244 | ~Rod(){}; 245 | 246 | void computeShearStrain0(); 247 | void computeCurvature0(); 248 | 249 | void computeInternalShearForces0(const REAL time); 250 | void computeInternalBendingTorques0(const REAL time); 251 | 252 | void computeInternalForces(); 253 | void computeInternalTorques(); 254 | 255 | void computeSelfCollisionForces(); 256 | void computeDampingForces(); 257 | 258 | void computeAllInternalResultingForcesAndTorques(); 259 | 260 | Vector3 computeVelocityCenterOfMass(); 261 | Vector3 computeAngularVelocityCenterOfMass(); 262 | 263 | void applyStaticFrictions(); 264 | 265 | void update(const REAL time); 266 | 267 | void computeBendingEnergy(); 268 | void computeShearEnergy(); 269 | void computeTranslationalEnergy(); 270 | void computeRotationalEnergy(); 271 | void computeEnergies(); 272 | 273 | void setDt(const REAL _dt) { dt = _dt; } 274 | void setTime(const REAL _time) { time = _time; } 275 | bool nanCheck(); 276 | void reset(); 277 | void dumpPovray(string filePovray, string fileData, int colorindex, 278 | REAL time); 279 | 280 | #ifdef SNAKE_VIZ 281 | void paint(); 282 | void draw_strokestring(void *font, float const size, char const *string); 283 | void draw_arrow(const float posx, const float posy, 284 | char const *const annotation_vert, 285 | char const *const annotation_hor, float annot_size); 286 | void draw_origin(const float posx, const float posy, float scale); 287 | #endif 288 | 289 | friend ostream &operator<<(ostream &, Rod &); 290 | }; 291 | 292 | #endif 293 | -------------------------------------------------------------------------------- /source/RodInitialConfigurations.h: -------------------------------------------------------------------------------- 1 | #ifndef RODINITIALCONFIGURATIONS_H_ 2 | #define RODINITIALCONFIGURATIONS_H_ 3 | 4 | #include "GeometryFunctions.h" 5 | #include "Matrix3.h" 6 | #include "Rod.h" 7 | #include "Tolerance.h" 8 | #include "UsualHeaders.h" 9 | #include "Vector3.h" 10 | 11 | using namespace std; 12 | 13 | class RodInitialConfigurations { 14 | public: 15 | static Rod *straightRod(const int n, const REAL totalMass, const REAL r0, 16 | const Matrix3 _J0, const Matrix3 _B0, 17 | const Matrix3 _S0, const REAL L0, const REAL totTwist, 18 | const Vector3 origin, const Vector3 direction, 19 | const Vector3 normal, const REAL nu, 20 | const REAL relaxationNu, const bool useSelfContact); 21 | 22 | static Rod compressedRod(const int n, const REAL totalMass, const REAL r, 23 | const Matrix3 J, const Matrix3 B, const Matrix3 S, 24 | const REAL L, const REAL F, const REAL totTwist, 25 | const REAL nu, const Vector3 origin, 26 | const Vector3 direction, const Vector3 normal, 27 | const bool useSelfContact); 28 | 29 | static Rod circleRod(const int n, const REAL totalMass, const REAL r, 30 | const Matrix3 I, const Matrix3 B, const Matrix3 S, 31 | const REAL L, const REAL totTwist, const REAL nu, 32 | const bool useSelfContact); 33 | 34 | static Rod *curvedRod(const int n, const REAL density, const vector r0, 35 | const vector J0, const vector B0, 36 | const vector S0, const REAL totTwist, 37 | const vector points, const Vector3 normal, 38 | const REAL nu, const REAL relaxationNu, 39 | const bool useSelfContact); 40 | 41 | static Rod *straightRod_v(const int n, const REAL density, 42 | const vector r0, const vector J0, 43 | const vector B0, const vector S0, 44 | const REAL L0, const REAL totTwist, 45 | const Vector3 origin, const Vector3 direction, 46 | const Vector3 normal, const REAL nu, 47 | const REAL relaxationNu, const bool useSelfContact); 48 | static Rod *straightRod_vscale( 49 | const int n, const REAL density, const vector r0, 50 | const vector J0, const vector B0, 51 | const vector S0, const REAL L0, const REAL totTwist, 52 | const Vector3 origin, const Vector3 direction, const Vector3 normal, 53 | const REAL nu, const REAL relaxationNu, const bool useSelfContact); 54 | // Twisted, helical artificial muscle 55 | static Rod *helicalRod(const int n, const REAL density, const REAL r0, 56 | const REAL pitch_factor, const REAL helix_radius, 57 | const REAL n_turns, const REAL E, const REAL G, 58 | const REAL totTwist, const Vector3 origin, 59 | const Vector3 direction, const REAL nu, 60 | const REAL relaxationNu, const bool useSelfContact); 61 | }; 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /source/SimpleConnection.h: -------------------------------------------------------------------------------- 1 | /* 2 | Simple Rod-Rod connection for toy examples 3 | */ 4 | #include "GeometryFunctions.h" 5 | #include "Matrix3.h" 6 | #include "Rod.h" 7 | #include "UsualHeaders.h" 8 | #include "Vector3.h" 9 | 10 | using namespace std; 11 | 12 | class SimpleConnection { 13 | protected: 14 | typedef std::vector Vrodptr; 15 | Vrodptr rodptrs; 16 | 17 | public: 18 | SimpleConnection(Vrodptr rodptrs) : rodptrs(rodptrs) {} 19 | ~SimpleConnection(){}; 20 | 21 | //----------------------------------------------------------------------------------------------------------------- 22 | /*This function exemplifies how to connect rods with a spherical joint. 23 | In this example, we will connect the last node of link one with the first node 24 | of link two. Link one is completely fixed, while link two is allowed to rotate 25 | in any direction. 26 | */ 27 | #ifdef FLAGSPHERICALJOINT 28 | void RodRodSimpleConnection(const REAL time) { 29 | // Connection between link one and link two 30 | // set up check points 31 | Vector3 check1 = Vector3(0.0, 0.0, 0.0); 32 | Vector3 check2 = Vector3(0.0, 0.0, 0.0); 33 | 34 | // stiffness of the joint connection -- tuned empirically 35 | const REAL k = 1e8; 36 | 37 | // Select rods 38 | Rod *One = rodptrs[0]; 39 | Rod *Two = rodptrs[1]; 40 | 41 | // Force —- Rod One-Two Connection 42 | const int size = One->n; 43 | 44 | // The checks points are the nodes sopposed to be connected. 45 | // Here, they are the last node of rod one and first node of rod two 46 | check1 = One->x[size]; 47 | check2 = Two->x[0]; 48 | 49 | // Compute their position difference, and then connecting force 50 | Vector3 force = k * (check2 - check1); 51 | // Force will be applied on each node with opposite direction to bring them 52 | // together. 53 | One->externalForces[size] += force; 54 | Two->externalForces[0] -= force; 55 | 56 | // Some load 57 | // This is the external load to test out our joint connection. 58 | const REAL Amp = 59 | 5e3; // 5 mN --- a very small force since we assume no gravity 60 | // Forces are applied on the last node of link two. 61 | // Initially, force is applied in negative z direction 62 | if (time < 0.2) { 63 | Two->externalForces[size] += Vector3(0.0, 0.0, -2 * Amp); 64 | } 65 | // After a short time period, force will remain in x-z plane, 66 | // but start changing direction to rotate link two around link one. 67 | else { 68 | Two->externalForces[size] += 69 | Vector3(Amp * cos(0.5 * M_PI * (time - 0.2)), 0.0, 70 | Amp * sin(0.5 * M_PI * (time - 0.2))); 71 | } 72 | } 73 | #endif 74 | 75 | //----------------------------------------------------------------------------------------------------------------- 76 | /*This function exemplifies how to connect rods with a hinge joint. 77 | A hinge connection is based on a spherical joint with additional constraint to 78 | allow only in plane rotation. In this example, we will still connect the last 79 | node of link one with the first node of link two, and link two is only allowed 80 | to rotation in y-z plane. 81 | */ 82 | #ifdef FLAGHINGEJOINT 83 | void RodRodSimpleConnection(const REAL time) { 84 | // Connection between link one and link two 85 | // Same with the spherical joint connection 86 | Vector3 check1 = Vector3(0.0, 0.0, 0.0); 87 | Vector3 check2 = Vector3(0.0, 0.0, 0.0); 88 | 89 | const REAL k = 1e8; 90 | 91 | Rod *One = rodptrs[0]; 92 | Rod *Two = rodptrs[1]; 93 | 94 | // Force —- Rod One-Two Connection 95 | const int size = One->n; 96 | check1 = One->x[size]; 97 | check2 = Two->x[0]; 98 | 99 | Vector3 force = k * (check2 - check1); 100 | One->externalForces[size] += force; 101 | Two->externalForces[0] -= force; 102 | 103 | // Additional in-plane constraint through restoring torque 104 | // Stiffness of the restoring constraint -- tuned empirically 105 | const REAL kt = 5e6; 106 | // Find the normal direction of the contraint plane (y-z plane) 107 | const Vector3 normaldirection = Vector3(1.0, 0.0, 0.0); 108 | // Current direction of the first element of link two 109 | const Vector3 linkdirection = Two->x[1] - Two->x[0]; 110 | // Projection of the linkdirection onto the plane normal 111 | const Vector3 Forcedirection = 112 | -(linkdirection % normaldirection) * normaldirection; 113 | // Compute the restoring torque 114 | Vector3 Torque = kt * linkdirection * Forcedirection; 115 | // The opposite torque will be applied on link one (no effect in this case 116 | // since link one is completely fixed) 117 | One->externalTorques[size - 1] -= One->Q[size - 1] * Torque; 118 | Two->externalTorques[0] += Two->Q[0] * Torque; 119 | 120 | // Some load 121 | // Same load as the previous case. 122 | const REAL Amp = 5e3; 123 | if (time < 0.2) { 124 | Two->externalForces[size] += Vector3(0.0, 0.0, -2 * Amp); 125 | } else { 126 | Two->externalForces[size] += 127 | Vector3(Amp * cos(0.5 * M_PI * (time - 0.2)), 0.0, 128 | Amp * sin(0.5 * M_PI * (time - 0.2))); 129 | } 130 | } 131 | #endif 132 | 133 | //----------------------------------------------------------------------------------------------------------------- 134 | /*This function exemplifies how to connect rods with a fixed joint. 135 | The concept of fixed connection is same with a hinge joint, but now the 136 | restoring torque will resist any motion that changes the orientation of link 137 | two. In this example, we will still connect the last node of link one with the 138 | first node of link two, and link two is constrained to have the same orientation 139 | with link one. 140 | */ 141 | #ifdef FLAGFIXEDJOINT 142 | void RodRodSimpleConnection(const REAL time) { 143 | // Connection between link one and link two 144 | // Same with the spherical joint connection 145 | Vector3 check1 = Vector3(0.0, 0.0, 0.0); 146 | Vector3 check2 = Vector3(0.0, 0.0, 0.0); 147 | Vector3 check3 = Vector3(0.0, 0.0, 0.0); 148 | Vector3 check4 = Vector3(0.0, 0.0, 0.0); 149 | 150 | const REAL k = 1e8; 151 | 152 | Rod *One = rodptrs[0]; 153 | Rod *Two = rodptrs[1]; 154 | 155 | // Force —- Rod One-Two Connection 156 | const int size = One->n; 157 | check1 = One->x[size]; 158 | check2 = Two->x[0]; 159 | 160 | Vector3 force = k * (check2 - check1); 161 | One->externalForces[size] += force; 162 | Two->externalForces[0] -= force; 163 | 164 | // Additional orientation constraint through restoring torque 165 | // Stiffness of the restoring constraint -- tuned empirically 166 | const REAL kt = 5e6; 167 | // Segment length of link two 168 | const REAL dl = 200.0 / 10.0; 169 | // Current direction of the first element of link two 170 | const Vector3 linkdirection = Two->x[1] - Two->x[0]; 171 | /*To constrain the orientation of link two, the second node of link two 172 | should align with the direction of link one. Thus, we compute the desired 173 | position of the second node of link two as check 3, and the current 174 | position of the second node of link two as check 4. Check 3 and check 4 175 | should overlap.*/ 176 | check3 = One->x[size] + dl * One->edge[size - 1].unitize(); 177 | check4 = Two->x[1]; 178 | // Compute the restoring torque 179 | const Vector3 Forcedirection = -kt * (check4 - check3); 180 | Vector3 Torque = linkdirection * Forcedirection; 181 | // The opposite torque will be applied on link one (no effect in this case 182 | // since link one is completely fixed) 183 | One->externalTorques[size - 1] -= One->Q[size - 1] * Torque; 184 | Two->externalTorques[0] += Two->Q[0] * Torque; 185 | 186 | // Some load 187 | // Same load as the previous case. 188 | const REAL Amp = 5e3; 189 | if (time < 0.2) { 190 | Two->externalForces[size] += Vector3(0.0, 0.0, -2 * Amp); 191 | } else { 192 | Two->externalForces[size] += 193 | Vector3(Amp * cos(0.5 * M_PI * (time - 0.2)), 0.0, 194 | Amp * sin(0.5 * M_PI * (time - 0.2))); 195 | } 196 | } 197 | #endif 198 | 199 | //----------------------------------------------------------------------------------------------------------------- 200 | // This function exemplifies how to connect muscle with substrate, and how to 201 | // setup muscle force. 202 | #ifdef FLAGPULLINGMUSCLE 203 | void RodRodSimpleConnection(const REAL time) { 204 | // Connection 205 | Vector3 check1 = Vector3(0.0, 0.0, 0.0); 206 | Vector3 check2 = Vector3(0.0, 0.0, 0.0); 207 | Vector3 check3 = Vector3(0.0, 0.0, 0.0); 208 | Vector3 check4 = Vector3(0.0, 0.0, 0.0); 209 | 210 | // stiffness of the joint connection -- tuned empirically 211 | const REAL k = 1e8; 212 | 213 | // One is the left side substrate and Two is right side substrate that the 214 | // muscle will be connected to and pulling against 215 | Rod *One = rodptrs[0]; 216 | Rod *Two = rodptrs[1]; 217 | Rod *Muscle = rodptrs[2]; 218 | 219 | // Spherical joint for both ends -- same implementation as before 220 | // Left side 221 | const int size = One->n; 222 | check1 = One->x[6]; 223 | check2 = Muscle->x[0]; 224 | 225 | Vector3 force = k * (check2 - check1); 226 | One->externalForces[6] += force; 227 | Muscle->externalForces[0] -= force; 228 | 229 | // Right side 230 | check3 = Two->x[6]; 231 | check4 = Muscle->x[10]; 232 | 233 | force = k * (check4 - check3); 234 | Two->externalForces[6] += force; 235 | Muscle->externalForces[10] -= force; 236 | 237 | // Muscle Force 238 | // Amplitude of the muscle force -- 2N 239 | const REAL Amp = 2e6; 240 | // Frequency of the muscle actuation -- 1Hz after taking absolute value of 241 | // sin 242 | const REAL w = 2.0 * M_PI / 2.0; 243 | 244 | const int size_m = Muscle->n; 245 | const int Tendon_n = 3; 246 | const int Muscle_n = size_m - 2 * Tendon_n; 247 | /*Muscle force will be applied only on the muscle segments 248 | Here we assume a simple sinusoidal signal for muscle activation, but it can 249 | be replaced by any experimental/ theoretical muscle functions. Muscle forces 250 | are applied at the two ends of each muscle segment, with opposite direction. 251 | Thus the muscle contraction will be uniform through out the entire muscle. 252 | No force on tendons. 253 | */ 254 | for (unsigned int i = Tendon_n; i < (Tendon_n + Muscle_n); i++) { 255 | Muscle->externalForces[i] += 256 | Amp * (Muscle->edge[i].unitize()) * abs(sin(w * time)); 257 | } 258 | for (unsigned int i = (Tendon_n + 1); i < (Tendon_n + Muscle_n + 1); i++) { 259 | Muscle->externalForces[i] += 260 | -1 * Amp * (Muscle->edge[i - 1].unitize()) * abs(sin(w * time)); 261 | } 262 | } 263 | #endif 264 | }; -------------------------------------------------------------------------------- /source/Snake.cpp: -------------------------------------------------------------------------------- 1 | #include "Snake.h" 2 | 3 | /* 4 | 5 | This example demonstrates a soft bio-inspired robotic slithering snake, 6 | actuated by a continuum analytical bending torque profile slithering on a 7 | plane-ground with anisotropic friction. The snake itself is a single rod lying 8 | on a plane (which provides normal forces through contact), but in addition to 9 | the internal forces, we also add an actuation (which mimics the internal 10 | actuation by muscles in a real snake). This actuation is represented by a 11 | continuum torque acting in the plane-normal direction. The torque is implemented 12 | analytically as a spatio-temporally varying traveling wave from the snake's head 13 | to its tail. Furthermore, to capture amplitude variation of this torque along 14 | the centerline (body) of the snake, we use a B-spline. 15 | 16 | With such an actuation, and on the plane, the snake wiggles around. What enables 17 | locomotion of the snake are the anisotropic frictional forces provided by the 18 | ground. The friction module in Elastica implements forward, backward, lateral 19 | and rolling static and kinetic friction. With this "complete" physical model, we 20 | optimized the snake's torque profile for maximum forward velocity, and these 21 | optimized parameters are the ones used in this file. 22 | 23 | For more details, especially on the parameters, the reader is referred to 24 | "Forward and inverse problems in the mechanics of soft filaments" by Gazzola 25 | et.al, RSoS, 2018. This example is instructive in showing how a rod along with 26 | proper contact and friction models can be used to investigate bio-physical 27 | phenomenon in the framework of Elastica. 28 | 29 | 30 | */ 31 | 32 | Snake::Snake(const int argc, const char **argv) : amp(0.0), w(0.0), v(0.0) { 33 | amp.clear(); 34 | 35 | amp.push_back(17.29); 36 | amp.push_back(48.54); 37 | amp.push_back(5.392); 38 | amp.push_back(14.74); 39 | v = 6.133; 40 | } 41 | 42 | REAL Snake::_snakeRun() { 43 | // Driving parameters 44 | const int n = 100; 45 | const REAL density = 1000.0; 46 | const REAL L0 = 1.0; 47 | const REAL r0 = 0.025 * L0; 48 | const REAL totalMass = density * M_PI * r0 * r0 * L0; 49 | const REAL E = 1e7; 50 | const REAL g = 9.81; 51 | const REAL Froude = 0.1; 52 | const REAL T = 1.0; 53 | const REAL mu = L0 / (T * T * g * Froude); 54 | const REAL dt = 1e-5 * T; 55 | const REAL timeSimulation = 5.1; 56 | w = 2.0 * M_PI / T; 57 | 58 | // Dumping frequencies (number of frames/dumps per unit time) 59 | const REAL diagPerUnitTime = 120 / T; 60 | const REAL povrayPerUnitTime = 50; 61 | 62 | const REAL dL0 = L0 / (double)n; // length of cross-section element 63 | const REAL A0 = M_PI * r0 * r0; 64 | const REAL poissonRatio = 0.5; // Incompressible 65 | const REAL G = E / (poissonRatio + 1.0); 66 | 67 | // Define plane 68 | const REAL angle = 0.0; 69 | const Vector3 originPlane = Vector3(0.0, 0.0, 0.0); 70 | const Vector3 normalPlane = Vector3(0.0, sin(angle), cos(angle)).unitize(); 71 | 72 | // Define rod 73 | const Vector3 directionRod = Vector3(1.0, 0.0, 0.0); 74 | const Vector3 normalRod = Vector3(0.0, 0.0, 1.0); 75 | const Vector3 originRod = 76 | (originPlane - L0 / 2.0 * directionRod) + r0 * normalPlane; 77 | 78 | // Second moment of area for disk cross section 79 | const REAL I0_1 = A0 * A0 / (4.0 * M_PI); 80 | const REAL I0_2 = I0_1; 81 | const REAL I0_3 = 2.0 * I0_1; 82 | const Matrix3 I0 = Matrix3(I0_1, 0.0, 0.0, 0.0, I0_2, 0.0, 0.0, 0.0, I0_3); 83 | 84 | // Mass inertia matrix for disk cross section 85 | const Matrix3 J0 = density * dL0 * I0; 86 | 87 | // Bending matrix 88 | Matrix3 B0 = 89 | Matrix3(E * I0_1, 0.0, 0.0, 0.0, E * I0_2, 0.0, 0.0, 0.0, G * I0_3); 90 | 91 | // Shear matrix 92 | Matrix3 S0 = Matrix3((4.0 / 3.0) * G * A0, 0.0, 0.0, 0.0, 93 | (4.0 / 3.0) * G * A0, 0.0, 0.0, 0.0, E * A0); 94 | 95 | // Initialize straight rod and pack it into a vector of pointers to rod 96 | const REAL initialTotalTwist = 0.0; 97 | const REAL nu = 5; 98 | const REAL relaxationNu = 0.0; 99 | const bool useSelfContact = false; 100 | 101 | vector rodPtrs; 102 | Rod *rod = RodInitialConfigurations::straightRod( 103 | n, totalMass, r0, J0, B0, S0, L0, initialTotalTwist, originRod, 104 | directionRod, normalRod, nu, relaxationNu, useSelfContact); 105 | rodPtrs.push_back(rod); 106 | rod->update(0.0); 107 | rod->computeEnergies(); 108 | 109 | // Pack boundary conditions 110 | FreeBC freeBC = FreeBC(); 111 | vector boundaryConditionsPtrs; 112 | boundaryConditionsPtrs.push_back(&freeBC); 113 | 114 | // Pack all forces together 115 | // Undulation Torque 116 | SplineMuscleTorques_O muscleLateral = 117 | SplineMuscleTorques_O(amp, w, v, normalPlane, T); 118 | GravityForce gravity = GravityForce(Vector3(0.0, 0.0, -g)); 119 | MultipleForces multipleForces; 120 | multipleForces.add(&muscleLateral); 121 | multipleForces.add(&gravity); 122 | MultipleForces *multipleForcesPtr = multipleForces.get(); 123 | vector externalForcesPtrs; 124 | externalForcesPtrs.push_back(multipleForcesPtr); 125 | 126 | // Set up substrate properties and snake-plane interaction object 127 | const REAL kPlane = 1.0; // stiffness of ground 128 | const REAL nuPlane = 1e-6; // viscous damping of ground 129 | const REAL muKineticForward = mu; 130 | const REAL muKineticSideways = 2.0 * muKineticForward; 131 | const REAL muKineticBackward = 1.5 * muKineticForward; 132 | const REAL muStaticForward = 2.0 * mu; 133 | const REAL muStaticSideways = 2.0 * muStaticForward; 134 | const REAL muStaticBackward = 1.5 * muStaticForward; 135 | const REAL vStatic = 1e-8; 136 | AnisotropicFrictionPlaneInteraction frictionPlane = 137 | AnisotropicFrictionPlaneInteraction( 138 | rodPtrs, normalPlane, originPlane, kPlane, nuPlane, muKineticForward, 139 | muKineticBackward, muKineticSideways, muStaticForward, 140 | muStaticBackward, muStaticSideways, vStatic); 141 | vector substrateInteractionsPtrs; 142 | substrateInteractionsPtrs.push_back(&frictionPlane); 143 | 144 | // Set up External Contact -- no external contact 145 | vector externalcontactPtrs; 146 | vector> attachpoint; 147 | ExternalContact externalcontact = 148 | ExternalContact(rodPtrs, 0.0, 0.0, attachpoint); 149 | externalcontactPtrs.push_back(&externalcontact); 150 | 151 | // No Simple Connection needed 152 | vector simpleconnectionPtrs; 153 | SimpleConnection simpleconnection = SimpleConnection(rodPtrs); 154 | simpleconnectionPtrs.push_back(&simpleconnection); 155 | 156 | // Set up integrator (define integration order) 157 | PolymerIntegrator *integrator = new PositionVerlet2nd( 158 | rodPtrs, externalForcesPtrs, boundaryConditionsPtrs, 159 | substrateInteractionsPtrs, externalcontactPtrs, simpleconnectionPtrs); 160 | 161 | // Instantiate simulator 162 | Polymer poly = Polymer(integrator); 163 | 164 | // I am goint go collect data over this time window 165 | poly.setWindowStats(1, 2); 166 | 167 | // Run simulation 168 | string outfileName = string("prova"); 169 | const bool goodRun = poly.simulate(timeSimulation, dt, diagPerUnitTime, 170 | povrayPerUnitTime, outfileName); 171 | 172 | // Throw exception if something went wrong 173 | if (!goodRun) 174 | throw "not good run in localized helical buckling, what is going on?"; 175 | 176 | const vector avgVel = poly.getAverageVelocity(); 177 | 178 | vector fwdAvgVel; 179 | fwdAvgVel.push_back(avgVel[0] % directionRod); 180 | fwdAvgVel.push_back(avgVel[0] % Vector3(0.0, -1.0, 0.0)); 181 | 182 | const REAL fitness = fwdAvgVel[0]; 183 | return (fitness); 184 | } 185 | 186 | void Snake::run() { 187 | const REAL fitness = _snakeRun(); 188 | cout << "Mean velocity: " << fitness << endl; 189 | exit(0); 190 | } 191 | -------------------------------------------------------------------------------- /source/Snake.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Snake.h 3 | * 4 | * Created on: Jun 22, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #ifndef SNAKE_H_ 9 | #define SNAKE_H_ 10 | 11 | #include "ArgumentParser.h" 12 | #include "Polymer.h" 13 | #include "PositionVerlet2nd.h" 14 | #include "RodInitialConfigurations.h" 15 | #include "Test.h" 16 | #include "UsualHeaders.h" 17 | 18 | using namespace std; 19 | 20 | class Snake : public Test { 21 | protected: 22 | vector amp; 23 | double w, v; 24 | 25 | REAL _snakeRun(); 26 | 27 | #ifdef SNAKE_VIZ 28 | void _paint(){}; 29 | #endif 30 | 31 | public: 32 | Snake(const int argc, const char **argv); 33 | ~Snake(){}; 34 | 35 | void run(); 36 | void paint(){}; 37 | }; 38 | 39 | #endif /* SNAKE_H_ */ -------------------------------------------------------------------------------- /source/SpeedFunctions.h: -------------------------------------------------------------------------------- 1 | #ifndef SPEEDFUNCTIONS_H_ 2 | #define SPEEDFUNCTIONS_H_ 3 | 4 | #include 5 | #include 6 | #include "ArithmeticPrecision.h" 7 | #include "Matrix3.h" 8 | #include "UsualHeaders.h" 9 | #include "Vector3.h" 10 | 11 | using namespace std; 12 | 13 | //////////////////// Mattia's new vector functions for speed (no memory 14 | /// allocation) //////////////////////////// 15 | 16 | // No need for inline functions here (I checked - no gain) 17 | 18 | void vLength(const vector &A, vector &B); 19 | void vUnitize(const vector &A, vector &B); 20 | void vDelta(const vector &A, vector &B); 21 | void vSqrt(const vector &A, vector &B); 22 | void vDiff(const vector &A, vector &B); 23 | void vRotDiff(const vector &A, vector &B); 24 | void vLog(const vector &A, vector &B); 25 | void vExp(const vector &A, vector &B, 26 | const REAL tol = Tolerance::tol()); 27 | void vMidAvg(const vector &A, vector &B); 28 | void vMidAvgInterior(const vector &A, vector &B); 29 | void vMidAvgInterior(const vector &A, vector &B); 30 | void vFromPointsToElements(const vector &A, vector &B); 31 | void vFromPointsToElements(const vector &A, vector &B); 32 | void vT(const vector &A, vector &B); 33 | void vDiagI(const vector &A, vector &B); 34 | void v_timesequal(const vector &A, vector &B); // a = b * a 35 | void v_a_plus_b_equal_c(const vector &A, const vector &B, 36 | vector &C); // c = a + b 37 | void v_plusequal_a_times_b(const REAL A, const vector &B, 38 | vector &C); // c += a * b 39 | void v_a_plus_b_plus_c_equal_d(const vector &A, 40 | const vector &B, 41 | const vector &C, 42 | vector &D); // d = a + b + c 43 | void v_a_minus_b_equal_c(const vector &A, const vector &B, 44 | vector &C); // c = a - b 45 | void v_a_times_b_equal_c(const vector &A, const REAL B, 46 | vector &C); // c = a * b 47 | void v_a_times_b_equal_c(const vector &A, const REAL B, 48 | vector &C); // c = a * b 49 | void v_a_times_b_equal_c(const vector &A, const vector &B, 50 | vector &C); // c = a * b 51 | void v_a_cross_b_equal_c(const vector &A, const vector &B, 52 | vector &C); // c = a x b 53 | void v_a_times_b_times_c_equal_d(const REAL A, const vector &B, 54 | const vector &C, 55 | vector &D); // d = a * b * c 56 | void v_a_times_b_cross_c_equal_d(const vector &A, 57 | const vector &B, 58 | const vector &C, 59 | vector &D); // d = a * b x c 60 | void v_a_times_b_cross_c_equal_d(const REAL A, const vector &B, 61 | const vector &C, 62 | vector &D); // d = a * b x c 63 | void v_a_divide_b_equal_c(const vector &A, const vector &B, 64 | vector &C); // c = a / b 65 | void v_a_dividepar_b_times_c_equal_d(const vector &A, 66 | const vector &B, const REAL C, 67 | vector &D); // d = a / (b*c) 68 | void v_sqrt_a_dividepar_b_times_c_equal_d( 69 | const vector &A, const vector &B, const REAL C, 70 | vector &D); // d = sqrt( a / (b*c) ) 71 | void v_a_times_b_divide_c_minus_d_equal_c( 72 | const vector &A, const vector &B, const vector &C, 73 | const Vector3 &D, vector &E); // e = (a * b / c) - d 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /source/SphericalJoint.cpp: -------------------------------------------------------------------------------- 1 | #include "SphericalJoint.h" 2 | 3 | /* 4 | Spherical joint 5 | 6 | This example demonstrates how we connect two Cosserat rods together 7 | using a spherical joint like so, with a force at the tip 8 | 9 | // clang-format off 10 | 11 | * Force 12 | \ 13 | \ 14 | ============================= (o) ============================= 15 | Rod1 Spherical joint Rod 2 16 | 17 | ^ z 18 | | 19 | | 20 | o — — —> y 21 | / 22 | / 23 | x 24 | 25 | // clang-format on 26 | 27 | No other forces (like gravity) are considered. 28 | 29 | The spherical joint allows for relative rod rotations about it (across 30 | any plane), but prevents relative displacements. That means Rod2 in this case 31 | can have any orientation wrt Rod1 (due to the influence of the force ) but is 32 | always connected to Rod1. In this case, initially the rods are aligned along the 33 | y-axis, and a force with time-varying direction (but fixed magnitude) acts only 34 | along the x-z direction (after an initial transient). The torque due to this 35 | force then brings Rod2 into the x-z plane passing through the spherical joint 36 | (which only counteracts all forces), at which point no out-of-plane torques 37 | exist. From thereon, Rod2 lies in the x-z plane and rotates about the joint in a 38 | circular fashion. In our python script, we also visualize the time-varying force 39 | that acts on the tip so that the user can intuit the rod-motion. 40 | 41 | Note that the rods and joints are minimally damped in this case and 42 | hence you might observe oscillations in the data—for example, the rod does not 43 | perfectly lie on the x-z plane, but rather oscillates about it. 44 | 45 | */ 46 | 47 | SphericalJoint::SphericalJoint(const int argc, const char **argv) 48 | : amp(0.0), w(0.0), v(0.0) {} 49 | 50 | // Units in this case are mm/g/s 51 | 52 | vector SphericalJoint::_sphericaljointRun() { 53 | vector rodPtrs; 54 | 55 | // Dumping frequencies (number of frames/dumps per unit time) 56 | const REAL diagPerUnitTime = 120; 57 | const REAL povrayPerUnitTime = 50; 58 | const REAL dt = 1e-6; 59 | const REAL timeSimulation = (10.0); 60 | 61 | //----------------------------------------------------------------------------------------------------------------- 62 | // Define Links 63 | // Driving parameters 64 | // Link One shape 65 | const int n = 10; 66 | const REAL density = 1.75e-3; // 1.75g/cm^3 67 | const REAL L0 = 200.0; 68 | const REAL r0 = 7.0; 69 | const REAL E = 3e7; 70 | const REAL totalMass = density * M_PI * r0 * r0 * L0; 71 | 72 | const REAL dL0 = L0 / (double)n; // length of cross-section element 73 | const REAL poissonRatio = 0.5; // Incompressible 74 | const REAL G = E / (poissonRatio + 1.0); 75 | 76 | // Define rod 77 | const Vector3 Linkdirection = Vector3(0.0, -1.0, 0.0); 78 | const Vector3 Linknormal = Vector3(0.0, 0.0, 1.0); 79 | const Vector3 Linkoneorigin = Vector3(0.0, L0, L0); 80 | const Vector3 Linktwoorigin = Linkoneorigin + L0 * Linkdirection; 81 | 82 | // Second moment of area for disk cross section 83 | const REAL A0 = M_PI * r0 * r0; 84 | const REAL I0_1 = A0 * A0 / (4.0 * M_PI); 85 | const REAL I0_2 = I0_1; 86 | const REAL I0_3 = 2.0 * I0_1; 87 | const Matrix3 I0 = Matrix3(I0_1, 0.0, 0.0, 0.0, I0_2, 0.0, 0.0, 0.0, I0_3); 88 | // Mass inertia matrix for disk cross section 89 | const Matrix3 J0 = density * dL0 * I0; 90 | 91 | // Bending matrix 92 | Matrix3 B0 = 93 | Matrix3(E * I0_1, 0.0, 0.0, 0.0, E * I0_2, 0.0, 0.0, 0.0, G * I0_3); 94 | // Shear matrix 95 | Matrix3 S0 = Matrix3((4.0 / 3.0) * G * A0, 0.0, 0.0, 0.0, 96 | (4.0 / 3.0) * G * A0, 0.0, 0.0, 0.0, E * A0); 97 | 98 | // Initialize straight rod and pack it into a vector of pointers to rod 99 | const REAL initialTotalTwist = 0.0; 100 | const REAL nu = 0.1; 101 | const REAL relaxationNu = 0.0; 102 | const bool useSelfContact = false; 103 | 104 | Rod *rod1 = RodInitialConfigurations::straightRod( 105 | n, totalMass, r0, J0, B0, S0, L0, initialTotalTwist, Linkoneorigin, 106 | Linkdirection, Linknormal, nu, relaxationNu, useSelfContact); 107 | rodPtrs.push_back(rod1); 108 | rod1->update(0.0); 109 | Rod *rod2 = RodInitialConfigurations::straightRod( 110 | n, totalMass, r0, J0, B0, S0, L0, initialTotalTwist, Linktwoorigin, 111 | Linkdirection, Linknormal, nu, relaxationNu, useSelfContact); 112 | rodPtrs.push_back(rod2); 113 | rod2->update(0.0); 114 | 115 | //----------------------------------------------------------------------------------------------------------------- 116 | // Pack boundary conditions 117 | vector boundaryConditionsPtrs; 118 | // Link One 119 | FixedBC fixed = FixedBC(rodPtrs[0]); 120 | boundaryConditionsPtrs.push_back(&fixed); 121 | // Link Two 122 | FreeBC freeBC = FreeBC(); 123 | boundaryConditionsPtrs.push_back(&freeBC); 124 | 125 | // Pack all forces together (no forces applied) 126 | vector externalForcesPtrs; 127 | 128 | // Gravity 129 | MultipleForces multipleForces1; 130 | GravityForce gravity = GravityForce(Vector3(0.0, 0.0, 0.0)); 131 | multipleForces1.add(&gravity); 132 | MultipleForces *multipleForcesPtr1 = multipleForces1.get(); 133 | for (unsigned int i = 0; i < 2; i++) { 134 | externalForcesPtrs.push_back(multipleForcesPtr1); 135 | } 136 | 137 | vector substrateInteractionsPtrs; 138 | 139 | // Set up External Contact -- This is for the five cases in the paper, not 140 | // used in this case 141 | vector> attachpoint; 142 | vector externalcontactPtrs; 143 | /* The second and third argument are unimportant, but 144 | are preserved here for legacy purposes. Hence we simply 145 | set it to 0.0 146 | */ 147 | ExternalContact externalcontact = 148 | ExternalContact(rodPtrs, 0.0, 0.0, attachpoint); 149 | externalcontactPtrs.push_back(&externalcontact); 150 | 151 | // Set up Simple Connection 152 | vector simpleconnectionPtrs; 153 | SimpleConnection simpleconnection = SimpleConnection(rodPtrs); 154 | simpleconnectionPtrs.push_back(&simpleconnection); 155 | //----------------------------------------------------------------------------------------------------------------- 156 | // Set up integrator (define integration order) 157 | PolymerIntegrator *integrator = new PositionVerlet2nd( 158 | rodPtrs, externalForcesPtrs, boundaryConditionsPtrs, 159 | substrateInteractionsPtrs, externalcontactPtrs, simpleconnectionPtrs); 160 | 161 | // Instantiate simulator 162 | Polymer poly = Polymer(integrator); 163 | 164 | // I am goint go collect data over this time window 165 | poly.setWindowStats(1.0, 2.0); 166 | 167 | // Run simulation 168 | string outfileName = string("prova"); 169 | const bool goodRun = poly.simulate(timeSimulation, dt, diagPerUnitTime, 170 | povrayPerUnitTime, outfileName); 171 | 172 | // Throw exception if something went wrong 173 | if (!goodRun) 174 | throw "not good run in localized helical buckling, what is going on?"; 175 | 176 | const vector avgVel = poly.getAverageVelocity(); 177 | 178 | vector fwdAvgVel; 179 | for (unsigned int i = 0; i < 1; i++) { 180 | fwdAvgVel.push_back(avgVel[i] % Linkdirection); 181 | } 182 | const vector fitness = fwdAvgVel; 183 | 184 | return (fitness); 185 | } 186 | 187 | void SphericalJoint::run() { 188 | const vector fitness = _sphericaljointRun(); 189 | exit(0); 190 | } 191 | -------------------------------------------------------------------------------- /source/SphericalJoint.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SphericalJoint.h 3 | * 4 | */ 5 | 6 | #ifndef SPHERICALJOINT_H_ 7 | #define SPHERICALJOINT_H_ 8 | 9 | #include "ArgumentParser.h" 10 | #include "Polymer.h" 11 | #include "PositionVerlet2nd.h" 12 | #include "RodInitialConfigurations.h" 13 | #include "Test.h" 14 | #include "UsualHeaders.h" 15 | 16 | using namespace std; 17 | 18 | class SphericalJoint : public Test { 19 | protected: 20 | vector amp; 21 | double w, v; 22 | int NOR; 23 | 24 | vector _sphericaljointRun(); 25 | 26 | #ifdef SNAKE_VIZ 27 | void _paint(){}; 28 | #endif 29 | 30 | public: 31 | SphericalJoint(const int argc, const char **argv); 32 | ~SphericalJoint(){}; 33 | 34 | void run(); 35 | void paint(){}; 36 | }; 37 | 38 | #endif /* SPHERICALJOINT_H_ */ 39 | -------------------------------------------------------------------------------- /source/SplineProfileZeroEnds.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SplineProfileZeroEnds.cpp 3 | * 4 | * Created on: Jul 2, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #include "SplineProfileZeroEnds.h" 9 | 10 | SplineProfileZeroEnds::SplineProfileZeroEnds(vector ctrlPoints) 11 | : N(1000), Nw(ctrlPoints.size() + 2), ndegree(3) { 12 | // Clear internal profile 13 | xProfile.clear(); 14 | yProfile.clear(); 15 | 16 | // Store control points 17 | this->ctrlPoints = ctrlPoints; 18 | 19 | // Check control parameter vector size 20 | if (this->ctrlPoints.size() < 3) { 21 | printf("WIDTH vector too small!\n"); 22 | abort(); 23 | } 24 | 25 | _computeAndStoreProfile(); 26 | } 27 | 28 | SplineProfileZeroEnds::~SplineProfileZeroEnds() {} 29 | 30 | double SplineProfileZeroEnds::_coxDeBoorRecursion(const int j, const int d, 31 | const double *t, 32 | const double u) const { 33 | const double thres = 1e-6; 34 | double fac1 = 0.0; 35 | double fac2 = 0.0; 36 | 37 | if (fabs((double)d - 1.0) < thres) return (t[j] <= u && u < t[j + 1]) ? 1 : 0; 38 | 39 | if (fabs(t[j + d - 1] - t[j]) < thres) 40 | fac1 = 0.0; 41 | else 42 | fac1 = (u - t[j]) / (t[j + d - 1] - t[j]); 43 | 44 | if (fabs(t[j + d] - t[j + 1]) < thres) 45 | fac2 = 0.0; 46 | else 47 | fac2 = (t[j + d] - u) / (t[j + d] - t[j + 1]); 48 | 49 | const double bOld = _coxDeBoorRecursion(j, d - 1, t, u); 50 | const double bOldRight = _coxDeBoorRecursion(j + 1, d - 1, t, u); 51 | 52 | return fac1 * bOld + fac2 * bOldRight; 53 | } 54 | 55 | void SplineProfileZeroEnds::_computeAndStoreProfile() { 56 | // Very inefficient implementation, but the filling of the W vector 57 | // is done once at the beginning and that s it, so no big deal! 58 | 59 | // Constants 60 | const unsigned int Nx = N; 61 | const unsigned int n = Nw - 1; 62 | const unsigned int d = ndegree + 1; 63 | const unsigned int m = n + d + 1; 64 | double t[m]; 65 | 66 | // Allocate control points 67 | double Pwx[Nw]; 68 | double Pwy[Nw]; 69 | 70 | // Allocate x and y vectors and set them to zero 71 | double xr[Nx]; 72 | double yr[Nx]; 73 | for (unsigned int i = 0; i < Nx; i++) { 74 | xr[i] = 0.0; 75 | yr[i] = 0.0; 76 | } 77 | 78 | // Define control points, first the ends 79 | Pwx[0] = 0.0; 80 | Pwy[0] = 0.0; 81 | Pwx[Nw - 1] = 1.0; 82 | Pwy[Nw - 1] = 0.0; 83 | 84 | // Now the interior 85 | unsigned int counter = 0; 86 | const double dxw = 1.0 / ((double)Nw - 3.0); 87 | for (unsigned int i = 1; i < Nw - 1; i++) { 88 | Pwx[i] = (i - 1) * dxw; 89 | Pwy[i] = ctrlPoints[counter]; 90 | counter++; 91 | } 92 | 93 | // Build t vector 94 | for (unsigned int i = 0; i < m; i++) { 95 | if (i < d) 96 | t[i] = 0.0; 97 | else if (i > n) 98 | t[i] = n - d + 2.0; 99 | else 100 | t[i] = i - d + 1.0; 101 | } 102 | 103 | for (unsigned int i = 0; i < m; i++) t[i] /= t[m - 1]; 104 | 105 | // Build the spline 106 | const double du = t[m - 1] / ((double)Nx - 1.0); 107 | for (unsigned int i = 0; i < Nx; i++) { 108 | const double u = (double)i * du; 109 | for (unsigned int j = 0; j < n + 1; j++) { 110 | xr[i] += Pwx[j] * _coxDeBoorRecursion(j, d, t, u); 111 | yr[i] += Pwy[j] * _coxDeBoorRecursion(j, d, t, u); 112 | } 113 | } 114 | 115 | xr[Nx - 1] = Pwx[Nw - 1]; 116 | yr[Nx - 1] = Pwy[Nw - 1]; 117 | 118 | for (unsigned int i = 0; i < Nx; i++) { 119 | xProfile.push_back(xr[i]); 120 | yProfile.push_back(yr[i]); 121 | } 122 | } 123 | 124 | double SplineProfileZeroEnds::getProfile(const double ss) const { 125 | assert((ss >= 0.0) && (ss <= 1.0)); 126 | 127 | // Generate profile via linear interpolation 128 | const unsigned int Nx = N; 129 | const double s = ss; 130 | double width = 0.0; 131 | 132 | unsigned int idx = 0; 133 | for (unsigned int i = 0; i < Nx; i++) { 134 | if (xProfile[i] >= s) { 135 | idx = std::max(0, (int)i); 136 | break; 137 | } 138 | } 139 | 140 | const unsigned int idxPlus = std::min(int(idx + 1), int(Nx - 1)); 141 | const double denominator = 142 | (idx == idxPlus) ? 1.0 : (xProfile[idxPlus] - xProfile[idx]); 143 | width = yProfile[idx] + (yProfile[idxPlus] - yProfile[idx]) * 144 | (s - xProfile[idx]) / denominator; 145 | 146 | return width; 147 | } 148 | -------------------------------------------------------------------------------- /source/SplineProfileZeroEnds.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SplineProfileZeroEnds.h 3 | * 4 | * Created on: Jul 2, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #ifndef SPLINEPROFILEZEROENDS_H_ 9 | #define SPLINEPROFILEZEROENDS_H_ 10 | 11 | #include "UsualHeaders.h" 12 | 13 | using namespace std; 14 | 15 | class SplineProfileZeroEnds { 16 | vector ctrlPoints; 17 | vector xProfile; 18 | vector yProfile; 19 | double _coxDeBoorRecursion(const int j, const int d, const double *t, 20 | const double u) const; 21 | void _computeAndStoreProfile(); 22 | 23 | public: 24 | // Number of discretization points for spline. 25 | // It should be at any time larger the the number of 26 | // discretization points of a rod - check assert in spline force class) 27 | const unsigned int N; 28 | 29 | // Total number of control points, two of which are anchored to zero at the 30 | // extrema therefore the number of actual control parameters is 31 | // Nw=ctrlPoints.size()+2 32 | const unsigned int Nw; 33 | 34 | // Spline degree 35 | const unsigned int ndegree; 36 | 37 | SplineProfileZeroEnds(vector ctrlPoints); 38 | virtual ~SplineProfileZeroEnds(); 39 | 40 | double getProfile(const double ss) const; 41 | }; 42 | 43 | #endif /* SPLINEPROFILEZEROENDS_H_ */ 44 | -------------------------------------------------------------------------------- /source/Test.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Test.h 3 | * 4 | * Created on: Jun 22, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #ifndef TEST_H_ 9 | #define TEST_H_ 10 | 11 | #include "ExternalContact.h" 12 | #include "Interaction.h" 13 | #include "MRAGProfiler.h" 14 | #include "Matrix3.h" 15 | #include "Rod.h" 16 | #include "RodBoundaryConditions.h" 17 | #include "RodExternalForces.h" 18 | #include "UsualHeaders.h" 19 | #include "Vector3.h" 20 | 21 | class Test { 22 | protected: 23 | typedef std::vector VV3; 24 | typedef std::vector VM3; 25 | typedef std::vector VREAL; 26 | typedef std::vector VBOOL; 27 | typedef std::vector VINT; 28 | typedef std::vector Vbcptr; 29 | typedef std::vector Vefptr; 30 | typedef std::vector Vrodptr; 31 | typedef std::vector Vinterptr; 32 | 33 | public: 34 | Test(){}; 35 | virtual ~Test(){}; 36 | 37 | virtual void run() = 0; 38 | virtual void paint() = 0; 39 | }; 40 | 41 | #endif /* TEST_H_ */ 42 | -------------------------------------------------------------------------------- /source/Tolerance.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Tolerance.h 3 | * 4 | * Created on: Jul 28, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #ifndef TOLERANCE_H_ 9 | #define TOLERANCE_H_ 10 | 11 | #include "ArithmeticPrecision.h" 12 | #include "UsualHeaders.h" 13 | 14 | class Tolerance { 15 | public: 16 | Tolerance(); 17 | virtual ~Tolerance(); 18 | 19 | inline static REAL tol() { 20 | return 10.0 * std::numeric_limits::epsilon(); 21 | } 22 | }; 23 | 24 | #endif /* TOLERANCE_H_ */ 25 | -------------------------------------------------------------------------------- /source/UsualHeaders.h: -------------------------------------------------------------------------------- 1 | /* 2 | * UsualHeaders.h 3 | * 4 | * Created on: Jul 2, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #ifndef USUALHEADERS_H_ 9 | #define USUALHEADERS_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #endif /* USUALHEADERS_H_ */ 27 | -------------------------------------------------------------------------------- /source/Vector3.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Vector3.cpp 3 | * 4 | * Created on: Feb 9, 2015 5 | * Author: mgazzola 6 | */ 7 | 8 | #include "Vector3.h" 9 | 10 | // constructors 11 | Vector3::Vector3() : x(0), y(0), z(0) {} 12 | 13 | Vector3::Vector3(REAL x, REAL y, REAL z) : x(x), y(y), z(z) {} 14 | 15 | Vector3::Vector3(const Vector3 &V) : x(V.x), y(V.y), z(V.z) {} 16 | 17 | // io 18 | std::ostream &operator<<(std::ostream &out, const Vector3 &A) { 19 | char buffer[1000]; 20 | sprintf(buffer, "%10.10e %10.10e %10.10e", A.x, A.y, A.z); 21 | out << buffer; 22 | return out; 23 | } 24 | 25 | ///// Additional vector functions not defined as member functions ////// 26 | 27 | Vector3 operator*(REAL a, const Vector3 &V) { return V * a; } 28 | 29 | // gets the angle between two vector in range(0, PI) 30 | REAL angle(const Vector3 &A, const Vector3 &B) { 31 | REAL cosTheta = (A % B) / (A.length() * B.length()); 32 | return arcCos(cosTheta); 33 | } 34 | 35 | // generates a uniform gaussian random variable in 3 dimensions 36 | Vector3 randVector3() { 37 | return Vector3(randn_notrig(0., 1.), randn_notrig(0., 1.), 38 | randn_notrig(0., 1.)); 39 | } 40 | 41 | Vector3 randVector3(REAL mu, REAL sigma) { 42 | return Vector3(randn_notrig(mu, sigma), randn_notrig(mu, sigma), 43 | randn_notrig(mu, sigma)); 44 | } 45 | 46 | // random only in x direction 47 | Vector3 randXOnly() { return Vector3(randn_notrig(0., 1.), 0, 0); } 48 | -------------------------------------------------------------------------------- /source/Vector3.h: -------------------------------------------------------------------------------- 1 | #ifndef VECTOR3_H_ 2 | #define VECTOR3_H_ 3 | 4 | #include "ArithmeticPrecision.h" 5 | #include "MathFunctions.h" 6 | #include "Tolerance.h" 7 | #include "UsualHeaders.h" 8 | 9 | class Vector3 { 10 | private: 11 | static const unsigned int PRINTPRECISION = 6; 12 | 13 | public: 14 | // This should be completely recoded in a HPC code 15 | REAL x, y, z; 16 | 17 | // constructors (never inline) 18 | Vector3(); 19 | Vector3(REAL x, REAL y, REAL z); 20 | Vector3(const Vector3 &V); 21 | 22 | // accessor 23 | inline REAL operator[](int element) const { 24 | switch (element) { 25 | case 0: 26 | return x; 27 | case 1: 28 | return y; 29 | case 2: 30 | return z; 31 | default: 32 | throw "Bad [] access attempt on Vector3.\n"; 33 | } 34 | } 35 | 36 | // vector operators 37 | inline Vector3 operator+(const Vector3 &V) const { 38 | return Vector3(x + V.x, y + V.y, z + V.z); 39 | } 40 | inline Vector3 operator-(const Vector3 &V) const { 41 | return Vector3(x - V.x, y - V.y, z - V.z); 42 | } 43 | inline Vector3 operator*(const Vector3 &V) const { 44 | return Vector3(y * V.z - z * V.y, z * V.x - x * V.z, x * V.y - y * V.x); 45 | } // cross product 46 | inline REAL operator%(const Vector3 &V) const { 47 | return x * V.x + y * V.y + z * V.z; 48 | } // dot product 49 | 50 | // scalar operators 51 | inline Vector3 operator*(REAL a) const { 52 | return Vector3(x * a, y * a, z * a); 53 | } 54 | inline Vector3 operator/(REAL a) const { 55 | return Vector3(x / a, y / a, z / a); 56 | } 57 | inline Vector3 operator-() const { return Vector3(-x, -y, -z); } 58 | 59 | // in-place modifiers 60 | inline Vector3 &operator+=(const Vector3 &V) { 61 | x += V.x; 62 | y += V.y; 63 | z += V.z; 64 | return *this; 65 | } 66 | inline Vector3 &operator-=(const Vector3 &V) { 67 | x -= V.x; 68 | y -= V.y; 69 | z -= V.z; 70 | return *this; 71 | } 72 | inline Vector3 &operator*=(const Vector3 &V) { 73 | *this = *this * V; 74 | return *this; 75 | } 76 | 77 | inline Vector3 &operator*=(REAL a) { 78 | x *= a; 79 | y *= a; 80 | z *= a; 81 | return *this; 82 | } 83 | inline Vector3 &operator/=(REAL a) { 84 | x /= a; 85 | y /= a; 86 | z /= a; 87 | return *this; 88 | } 89 | 90 | // utility functions 91 | inline REAL length() const { return sqrt(x * x + y * y + z * z); } 92 | inline bool isUnit(const REAL tol = Tolerance::tol()) const { 93 | return fabs(length() - 1.0) < tol; 94 | } 95 | 96 | inline Vector3 unitize() const { 97 | const REAL l = length(); 98 | if (l < Tolerance::tol()) return Vector3(); 99 | return *this / l; 100 | } 101 | 102 | // speed functions 103 | static inline void plusequal_a_times_b(const REAL A, const Vector3 &B, 104 | Vector3 &C) { 105 | C.x += A * B.x; 106 | C.y += A * B.y; 107 | C.z += A * B.z; 108 | } 109 | 110 | static inline void a_plus_b_equal_c(const Vector3 &A, const Vector3 &B, 111 | Vector3 &C) { 112 | C.x = A.x + B.x; 113 | C.y = A.y + B.y; 114 | C.z = A.z + B.z; 115 | } 116 | 117 | static inline void a_plus_b_plus_c_equal_d(const Vector3 &A, const Vector3 &B, 118 | const Vector3 &C, Vector3 &D) { 119 | D.x = A.x + B.x + C.x; 120 | D.y = A.y + B.y + C.y; 121 | D.z = A.z + B.z + C.z; 122 | } 123 | 124 | static inline void a_minus_b_equal_c(const Vector3 &A, const Vector3 &B, 125 | Vector3 &C) { 126 | C.x = A.x - B.x; 127 | C.y = A.y - B.y; 128 | C.z = A.z - B.z; 129 | } 130 | 131 | static inline void a_times_b_equal_c(const Vector3 &A, const REAL B, 132 | Vector3 &C) { 133 | C.x = A.x * B; 134 | C.y = A.y * B; 135 | C.z = A.z * B; 136 | } 137 | 138 | static inline void a_cross_b_equal_c(const Vector3 &A, const Vector3 &B, 139 | Vector3 &C) // cross product 140 | { 141 | C.x = A.y * B.z - A.z * B.y; 142 | C.y = A.z * B.x - A.x * B.z; 143 | C.z = A.x * B.y - A.y * B.x; 144 | } 145 | 146 | static inline void a_divide_b_equal_c(const Vector3 &A, const REAL B, 147 | Vector3 &C) { 148 | C.x = A.x / B; 149 | C.y = A.y / B; 150 | C.z = A.z / B; 151 | } 152 | 153 | static inline void a_plus_b_times_c_equal_d(const Vector3 &A, 154 | const Vector3 &B, const REAL C, 155 | Vector3 &D) { 156 | D.x = (A.x + B.x) * C; 157 | D.y = (A.y + B.y) * C; 158 | D.z = (A.z + B.z) * C; 159 | } 160 | 161 | // It performs the following operation a = v*(I-0.5*t'*t), where v is the 162 | // velocity vector, t is the tangent and I the unit matrix 163 | static inline void projectionSBT(const Vector3 &v, const Vector3 &t, 164 | Vector3 &f) { 165 | const REAL tx = t.x; 166 | const REAL ty = t.y; 167 | const REAL tz = t.z; 168 | 169 | const REAL vx = v.x; 170 | const REAL vy = v.y; 171 | const REAL vz = v.z; 172 | 173 | // t'*t 174 | const REAL a11 = tx * tx; 175 | const REAL a12 = tx * ty; 176 | const REAL a13 = tx * tz; 177 | 178 | const REAL a21 = ty * tx; 179 | const REAL a22 = ty * ty; 180 | const REAL a23 = ty * tz; 181 | 182 | const REAL a31 = tz * tx; 183 | const REAL a32 = tz * ty; 184 | const REAL a33 = tz * tz; 185 | 186 | // v*(I-0.5*t'*t) 187 | f.x = (1 - 0.5 * a11) * vx + (0 - 0.5 * a12) * vy + (0 - 0.5 * a13) * vz; 188 | f.y = (0 - 0.5 * a21) * vx + (1 - 0.5 * a22) * vy + (0 - 0.5 * a23) * vz; 189 | f.z = (0 - 0.5 * a31) * vx + (0 - 0.5 * a32) * vy + (1 - 0.5 * a33) * vz; 190 | } 191 | 192 | // IO (never inline) 193 | friend std::ostream &operator<<(std::ostream &out, const Vector3 &A); 194 | }; 195 | 196 | ///// Additional vector functions not defined as member functions ////// 197 | 198 | Vector3 operator*(REAL a, const Vector3 &V); 199 | 200 | // gets the angle between two vector in range(0, PI) 201 | REAL angle(const Vector3 &A, const Vector3 &B); 202 | 203 | // generates a uniform gaussian random variable in 3 dimensions 204 | Vector3 randVector3(); 205 | Vector3 randVector3(REAL mu, REAL sigma); 206 | 207 | // random only in x direction 208 | Vector3 randXOnly(); 209 | 210 | #endif 211 | -------------------------------------------------------------------------------- /source/Walker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Walker.h 3 | * 4 | * Created on: Jun 22, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #ifndef WALKER_H_ 9 | #define WALKER_H_ 10 | 11 | #include "ArgumentParser.h" 12 | #include "Polymer.h" 13 | #include "PositionVerlet2nd.h" 14 | #include "RodInitialConfigurations.h" 15 | #include "Test.h" 16 | #include "UsualHeaders.h" 17 | 18 | using namespace std; 19 | 20 | class Walker : public Test { 21 | protected: 22 | double YoungM; 23 | double LegL; 24 | double Location; 25 | int NOR; 26 | 27 | REAL _walkerRun(); 28 | 29 | #ifdef SNAKE_VIZ 30 | void _paint(){}; 31 | #endif 32 | 33 | public: 34 | Walker(const int argc, const char **argv); 35 | ~Walker(){}; 36 | 37 | void run(); 38 | void paint(){}; 39 | }; 40 | 41 | #endif /* WALKER_H_ */ 42 | -------------------------------------------------------------------------------- /source/Wing.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MuscularSnake.h 3 | * 4 | * Created on: Jun 22, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #ifndef WING_H_ 9 | #define WING_H_ 10 | 11 | #include "ArgumentParser.h" 12 | #include "Polymer.h" 13 | #include "PositionVerlet2nd.h" 14 | #include "RodInitialConfigurations.h" 15 | #include "Test.h" 16 | #include "UsualHeaders.h" 17 | 18 | using namespace std; 19 | 20 | class Wing : public Test { 21 | protected: 22 | vector amp; 23 | double w, v; 24 | int NOR; 25 | 26 | vector _wingRun(); 27 | 28 | #ifdef SNAKE_VIZ 29 | void _paint(){}; 30 | #endif 31 | 32 | public: 33 | Wing(const int argc, const char **argv); 34 | ~Wing(){}; 35 | 36 | void run(); 37 | void paint(){}; 38 | }; 39 | 40 | #endif /* WING_H_ */ 41 | -------------------------------------------------------------------------------- /source/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * main.cpp 3 | * 4 | * Created on: Jun 22, 2014 5 | * Author: mgazzola 6 | */ 7 | 8 | #include "ArgumentParser.h" 9 | #include "Elbow.h" 10 | #include "FixedJoint.h" 11 | #include "Flagella.h" 12 | #include "HingeJoint.h" 13 | #include "InstabilityHelical.h" 14 | #include "MRAGEnvironment.h" 15 | #include "MuscularSnake.h" 16 | #include "PullingMuscle.h" 17 | #include "QuasistaticTimoshenkoBeam.h" 18 | #include "Snake.h" 19 | #include "SphericalJoint.h" 20 | #include "Test.h" 21 | #include "UsualHeaders.h" 22 | #include "Walker.h" 23 | #include "Wing.h" 24 | 25 | #ifdef SNAKE_VIZ 26 | #ifdef __APPLE__ 27 | #include 28 | #include "GLUT/glut.h" 29 | #else 30 | #include 31 | #endif 32 | #endif 33 | 34 | using namespace std; 35 | 36 | Test *test = NULL; 37 | 38 | #ifdef SNAKE_VIZ 39 | struct VisualSupport { 40 | static void display() {} 41 | 42 | static void idle(void) { 43 | glClear(GL_COLOR_BUFFER_BIT); 44 | test->run(); 45 | glutSwapBuffers(); 46 | } 47 | 48 | static void run(int argc, const char **argv) { 49 | static bool bSetup = false; 50 | 51 | if (!bSetup) { 52 | setup(argc, argv); 53 | bSetup = true; 54 | } 55 | 56 | glutDisplayFunc(display); 57 | glutIdleFunc(idle); 58 | glutMainLoop(); 59 | } 60 | 61 | static void setup(int argc, const char **argv) { 62 | glutInit(&argc, const_cast(argv)); 63 | glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH); 64 | // glutInitWindowSize(1024,1024); 65 | glutInitWindowSize(700, 700); 66 | glutCreateWindow("School"); 67 | glutDisplayFunc(display); 68 | // glClearColor(1,1,1,1); 69 | glClearColor(0, 0, 0, 1); 70 | 71 | glMatrixMode(GL_PROJECTION); 72 | glLoadIdentity(); 73 | glOrtho(0, 1, 0, 1, 0, 1); 74 | glMatrixMode(GL_MODELVIEW); 75 | glLoadIdentity(); 76 | } 77 | }; 78 | #endif 79 | 80 | int main(const int argc, const char **argv) { 81 | MRAG::ArgumentParser parser(argc, argv); 82 | 83 | MRAG::Environment::setup(max(1, parser("-nthreads").asInt())); 84 | 85 | const string studycase = parser("-study").asString(); 86 | 87 | #ifdef FLAGELBOW 88 | test = new Elbow(argc, argv); 89 | #endif 90 | #ifdef FLAGFLAGELLA 91 | test = new Flagella(argc, argv); 92 | #endif 93 | #ifdef FLAGWALKER 94 | test = new Walker(argc, argv); 95 | #endif 96 | #ifdef FLAGMUSCULARSNAKE 97 | test = new MuscularSnake(argc, argv); 98 | #endif 99 | #ifdef FLAGWING 100 | test = new Wing(argc, argv); 101 | #endif 102 | #ifdef FLAGSPHERICALJOINT 103 | test = new SphericalJoint(argc, argv); 104 | #endif 105 | #ifdef FLAGHINGEJOINT 106 | test = new HingeJoint(argc, argv); 107 | #endif 108 | #ifdef FLAGFIXEDJOINT 109 | test = new FixedJoint(argc, argv); 110 | #endif 111 | #ifdef FLAGPULLINGMUSCLE 112 | test = new PullingMuscle(argc, argv); 113 | #endif 114 | #ifdef FLAGQUASISTATICTIMOSHENKOBEAM 115 | test = new QuasistaticTimoshenkoBeam(argc, argv); 116 | #endif 117 | #ifdef FLAGHELICALBUCKLING 118 | test = new InstabilityHelical(argc, argv); 119 | #endif 120 | #ifdef FLAGSNAKE 121 | test = new Snake(argc, argv); 122 | #endif 123 | 124 | try { 125 | #ifdef SNAKE_VIZ 126 | VisualSupport::run(argc, argv); 127 | #else 128 | test->run(); 129 | #endif 130 | 131 | return 0; 132 | } catch (string &exc) { 133 | cout << exc << endl; 134 | } catch (char const *exc) { 135 | cout << exc << endl; 136 | } 137 | } 138 | --------------------------------------------------------------------------------