functions(6);
diff -Nru simbody-3.6.1+dfsg/README.md simbody-3.7+dfsg/README.md
--- simbody-3.6.1+dfsg/README.md 2018-06-12 01:04:33.000000000 +0000
+++ simbody-3.7+dfsg/README.md 2019-12-08 20:33:52.000000000 +0000
@@ -85,7 +85,7 @@
--------------
* **[install Simbody](#installing)**.
* [use Simbody in your own program][user].
-* [view API documentation](https://simbody.github.io/simbody-latest-doxygen).
+* [view API documentation](https://simbody.github.io).
* [learn the theory behind Simbody](https://github.com/simbody/simbody/raw/master/Simbody/doc/SimbodyTheoryManual.pdf).
* [extend Simbody](https://github.com/simbody/simbody/raw/master/Simbody/doc/SimbodyAdvancedProgrammingGuide.pdf).
* [**get support** at the Simbody Forum](https://simtk.org/forums/viewforum.php?f=47).
@@ -100,7 +100,7 @@
Simbody depends on the following:
* cross-platform building: [CMake](http://www.cmake.org/cmake/resources/software.html) 2.8.10 or later (3.1.3 or later for Visual Studio).
-* compiler: [Visual Studio](http://www.visualstudio.com) 2015 or 2017 (Windows only), [gcc](http://gcc.gnu.org/) 4.9.0 or later (typically on Linux), [Clang](http://clang.llvm.org/) 3.4 or later, or Apple Clang (Xcode) 8 or later.
+* compiler: [Visual Studio](http://www.visualstudio.com) 2015, 2017, or 2019 (Windows only), [gcc](http://gcc.gnu.org/) 4.9.0 or later (typically on Linux), [Clang](http://clang.llvm.org/) 3.4 or later, or Apple Clang (Xcode) 8 or later.
* linear algebra: [LAPACK](http://www.netlib.org/lapack/) 3.6.0 or later and [BLAS](http://www.netlib.org/blas/)
* visualization (optional): [FreeGLUT](http://freeglut.sourceforge.net/), [Xi and Xmu](http://www.x.org/wiki/)
* API documentation (optional): [Doxygen](http://www.stack.nl/~dimitri/doxygen/) 1.8.6 or later; we recommend at least 1.8.8.
@@ -123,8 +123,11 @@
2. [**Linux or Mac (make)**](#linux-or-mac-using-make): build from source using gcc or Clang with make.
3. [**Mac (Homebrew)**](#mac-and-homebrew): automated build/install with Homebrew.
4. [**Ubuntu/Debian**](#ubuntu-and-apt-get): install pre-built binaries with apt-get.
-5. [**Windows using MinGW**](#windows-using-mingw): build from source using MinGW.
-6. [**Windows/Mac/Linux**](#windows-mac-and-linux-using-conda): install pre-built binaries with the Conda package manager.
+5. [**FreeBSD**](#freebsd): install pre-built binaries with pkg.
+6. [**Windows using MinGW**](#windows-using-mingw): build from source using MinGW.
+7. [**Windows/Mac/Linux**](#windows-mac-and-linux-using-conda): install pre-built binaries with the Conda package manager.
+
+If you use Linux, check [Repology](https://repology.org/project/simbody/versions) to see if your distribution provides a package for Simbody.
These are not the only ways to install Simbody, however. For example, on a Mac, you could use CMake and Xcode.
@@ -144,9 +147,9 @@
All needed library dependencies are provided with the Simbody installation on Windows, including linear algebra and visualization dependencies.
-1. Download and install [Microsoft Visual Studio](http://www.visualstudio.com), version [2015](https://www.visualstudio.com/vs/older-downloads/) or 2017. The Community edition is free and sufficient.
+1. Download and install [Microsoft Visual Studio](http://www.visualstudio.com), version [2015](https://www.visualstudio.com/vs/older-downloads/), 2017, or 2019. The Community edition is free and sufficient.
* 2015: By default, Visual Studio 2015 does not provide C++ support; when installing, be sure to select *Custom*, and check *Programming Languages > Visual C++ > Common Tools for Visual C++ 2015*. If you have already installed Visual Studio without C++ support, simply re-run the installer and select *Modify*.
- * 2017: In the installer, select the *Desktop development with C++* workload.
+ * 2017 and later: In the installer, select the *Desktop development with C++* workload.
* Any other C++ code you plan to use with Simbody should be compiled with the
same compiler as used for Simbody.
2. Download and install [CMake](http://www.cmake.org/download), version 3.1.3 or higher.
@@ -165,7 +168,7 @@
2. Clone the github repository into `C:/Simbody-source`. Run the following in a Git Bash / Git Shell, or find a way to run the equivalent commands in a GUI client:
$ git clone https://github.com/simbody/simbody.git C:/Simbody-source
- $ git checkout Simbody-3.6.1
+ $ git checkout Simbody-3.7
3. In the last line above, we assumed you want to build a released version.
Feel free to change the version you want to build.
@@ -184,7 +187,7 @@
2. In the field **Where is the source code**, specify `C:/Simbody-source`.
3. In the field **Where to build the binaries**, specify something like `C:/Simbody-build`, just not inside your source directory. This is *not* where we will install Simbody; see below.
4. Click the **Configure** button.
- 1. When prompted to select a *generator*, select one ending with **Win64** to build 64-bit binaries (e.g., **Visual Studio 14 2015 Win64** or **Visual Studio 15 2017 Win64**), or select one *without* **Win64** to build 32-bit binaries (e.g., **Visual Studio 14 2015** or **Visual Studio 15 2017**).
+ 1. When prompted to select a *generator*, in the dropdown for *Optional platform for generator*, choose **x64** to build 64-bit binaries or leave blank to build 32-bit binaries. In older versions of CMake, select a generator ending with **Win64** to build 64-bit binaries (e.g., **Visual Studio 14 2015 Win64** or **Visual Studio 15 2017 Win64**), or select one *without* **Win64** to build 32-bit binaries (e.g., **Visual Studio 14 2015** or **Visual Studio 15 2017**).
2. Click **Finish**.
5. Where do you want to install Simbody on your computer? Set this by changing the `CMAKE_INSTALL_PREFIX` variable. We'll assume you set it to `C:/Simbody`. If you choose a different installation location, make sure to use *yours* where we use `C:/Simbody` below.
6. Play around with the other build options:
@@ -329,7 +332,7 @@
2. Clone the github repository into `~/simbody-source`.
$ git clone https://github.com/simbody/simbody.git ~/simbody-source
- $ git checkout Simbody-3.6.1
+ $ git checkout Simbody-3.7
3. In the last line above, we assumed you want to build a released version.
Feel free to change the version you want to build.
@@ -538,6 +541,17 @@
* `examples/src` source code for the examples.
* `examples/bin` symbolic link to executable examples.
+FreeBSD and pkg
+---------------
+
+Simbody is available via the FreeBSD package repository.
+
+#### Install
+
+1. Open a terminal and run the following command:
+
+ $ sudo pkg install simbody
+
Windows using MinGW
-------------------
Binary files /tmp/tmp7e8khjgn/ooTb6Qc3BR/simbody-3.6.1+dfsg/Simbody/doc/SimbodyAdvancedProgrammingGuide.docx and /tmp/tmp7e8khjgn/spVwKQsF4D/simbody-3.7+dfsg/Simbody/doc/SimbodyAdvancedProgrammingGuide.docx differ
Binary files /tmp/tmp7e8khjgn/ooTb6Qc3BR/simbody-3.6.1+dfsg/Simbody/doc/SimbodyAdvancedProgrammingGuide.pdf and /tmp/tmp7e8khjgn/spVwKQsF4D/simbody-3.7+dfsg/Simbody/doc/SimbodyAdvancedProgrammingGuide.pdf differ
Binary files /tmp/tmp7e8khjgn/ooTb6Qc3BR/simbody-3.6.1+dfsg/Simbody/doc/SimbodyAndMolmodelUserGuide.docx and /tmp/tmp7e8khjgn/spVwKQsF4D/simbody-3.7+dfsg/Simbody/doc/SimbodyAndMolmodelUserGuide.docx differ
Binary files /tmp/tmp7e8khjgn/ooTb6Qc3BR/simbody-3.6.1+dfsg/Simbody/doc/SimbodyAndMolmodelUserGuide.pdf and /tmp/tmp7e8khjgn/spVwKQsF4D/simbody-3.7+dfsg/Simbody/doc/SimbodyAndMolmodelUserGuide.pdf differ
Binary files /tmp/tmp7e8khjgn/ooTb6Qc3BR/simbody-3.6.1+dfsg/Simbody/doc/SimbodyTheoryManual.docx and /tmp/tmp7e8khjgn/spVwKQsF4D/simbody-3.7+dfsg/Simbody/doc/SimbodyTheoryManual.docx differ
Binary files /tmp/tmp7e8khjgn/ooTb6Qc3BR/simbody-3.6.1+dfsg/Simbody/doc/SimbodyTheoryManual.pdf and /tmp/tmp7e8khjgn/spVwKQsF4D/simbody-3.7+dfsg/Simbody/doc/SimbodyTheoryManual.pdf differ
diff -Nru simbody-3.6.1+dfsg/Simbody/include/simbody/internal/SmoothSphereHalfSpaceForce.h simbody-3.7+dfsg/Simbody/include/simbody/internal/SmoothSphereHalfSpaceForce.h
--- simbody-3.6.1+dfsg/Simbody/include/simbody/internal/SmoothSphereHalfSpaceForce.h 1970-01-01 00:00:00.000000000 +0000
+++ simbody-3.7+dfsg/Simbody/include/simbody/internal/SmoothSphereHalfSpaceForce.h 2019-12-08 20:33:52.000000000 +0000
@@ -0,0 +1,221 @@
+#ifndef SimTK_SIMBODY_SMOOTH_SPHERE_HALFSPACE_FORCE_H_
+#define SimTK_SIMBODY_SMOOTH_SPHERE_HALFSPACE_FORCE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) *
+ * -------------------------------------------------------------------------- *
+ * This is part of the SimTK biosimulation toolkit originating from *
+ * Simbios, the NIH National Center for Physics-Based Simulation of *
+ * Biological Structures at Stanford, funded under the NIH Roadmap for *
+ * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
+ * *
+ * Portions copyright (c) 2008-19 Stanford University and the Authors. *
+ * Authors: Antoine Falisse, Gil Serrancoli *
+ * Contributors: Peter Eastman *
+ * *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may *
+ * not use this file except in compliance with the License. You may obtain a *
+ * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
+ * *
+ * Unless required by applicable law or agreed to in writing, software *
+ * distributed under the License is distributed on an "AS IS" BASIS, *
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
+ * See the License for the specific language governing permissions and *
+ * limitations under the License. *
+ * -------------------------------------------------------------------------- */
+
+#include "SimTKcommon.h"
+
+#include "simbody/internal/common.h"
+#include "simbody/internal/Force.h"
+
+namespace SimTK {
+
+class SmoothSphereHalfSpaceForceImpl;
+
+/** This class models the forces generated by simple point contacts between a
+sphere and a half space. The contact model is a smooth (i.e., twice
+continuously differentiable) approximation of the HuntCrossleyForce already
+available in Simbody. The proposed implementation was designed for use with
+gradient-based optimization algorithms and algorithmic/automatic
+differentiation. To this aim, conditional if statements were approximated by
+using hyperbolic tangent functions. For example, the following if statement:
+ y = 0, if x < d
+ y = a, if x >= d
+can be approximated by:
+ f = 0.5 + 0.5 tanh(b(x-d))
+ y = a f
+where b is a parameter that determines the smoothness of the transition.
+
+This force does not rely on a GeneralContactSubsystem. Instead, it assumes
+contact between a sphere and a half space, where the half space is defined as
+x > 0 (that is, all coordinates with positive x are in contact) in half space's
+frame. Commonly, the gravity acceleration vector is (0, -g, 0) (g > 0), and the
+half space represents ground. To achieve this, the half space should be rotated
+-90 degrees about the Z-Axis. This can be done by defining the half space frame
+as: @code Transform exampleHalfSpaceFrame(Rotation(-0.5*Pi, ZAxis), Vec3(0))
+@endcode
+
+The contact model includes components for the normal restoring force,
+dissipation in the material, and surface friction. The force is only applied to
+point contacts.
+
+To use it, do the following:
+
+
+- Add a GeneralForceSubsystem to a MultibodySystem.
+- Add a SmoothSphereHalfSpaceForce to the GeneralForceSubsystem.
+- Add a MobilizedBody for the contact sphere and for the contact half
+space, and call setContactSphereBody(), setContactSphereLocationInBody(),
+setContactSphereRadius(), setContactHalfSpaceBody(),
+setContactHalfSpaceFrame(), and setParameters().
+
+
+Normal Force Components
+
+The normal restoring force (Hertz force) is given by:
+ fh_pos = (4/3) k (R k)^(1/2) ((x^2+cf)^(1/2))^(3/2)
+where k = 0.5 E^(2/3) with E the plain strain modulus, which is assumed
+identical for both contacting materials (i.e., sphere and half space), x is
+penetration depth, R is sphere radius, and cf (default is 1e-5) is a constant
+that enforces non-null derivatives. To smoothly transition between periods with
+and without sphere-half space contact, we use a tanh function:
+ fh_smooth = fh_pos (1/2+(1/2)tanh(bd x))
+where bd (default is 300) is a parameter that determines the smoothness of the
+tanh transition. The graph below compares the smooth approximation with
+respect to the original model.
+
+\htmlonly \endhtmlonly
+@image html SmoothSphereHalfSpaceForce_HertzForce.png "Curves produced using E=1e6, R=0.8, cf=1e-5, and bd=300"
+
+The dissipation force is combined with the normal restoring force
+(Hunt-Crossley force) as follows:
+ fhc_pos = fh_smooth (1+(3/2) c v)
+where c is dissipation and v is penetration rate. To smoothly transition
+between null and positive Hunt-Crossley force, we use a tanh function:
+ fhc_smooth = fhc_pos (1/2+(1/2) tanh(bv (v+(2/(3 c)))))
+where bv (default is 50) is a parameter that determines the smoothness of the
+tanh transition. The graph below compares the smooth approximation with
+respect to the original model.
+
+\htmlonly \endhtmlonly
+@image html SmoothSphereHalfSpaceForce_HuntCrossleyForce.png "Curves produced using x=0.1, E=1e6, R=0.8, c=2, cf=1e-5, bd=300, and bv=50"
+
+Friction Force
+
+The friction force is given by:
+ ff = fhc_smooth [min(vs/vt,1) (ud+2(us-ud)/(1+(vs/vt)^2))+uv vs]
+where vs is the slip velocity of the two bodies at the contact point (see
+below), vt is a transition velocity (see below), and us, ud, and uv are the
+coefficients of static, dynamic, and viscous friction, respectively.
+
+The slip velocity is defined as the norm of the tangential velocity. To
+enforce non-null derivatives, we added the small positive constant cf (default
+is 1e-5):
+ vs = sqrt(vtangent[1]^2 + vtangent[2]^2 + vtangent[3]^2 + cf)
+where vtangent is the tangential velocity.
+
+Because the friction force is a continuous function of the slip velocity, this
+model cannot represent stiction; as long as a tangential force is applied, the
+two bodies will move relative to each other. There will always be a nonzero
+drift, no matter how small the force is. The transition velocity vt acts as an
+upper limit on the drift velocity. By setting vt to a sufficiently small value,
+the drift velocity can be made arbitrarily small, at the cost of making the
+equations of motion very stiff.
+
+Contact Force
+
+The contact force is given by:
+ force = fhc_smooth*normal + ff*vtangent/vs
+where normal determines the direction of the normal to the contact half space
+(the normal points in the direction of contact).
+
+Potential energy
+
+The potential energy is the integral of the normal restoring force (Hertz
+force). Due to the smooth approximation, there is no exact expression for the
+potential energy. We therefore made a comprise by providing an approximation
+for the potential energy. Specifically, we used the original expression but
+replaced the original Hertz force by our smooth approximation as follows:
+ pe = (2/5)*fh_smooth*x
+This approximation results in the error depicted in the graph below when
+differentiating the potential energy with respect to the penetration, the user
+should thus be aware of the limitations of this model that was primarily
+designed for optimization problems.
+
+\htmlonly \endhtmlonly
+@image html SmoothSphereHalfSpaceForce_HertzForceEnergyError.png "Curves produced using E=1e6, R=0.8, cf=1e-5, and bd=300"
+*/
+class SimTK_SIMBODY_EXPORT SmoothSphereHalfSpaceForce : public Force {
+public:
+ /** Create a smooth sphere to half space Hunt-Crossley contact model.
+ @param forces the subsystem that will own this
+ SmoothSphereHalfSpaceForce element */
+ SmoothSphereHalfSpaceForce(GeneralForceSubsystem& forces);
+ /** Set the contact material parameters.
+ @param stiffness the stiffness constant (i.e., plain strain modulus),
+ default is 1 (N/m^2)
+ @param dissipation the dissipation coefficient, default is 0 (s/m)
+ @param staticFriction the coefficient of static friction, default is 0
+ @param dynamicFriction the coefficient of dynamic friction, default is 0
+ @param viscousFriction the coefficient of viscous friction, default is 0
+ @param transitionVelocity the transition velocity, default is 0.01 (m/s)
+ @param cf the constant that enforces non-null derivatives, default is 1e-5
+ @param bd the parameter that determines the smoothness of the transition of
+ the tanh used to smooth the Hertz force, default is 300
+ @param bv the parameter that determines the smoothness of the transition of
+ the tanh used to smooth the Hunt-Crossley force, default is 50 */
+ void setParameters(Real stiffness, Real dissipation, Real staticFriction,
+ Real dynamicFriction, Real viscousFriction, Real transitionVelocity,
+ Real cf, Real bd, Real bv);
+ /** Set the stiffness constant (i.e., plain strain modulus), default is 1
+ (N/m^2). */
+ void setStiffness(Real stiffness);
+ /** Set the dissipation coefficient, default is 0 (s/m). */
+ void setDissipation(Real dissipation);
+ /** Set the coefficient of static friction, default is 0. */
+ void setStaticFriction(Real staticFriction);
+ /** Set the coefficient of dynamic friction, default is 0. */
+ void setDynamicFriction(Real dynamicFriction);
+ /** Set the coefficient of viscous friction, default is 0. */
+ void setViscousFriction(Real viscousFriction);
+ /** Set the transition velocity, default is 0.01 (m/s). */
+ void setTransitionVelocity(Real transitionVelocity);
+ /** Set the constant that enforces non-null derivatives, default is 1e-5.*/
+ void setConstantContactForce(Real cf);
+ /** Set the parameter that determines the smoothness of the transition
+ of the tanh used to smooth the Hertz force. The larger the steeper the
+ transition but also the more discontinuous, default is 300. */
+ void setHertzSmoothing(Real bd);
+ /** Set the parameter that determines the smoothness of the transition
+ of the tanh used to smooth the Hunt-Crossley force. The larger the
+ steeper the transition but also the more discontinuous, default
+ is 50. */
+ void setHuntCrossleySmoothing(Real bv);
+ /** Set the MobilizedBody to which the contact sphere is attached. */
+ void setContactSphereBody(MobilizedBody bodyInput1);
+ /** Set the location of the contact sphere in the body frame. */
+ void setContactSphereLocationInBody(Vec3 locationSphere);
+ /** Set the radius of the contact sphere. */
+ void setContactSphereRadius(Real radius);
+ /** Set the MobilizedBody to which the contact half space is attached. */
+ void setContactHalfSpaceBody(MobilizedBody bodyInput2);
+ /** Set the transform of the contact half space in the body frame. */
+ void setContactHalfSpaceFrame(Transform halfSpaceFrame);
+ /** Get the MobilizedBody to which the contact sphere is attached. */
+ MobilizedBody getBodySphere();
+ /** Get the MobilizedBody to which the contact half space is attached. */
+ MobilizedBody getBodyHalfSpace();
+ /** Get the location of the contact sphere in the body frame. */
+ Vec3 getContactSphereLocationInBody();
+ /** Get the radius of the contact sphere. */
+ Real getContactSphereRadius();
+ /** Get the transform of the contact half space. */
+ Transform getContactHalfSpaceTransform();
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(SmoothSphereHalfSpaceForce,
+ SmoothSphereHalfSpaceForceImpl, Force);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_SMOOTH_SPHERE_HALFSPACE_FORCE_H_
diff -Nru simbody-3.6.1+dfsg/Simbody/include/SimTKsimbody.h simbody-3.7+dfsg/Simbody/include/SimTKsimbody.h
--- simbody-3.6.1+dfsg/Simbody/include/SimTKsimbody.h 2018-06-12 01:04:33.000000000 +0000
+++ simbody-3.7+dfsg/Simbody/include/SimTKsimbody.h 2019-12-08 20:33:52.000000000 +0000
@@ -31,9 +31,9 @@
* -------------------------------------------------------------------------- */
/** @file
-This header file includes all the Simbody header files that need to be
+This header file includes all the Simbody header files that need to be
visible to a compiler processing a Simbody-using compilation unit.\ However,
-user programs should included only the top-level Simbody.h header (which
+user programs should included only the top-level Simbody.h header (which
will include this one). **/
// This should be kept self-contained for backwards compatibility since
@@ -61,6 +61,7 @@
#include "simbody/internal/GeneralForceSubsystem.h"
#include "simbody/internal/HuntCrossleyContact.h"
#include "simbody/internal/HuntCrossleyForce.h"
+#include "simbody/internal/SmoothSphereHalfSpaceForce.h"
#include "simbody/internal/DecorationSubsystem.h"
#include "simbody/internal/TextDataEventReporter.h"
#include "simbody/internal/ObservedPointFitter.h"
diff -Nru simbody-3.6.1+dfsg/Simbody/src/ConstraintImpl.h simbody-3.7+dfsg/Simbody/src/ConstraintImpl.h
--- simbody-3.6.1+dfsg/Simbody/src/ConstraintImpl.h 2018-06-12 01:04:33.000000000 +0000
+++ simbody-3.7+dfsg/Simbody/src/ConstraintImpl.h 2019-12-08 20:33:52.000000000 +0000
@@ -3034,7 +3034,7 @@
const Array_& A_AB,
const Array_& constrainedUDot,
Array_& aerr) const override
-{ getImplementation().calcVelocityDotErrors
+{ getImplementation().calcAccelerationErrors
(state,A_AB,constrainedUDot,aerr); }
void addInAccelerationConstraintForcesVirtual
diff -Nru simbody-3.6.1+dfsg/Simbody/src/SmoothSphereHalfSpaceForce.cpp simbody-3.7+dfsg/Simbody/src/SmoothSphereHalfSpaceForce.cpp
--- simbody-3.6.1+dfsg/Simbody/src/SmoothSphereHalfSpaceForce.cpp 1970-01-01 00:00:00.000000000 +0000
+++ simbody-3.7+dfsg/Simbody/src/SmoothSphereHalfSpaceForce.cpp 2019-12-08 20:33:52.000000000 +0000
@@ -0,0 +1,344 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) *
+ * -------------------------------------------------------------------------- *
+ * This is part of the SimTK biosimulation toolkit originating from *
+ * Simbios, the NIH National Center for Physics-Based Simulation of *
+ * Biological Structures at Stanford, funded under the NIH Roadmap for *
+ * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
+ * *
+ * Portions copyright (c) 2008-19 Stanford University and the Authors. *
+ * Authors: Antoine Falisse, Gil Serrancoli *
+ * Contributors: Peter Eastman *
+ * *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may *
+ * not use this file except in compliance with the License. You may obtain a *
+ * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
+ * *
+ * Unless required by applicable law or agreed to in writing, software *
+ * distributed under the License is distributed on an "AS IS" BASIS, *
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
+ * See the License for the specific language governing permissions and *
+ * limitations under the License. *
+ * -------------------------------------------------------------------------- */
+
+#include "SimTKmath.h"
+
+#include "simbody/internal/common.h"
+#include "simbody/internal/MobilizedBody.h"
+
+#include "SmoothSphereHalfSpaceForceImpl.h"
+
+namespace SimTK {
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(SmoothSphereHalfSpaceForce,
+ SmoothSphereHalfSpaceForceImpl, Force);
+
+SmoothSphereHalfSpaceForce::SmoothSphereHalfSpaceForce
+ (GeneralForceSubsystem& forces) :
+ Force(new SmoothSphereHalfSpaceForceImpl(forces)) {
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+SmoothSphereHalfSpaceForceImpl::SmoothSphereHalfSpaceForceImpl
+ (GeneralForceSubsystem& subsystem) :
+ subsystem(subsystem){
+}
+
+void SmoothSphereHalfSpaceForceImpl::realizeTopology(State& state) const {
+ energyCacheIndex=state.allocateCacheEntry(subsystem.getMySubsystemIndex(),
+ Stage::Dynamics, new Value());
+}
+
+void SmoothSphereHalfSpaceForce::setParameters
+ (Real stiffness, Real dissipation, Real staticFriction,
+ Real dynamicFriction, Real viscousFriction, Real transitionVelocity, Real
+ cf, Real bd, Real bv) {
+ updImpl().setParameters(stiffness, dissipation, staticFriction,
+ dynamicFriction, viscousFriction, transitionVelocity, cf, bd, bv);
+}
+
+void SmoothSphereHalfSpaceForceImpl::setParameters
+ (Real stiffness, Real dissipation, Real staticFriction,
+ Real dynamicFriction, Real viscousFriction, Real transitionVelocity, Real
+ cf, Real bd, Real bv) {
+ updParameters() = Parameters(stiffness, dissipation, staticFriction,
+ dynamicFriction, viscousFriction, transitionVelocity, cf, bd, bv);
+}
+
+void SmoothSphereHalfSpaceForce::setStiffness(Real stiffness) {
+ updImpl().parameters.stiffness = stiffness;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setStiffness(Real stiffness) {
+ parameters.stiffness = stiffness;
+}
+
+void SmoothSphereHalfSpaceForce::setDissipation(Real dissipation) {
+ updImpl().parameters.dissipation = dissipation;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setDissipation(Real dissipation) {
+ parameters.dissipation = dissipation;
+}
+
+void SmoothSphereHalfSpaceForce::setStaticFriction(Real staticFriction) {
+ updImpl().parameters.staticFriction = staticFriction;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setStaticFriction(Real staticFriction) {
+ parameters.staticFriction = staticFriction;
+}
+
+void SmoothSphereHalfSpaceForce::setDynamicFriction(Real dynamicFriction) {
+ updImpl().parameters.dynamicFriction = dynamicFriction;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setDynamicFriction(Real dynamicFriction) {
+ parameters.dynamicFriction = dynamicFriction;
+}
+
+void SmoothSphereHalfSpaceForce::setViscousFriction(Real viscousFriction) {
+ updImpl().parameters.viscousFriction = viscousFriction;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setViscousFriction(Real viscousFriction) {
+ parameters.viscousFriction = viscousFriction;
+}
+
+void SmoothSphereHalfSpaceForce::setTransitionVelocity(
+ Real transitionVelocity) {
+ updImpl().parameters.transitionVelocity = transitionVelocity;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setTransitionVelocity
+ (Real transitionVelocity) {
+ parameters.transitionVelocity = transitionVelocity;
+}
+
+void SmoothSphereHalfSpaceForce::setConstantContactForce(Real cf) {
+ updImpl().parameters.cf = cf;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setConstantContactForce(Real cf) {
+ parameters.cf = cf;
+}
+
+void SmoothSphereHalfSpaceForce::setHertzSmoothing(Real bd) {
+ updImpl().parameters.bd = bd;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setHertzSmoothing(Real bd) {
+ parameters.bd = bd;
+}
+
+void SmoothSphereHalfSpaceForce::setHuntCrossleySmoothing(Real bv) {
+ updImpl().parameters.bv = bv;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setHuntCrossleySmoothing(Real bv) {
+ parameters.bv = bv;
+}
+
+void SmoothSphereHalfSpaceForce::setContactSphereBody(
+ MobilizedBody bodyInput1) {
+ updImpl().bodySphere = bodyInput1;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setContactSphereBody(
+ MobilizedBody bodyInput1){
+ bodySphere = bodyInput1;
+}
+
+void SmoothSphereHalfSpaceForce::setContactHalfSpaceBody(
+ MobilizedBody bodyInput2) {
+ updImpl().bodyHalfSpace = bodyInput2;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setContactHalfSpaceBody(
+ MobilizedBody bodyInput2){
+ bodyHalfSpace = bodyInput2;
+}
+
+void SmoothSphereHalfSpaceForce::setContactSphereLocationInBody(
+ Vec3 contactSphereLocation) {
+ updImpl().contactSphereLocation = contactSphereLocation;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setContactSphereLocationInBody(
+ Vec3 contactSphereLocation) {
+ contactSphereLocation = contactSphereLocation;
+}
+
+void SmoothSphereHalfSpaceForce::setContactHalfSpaceFrame(
+ Transform halfSpaceFrame) {
+ updImpl().contactHalfSpaceFrame = halfSpaceFrame;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setContactHalfSpaceFrame(
+ Transform halfSpaceFrame) {
+ contactHalfSpaceFrame = halfSpaceFrame;
+}
+
+void SmoothSphereHalfSpaceForce::setContactSphereRadius(Real radius) {
+ updImpl().contactSphereRadius = radius;
+}
+
+void SmoothSphereHalfSpaceForceImpl::setContactSphereRadius(Real radius) {
+ contactSphereRadius = radius;
+}
+
+MobilizedBody SmoothSphereHalfSpaceForce::getBodySphere() {
+ return updImpl().bodySphere;
+}
+
+MobilizedBody SmoothSphereHalfSpaceForceImpl::getBodySphere() {
+ return bodySphere;
+}
+
+MobilizedBody SmoothSphereHalfSpaceForce::getBodyHalfSpace() {
+ return updImpl().bodyHalfSpace;
+}
+
+MobilizedBody SmoothSphereHalfSpaceForceImpl::getBodyHalfSpace() {
+ return bodyHalfSpace;
+}
+
+Vec3 SmoothSphereHalfSpaceForce::getContactSphereLocationInBody() {
+ return updImpl().contactSphereLocation;
+}
+
+Vec3 SmoothSphereHalfSpaceForceImpl::getContactSphereLocationInBody() {
+ return contactSphereLocation;
+}
+
+Real SmoothSphereHalfSpaceForce::getContactSphereRadius() {
+ return updImpl().contactSphereRadius;
+}
+
+Real SmoothSphereHalfSpaceForceImpl::getContactSphereRadius() {
+ return contactSphereRadius;
+}
+
+Transform SmoothSphereHalfSpaceForce::getContactHalfSpaceTransform() {
+ return updImpl().contactHalfSpaceFrame;
+}
+
+Transform SmoothSphereHalfSpaceForceImpl::getContactHalfSpaceTransform() {
+ return contactHalfSpaceFrame;
+}
+
+const SmoothSphereHalfSpaceForceImpl::Parameters&
+ SmoothSphereHalfSpaceForceImpl::getParameters() const {
+ return parameters;
+}
+
+SmoothSphereHalfSpaceForceImpl::Parameters& SmoothSphereHalfSpaceForceImpl::
+ updParameters() {
+ return parameters;
+}
+
+void SmoothSphereHalfSpaceForceImpl::getNormalContactHalfSpace(
+ const State& state, UnitVec3& normalContactHalfSpace) const {
+ normalContactHalfSpace =
+ (bodyHalfSpace.getBodyRotation(state)*contactHalfSpaceFrame.x());
+}
+
+void SmoothSphereHalfSpaceForceImpl::getContactSphereOrigin(const State& state,
+ Vec3& contactSphereOrigin) const {
+ contactSphereOrigin =
+ bodySphere.getBodyTransform(state) * contactSphereLocation;
+}
+
+void SmoothSphereHalfSpaceForceImpl::calcForce(const State& state,
+ Vector_& bodyForces, Vector_& particleForces,
+ Vector& mobilityForces) const {
+ // Calculate the indentation.
+ const Vec3 contactSphereLocationInBodyHalfSpace =
+ bodySphere.findStationLocationInAnotherBody(
+ state,contactSphereLocation,bodyHalfSpace);
+ const Vec3 distanceSphereHalfSpaceInBodyHalfSpace =
+ contactSphereLocationInBodyHalfSpace - contactHalfSpaceFrame.p();
+ const Real indentation = -(dot(distanceSphereHalfSpaceInBodyHalfSpace,
+ -contactHalfSpaceFrame.x()) - contactSphereRadius);
+
+ // Initialize the potential energy.
+ Real& pe = Value::updDowncast(state.updCacheEntry(
+ subsystem.getMySubsystemIndex(), energyCacheIndex)).upd();
+ pe = 0.0;
+
+ // Calculate the contact point location (in ground).
+ Vec3 contactSphereOriginInGround;
+ getContactSphereOrigin(state, contactSphereOriginInGround);
+ UnitVec3 normal; // the normal is pointing in the direction of contact
+ getNormalContactHalfSpace(state, normal);
+ const Vec3 contactPointPositionInGround = contactSphereOriginInGround +
+ contactSphereRadius*normal;
+ // Adjust the contact location based on the relative stiffness of the two
+ // materials. Here we assume, as in the original Simbody Hunt-Crossley
+ // contact model, that both materials have the same relative stiffness.
+ // As described in Sherman et al. (2011), the point of contact will then be
+ // located midway between the two surfaces. We therefore need to subtract
+ // (subtraction due to normal direction) half the indentation to the
+ // contact location that was determined as the location of the contact
+ // sphere center plus its radius (plus due to normal direction).
+ const Vec3 contactPointPositionAdjustedInGround =
+ contactPointPositionInGround-Real(1./2.)*indentation*normal;
+ // Calculate the contact point velocity.
+ const Vec3 station1 = bodySphere.findStationAtGroundPoint(state,
+ contactPointPositionAdjustedInGround);
+ const Vec3 station2 = bodyHalfSpace.findStationAtGroundPoint(state,
+ contactPointPositionAdjustedInGround);
+ const Vec3 v1 = bodySphere.findStationVelocityInGround(state, station1);
+ const Vec3 v2 = bodyHalfSpace.findStationVelocityInGround(state, station2);
+ const Vec3 v = v1-v2;
+ // Calculate the normal and tangential velocities.
+ const Real vnormal = dot(v, normal);
+ const Vec3 vtangent = v - vnormal*normal;
+ // Get the contact model parameters.
+ const Parameters parameters = getParameters();
+ const Real stiffness = parameters.stiffness;
+ const Real dissipation = parameters.dissipation;
+ const Real vt = parameters.transitionVelocity;
+ const Real us = parameters.staticFriction;
+ const Real ud = parameters.dynamicFriction;
+ const Real uv = parameters.viscousFriction;
+ const Real cf = parameters.cf;
+ const Real bd = parameters.bd;
+ const Real bv = parameters.bv;
+ // Calculate the Hertz force.
+ const Real k = (1./2.)*std::pow(stiffness,2./3.);
+ const Real fh_pos = (4./3.)*k*std::sqrt(contactSphereRadius*k)*
+ std::pow(std::sqrt(indentation*indentation+cf),3./2.);
+ const Real fh_smooth = fh_pos*(1./2.+(1./2.)*std::tanh(bd*indentation));
+ // Calculate the potential energy.
+ // The potential energy is the integral of the Hertz force. Due to the
+ // smooth approximation, there is no exact expression for the potential
+ // energy. Here we provide an approximation based on the original
+ // expression (i.e., pe = Real(2./5.)*fHertz*indentation) where we replace
+ // fHertz by the smooth approximation (i.e., fh_smooth).
+ pe += Real(2./5.)*fh_smooth*indentation;
+ // Calculate the Hunt-Crossley force.
+ const Real c = dissipation;
+ const Real fhc_pos = fh_smooth*(1.+(3./2.)*c*vnormal);
+ const Real fhc_smooth = fhc_pos*(1./2.+(1./2.)*std::tanh(
+ bv*(vnormal+(2./(3.*c)))));
+ Vec3 force = fhc_smooth*normal;
+ // Calculate the friction force.
+ const Real aux = vtangent.normSqr()+cf;
+ const Real vslip = pow(aux,1./2.);
+ const Real vrel = vslip / vt;
+ const Real ff = fhc_smooth*(std::min(vrel,Real(1))*
+ (ud+2*(us-ud)/(1+vrel*vrel))+uv*vslip);
+ force += ff*(vtangent) / vslip;
+ // Apply the force to the bodies.
+ bodySphere.applyForceToBodyPoint(state, station1, -force, bodyForces);
+ bodyHalfSpace.applyForceToBodyPoint(state, station2, force, bodyForces);
+}
+
+Real SmoothSphereHalfSpaceForceImpl::calcPotentialEnergy(const State& state)
+ const { return Value::downcast(state.getCacheEntry(
+ subsystem.getMySubsystemIndex(), energyCacheIndex)).get();
+}
+
+} // namespace SimTK
+
diff -Nru simbody-3.6.1+dfsg/Simbody/src/SmoothSphereHalfSpaceForceImpl.h simbody-3.7+dfsg/Simbody/src/SmoothSphereHalfSpaceForceImpl.h
--- simbody-3.6.1+dfsg/Simbody/src/SmoothSphereHalfSpaceForceImpl.h 1970-01-01 00:00:00.000000000 +0000
+++ simbody-3.7+dfsg/Simbody/src/SmoothSphereHalfSpaceForceImpl.h 2019-12-08 20:33:52.000000000 +0000
@@ -0,0 +1,135 @@
+#ifndef SimTK_SIMBODY_SMOOTH_SPHERE_HALFSPACE_FORCE_IMPL_H_
+#define SimTK_SIMBODY_SMOOTH_SPHERE_HALFSPACE_FORCE_IMPL_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) *
+ * -------------------------------------------------------------------------- *
+ * This is part of the SimTK biosimulation toolkit originating from *
+ * Simbios, the NIH National Center for Physics-Based Simulation of *
+ * Biological Structures at Stanford, funded under the NIH Roadmap for *
+ * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
+ * *
+ * Portions copyright (c) 2008-19 Stanford University and the Authors. *
+ * Authors: Antoine Falisse, Gil Serrancoli *
+ * Contributors: Peter Eastman *
+ * *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may *
+ * not use this file except in compliance with the License. You may obtain a *
+ * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
+ * *
+ * Unless required by applicable law or agreed to in writing, software *
+ * distributed under the License is distributed on an "AS IS" BASIS, *
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
+ * See the License for the specific language governing permissions and *
+ * limitations under the License. *
+ * -------------------------------------------------------------------------- */
+
+#include "SimTKcommon.h"
+#include "simbody/internal/common.h"
+#include "simbody/internal/SmoothSphereHalfSpaceForce.h"
+#include "ForceImpl.h"
+
+namespace SimTK {
+
+class SmoothSphereHalfSpaceForceImpl : public ForceImpl {
+public:
+ class Parameters {
+ public:
+ Parameters() : stiffness(1), dissipation(0), staticFriction(0),
+ dynamicFriction(0), viscousFriction(0), transitionVelocity(0.01),
+ cf(1e-5), bd(300), bv(50) {
+ }
+ Parameters(Real stiffness, Real dissipation, Real staticFriction,
+ Real dynamicFriction, Real viscousFriction,
+ Real transitionVelocity, Real cf, Real bd, Real bv) :
+ stiffness(stiffness), dissipation(dissipation),
+ staticFriction(staticFriction), dynamicFriction(dynamicFriction),
+ viscousFriction(viscousFriction),
+ transitionVelocity(transitionVelocity), cf(cf), bd(bd), bv(bv) {
+ }
+ Real stiffness, dissipation, staticFriction, dynamicFriction,
+ viscousFriction, transitionVelocity, cf, bd, bv;
+ };
+
+ Real contactSphereRadius;
+ Vec3 contactSphereLocation;
+ Transform contactHalfSpaceFrame;
+ MobilizedBody bodySphere;
+ MobilizedBody bodyHalfSpace;
+ Parameters parameters;
+
+ SmoothSphereHalfSpaceForceImpl(GeneralForceSubsystem& subsystem);
+
+ SmoothSphereHalfSpaceForceImpl* clone() const override {
+ return new SmoothSphereHalfSpaceForceImpl(*this);
+ }
+ // Set the contact material parameters.
+ void setParameters(Real stiffness, Real dissipation, Real staticFriction,
+ Real dynamicFriction, Real viscousFriction, Real transitionVelocity,
+ Real cf, Real bd, Real bv);
+ // Get parameters.
+ const Parameters& getParameters() const;
+ // Update parameters.
+ Parameters& updParameters();
+ // Set the stiffness constant (i.e., plain strain modulus), default is 1
+ // N/m^2.
+ void setStiffness(Real stiffness);
+ // Set the dissipation coefficient, default is 0 s/m.
+ void setDissipation(Real dissipation);
+ // Set the coefficient of static friction, default is 0.
+ void setStaticFriction(Real staticFriction);
+ // Set the coefficient of dynamic friction, default is 0.
+ void setDynamicFriction(Real dynamicFriction);
+ // Set the coefficient of viscous friction, default is 0.
+ void setViscousFriction(Real viscousFriction);
+ // Set the transition velocity, default is 0.01 m/s.
+ void setTransitionVelocity(Real transitionVelocity);
+ // Set the constant that enforces non-null derivatives, default is 1e-5.
+ void setConstantContactForce(Real cf);
+ // Set the parameter that determines the smoothness of the transition
+ // of the tanh used to smooth the Hertz force. The larger the steeper the
+ // transition but also the more discontinuous, default is 300.
+ void setHertzSmoothing(Real bd);
+ // Set the parameter that determines the smoothness of the transition of
+ // the tanh used to smooth the Hunt-Crossley force. The larger the steeper
+ // the transition but also the more discontinuous, default is 50.
+ void setHuntCrossleySmoothing(Real bv);
+ // Set the MobilizedBody to which the contact sphere is attached.
+ void setContactSphereBody(MobilizedBody bodyInput1);
+ // Set the location of the contact sphere in the body frame.
+ void setContactSphereLocationInBody(Vec3 locationContactSphere);
+ // Set the radius of the contact sphere.
+ void setContactSphereRadius(Real radius);
+ // Set the MobilizedBody to which the contact half space is attached.
+ void setContactHalfSpaceBody(MobilizedBody bodyInput2);
+ // Set the transform of the contact half space in the body frame.
+ void setContactHalfSpaceFrame(Transform halfSpaceFrame);
+ // Get the MobilizedBody to which the contact sphere is attached.
+ MobilizedBody getBodySphere();
+ // Get the MobilizedBody to which the contact half space is attached.
+ MobilizedBody getBodyHalfSpace();
+ // Get the location of the contact sphere in the body frame.
+ Vec3 getContactSphereLocationInBody();
+ // Get the radius of the contact sphere.
+ Real getContactSphereRadius();
+ // Get the transform of the contact half space.
+ Transform getContactHalfSpaceTransform();
+ // Get the normal to the contact half space.
+ void getNormalContactHalfSpace(const State& state,
+ UnitVec3& normalContactHalfSpace) const;
+ // Get the location of the contact sphere origin in the ground frame.
+ void getContactSphereOrigin(const State& state,Vec3& contactPointPos)const;
+ // Calculate contact force.
+ void calcForce(const State& state, Vector_& bodyForces,
+ Vector_& particleForces, Vector& mobilityForces) const override;
+ // Calculate potential energy.
+ Real calcPotentialEnergy(const State& state) const override;
+ void realizeTopology(State& state) const override;
+private:
+ const GeneralForceSubsystem& subsystem;
+ mutable CacheEntryIndex energyCacheIndex;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_SMOOTH_SPHERE_HALFSPACE_FORCE_IMPL_H_
diff -Nru simbody-3.6.1+dfsg/Simbody/tests/TestSmoothSphereHalfSpaceForce.cpp simbody-3.7+dfsg/Simbody/tests/TestSmoothSphereHalfSpaceForce.cpp
--- simbody-3.6.1+dfsg/Simbody/tests/TestSmoothSphereHalfSpaceForce.cpp 1970-01-01 00:00:00.000000000 +0000
+++ simbody-3.7+dfsg/Simbody/tests/TestSmoothSphereHalfSpaceForce.cpp 2019-12-08 20:33:52.000000000 +0000
@@ -0,0 +1,166 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) *
+ * -------------------------------------------------------------------------- *
+ * This is part of the SimTK biosimulation toolkit originating from *
+ * Simbios, the NIH National Center for Physics-Based Simulation of *
+ * Biological Structures at Stanford, funded under the NIH Roadmap for *
+ * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
+ * *
+ * Portions copyright (c) 2008-19 Stanford University and the Authors. *
+ * Authors: Antoine Falisse, Gil Serrancoli *
+ * Contributors: Peter Eastman *
+ * *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may *
+ * not use this file except in compliance with the License. You may obtain a *
+ * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
+ * *
+ * Unless required by applicable law or agreed to in writing, software *
+ * distributed under the License is distributed on an "AS IS" BASIS, *
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
+ * See the License for the specific language governing permissions and *
+ * limitations under the License. *
+ * -------------------------------------------------------------------------- */
+
+#include "SimTKsimbody.h"
+
+using namespace SimTK;
+using namespace std;
+
+const Real TOL = 1e-10;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+template
+void assertEqual(T val1, T val2) {
+ ASSERT(abs(val1-val2) < TOL);
+}
+
+template
+void assertEqual(Vec val1, Vec val2) {
+ for (int i = 0; i < N; ++i)
+ ASSERT(abs(val1[i]-val2[i]) < TOL);
+}
+
+void testForces() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ const Vec3 gravity = Vec3(0, -9.8, 0);
+ Force::UniformGravity(forces, matter, gravity, 0);
+ const Real radius = 0.8;
+ const Real k = 1500.0;
+ const Real stiffness = 0.5*std::pow(k, 2.0/3.0);
+ const Real dissipation = 0.5;
+ const Real us = 1.0;
+ const Real ud = 0.5;
+ const Real uv = 0.1;
+ const Real vt = 0.001;
+ const Real cf = 1e-5;
+ const Real bd = 300;
+ const Real bv = 50;
+
+ Body::Rigid body1(MassProperties(1.0, Vec3(0), Inertia(1)));
+ MobilizedBody::Translation sphere(matter.updGround(),
+ Transform(), body1, Transform());
+
+ Body::Rigid body2(MassProperties(1.0, Vec3(0), Inertia(1)));
+ MobilizedBody::Free halfSpace(matter.updGround(),
+ Transform(), body2, Transform());
+
+ SmoothSphereHalfSpaceForce hc_smooth(forces);
+
+ hc_smooth.setParameters(k,dissipation,us,ud,uv,vt,cf,bd,bv);
+ hc_smooth.setContactSphereBody(sphere);
+ hc_smooth.setContactSphereLocationInBody(Vec3(0));
+ hc_smooth.setContactSphereRadius(radius);
+ Transform testFrame(Rotation(-0.5*Pi, ZAxis), Vec3(0));
+ hc_smooth.setContactHalfSpaceFrame(testFrame);
+ hc_smooth.setContactHalfSpaceBody(halfSpace);
+ State state = system.realizeTopology();
+ // Position the sphere at a variety of positions and see if the normal
+ // force and potential energy are correct.
+ for (Real height = radius+0.2; height > 0; height -= 0.1) {
+ sphere.setQToFitTranslation(state, Vec3(0, height, 0));
+ system.realize(state, Stage::Dynamics);
+ const Real depth = radius-height;
+ Real f = (4./3.)*stiffness*std::pow(std::sqrt(depth*depth+cf),3./2.)
+ *std::sqrt(radius*stiffness);
+ Real f_smooth = f*(1./2.+(1./2.)*std::tanh(bd*depth));
+ assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)
+ [sphere.getMobilizedBodyIndex()][1], gravity+Vec3(0, f_smooth, 0));
+ assertEqual(hc_smooth.calcPotentialEnergyContribution(state),
+ (2./5.)*f_smooth*depth);
+ }
+
+ // Now do it with a vertical velocity and see if the dissipation force is
+ // correct.
+ for (Real height = radius+0.2; height > 0; height -= 0.1) {
+ sphere.setQToFitTranslation(state, Vec3(0, height, 0));
+ const Real depth = radius-height;
+ Real fh = (4./3.)*stiffness*std::pow(std::sqrt(depth*depth+cf),3./2.)
+ *std::sqrt(radius*stiffness);
+ Real fh_smooth = fh*(1./2.+(1./2.)*std::tanh(bd*depth));
+
+ for (Real v = -1.0; v <= 1.0; v += 0.1) {
+ sphere.setUToFitLinearVelocity(state, Vec3(0, -v, 0));
+ system.realize(state, Stage::Dynamics);
+ Real f = fh_smooth*(1.+(3./2.)*dissipation*v);
+ Real f_smooth = f*(1./2.+(1./2.)
+ *std::tanh(bv*(v+(2./(3.*dissipation)))));
+ assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)
+ [sphere.getMobilizedBodyIndex()][1],
+ gravity+Vec3(0, f_smooth, 0));
+ }
+ }
+
+ // Now do it with a horizontal velocity and see if the friction force is
+ // correct.
+ Vector_ expectedForce(matter.getNumBodies());
+ for (Real height = radius+0.2; height > 0; height -= 0.1) {
+ sphere.setQToFitTranslation(state, Vec3(0, height, 0));
+ const Real depth = radius-height;
+ Real fh = (4./3.)*stiffness*std::pow(std::sqrt(depth*depth+cf),3./2.)
+ *std::sqrt(radius*stiffness);
+ Real fh_smooth = fh*(1./2.+(1./2.)*std::tanh(bd*depth));
+
+ for (Real v = -1.0; v <= 1.0; v += 0.1) {
+ sphere.setUToFitLinearVelocity(state, Vec3(v, 0, 0));
+ system.realize(state, Stage::Dynamics);
+ Vec3 vec3v(v,0,0);
+ UnitVec3 normal = (halfSpace.getBodyRotation(state)*testFrame.x());
+ Real vnormal = dot(vec3v, normal);
+ Vec3 vtangent = vec3v - vnormal*normal;
+ Real aux = vtangent.normSqr() + cf;
+ Real vslip = pow(aux,1./2.);
+ Real vrel = vslip / vt;
+ Real ff_smooth_scalar = fh_smooth*(std::min(vrel,Real(1))*
+ (ud+2*(us-ud)/(1+vrel*vrel))+uv*vslip);
+ Vec3 ff_smooth = ff_smooth_scalar*(vtangent) / vslip;
+ const Vec3 totalForceOnSphere =
+ gravity - ff_smooth - fh_smooth*normal;
+ expectedForce = SpatialVec(Vec3(0), Vec3(0));
+ Vec3 contactPointInSphere = sphere.findStationAtGroundPoint(state,
+ Vec3(0, -stiffness*depth/(stiffness+stiffness), 0));
+ sphere.applyForceToBodyPoint(state, contactPointInSphere,
+ totalForceOnSphere, expectedForce);
+ SpatialVec actualForce = system.getRigidBodyForces(state,
+ Stage::Dynamics)[sphere.getMobilizedBodyIndex()];
+ assertEqual(actualForce[0],
+ expectedForce[sphere.getMobilizedBodyIndex()][0]);
+ assertEqual(actualForce[1],
+ expectedForce[sphere.getMobilizedBodyIndex()][1]);
+ }
+ }
+}
+
+int main() {
+ try {
+ testForces();
+ }
+ catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff -Nru simbody-3.6.1+dfsg/Simbody/Visualizer/simbody-visualizer/CMakeLists.txt simbody-3.7+dfsg/Simbody/Visualizer/simbody-visualizer/CMakeLists.txt
--- simbody-3.6.1+dfsg/Simbody/Visualizer/simbody-visualizer/CMakeLists.txt 2018-06-12 01:04:33.000000000 +0000
+++ simbody-3.7+dfsg/Simbody/Visualizer/simbody-visualizer/CMakeLists.txt 2019-12-08 20:33:52.000000000 +0000
@@ -38,7 +38,7 @@
if(OPENGL_FOUND AND SIMBODY_HAS_GLUT)
-add_executable(${GUI_NAME}
+add_executable(${GUI_NAME} MACOSX_BUNDLE
simbody-visualizer.cpp lodepng.cpp lodepng.h
${GLUT32_HEADERS}) # only on Windows
@@ -49,9 +49,11 @@
# If building as debug, append the debug postfix to the name of the executable.
# CMAKE_DEBUG_POSTFIX only affects non-executable targets, but we use its value
# to set the postfix for this executable.
+# Setting the target property DEBUG_POSTFIX does not work for MACOSX bundles,
+# so we use DEBUG_OUTPUT_NAME instead.
set_target_properties(${GUI_NAME} PROPERTIES
PROJECT_LABEL "Code - ${GUI_NAME}"
- DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX})
+ DEBUG_OUTPUT_NAME ${GUI_NAME}${CMAKE_DEBUG_POSTFIX})
# On OSX, bake the relative path to the Simbody libraries into the visualizer
# executable. Then there's no need to set `DYLD_LIBRARY_PATH` to find the
@@ -61,9 +63,15 @@
# Linux we'll have to revisit.
# vis_dir_to_install_dir is most likely "../"
+ if (APPLE)
+ set(vis_install_dir
+ "${SIMBODY_VISUALIZER_INSTALL_DIR}/simbody-visualizer.app/Contents/MacOS")
+ else()
+ set(vis_install_dir "${SIMBODY_VISUALIZER_INSTALL_DIR}")
+ endif()
file(RELATIVE_PATH vis_dir_to_install_dir
- "${SIMBODY_VISUALIZER_INSTALL_DIR}"
- "${CMAKE_INSTALL_PREFIX}")
+ "${vis_install_dir}"
+ "${CMAKE_INSTALL_PREFIX}")
set(vis_dir_to_lib_dir "${vis_dir_to_install_dir}${CMAKE_INSTALL_LIBDIR}")
set_target_properties(${GUI_NAME} PROPERTIES
INSTALL_RPATH "\@executable_path/${vis_dir_to_lib_dir}"
diff -Nru simbody-3.6.1+dfsg/Simbody/Visualizer/simbody-visualizer/simbody-visualizer.cpp simbody-3.7+dfsg/Simbody/Visualizer/simbody-visualizer/simbody-visualizer.cpp
--- simbody-3.6.1+dfsg/Simbody/Visualizer/simbody-visualizer/simbody-visualizer.cpp 2018-06-12 01:04:33.000000000 +0000
+++ simbody-3.7+dfsg/Simbody/Visualizer/simbody-visualizer/simbody-visualizer.cpp 2019-12-08 20:33:52.000000000 +0000
@@ -138,6 +138,22 @@
// and can no longer write to the simulator.
static std::atomic writeToSimulator{true};
+// On Mac, if the simbody-visualizer.app/Contents/MacOS/Info.plist's field
+// NSHighResolutionCapable is set to true, then this function is expected
+// to return 2.0 for high-DPI screens.
+// This function could also be used for other operating systems that support
+// high-DPI screens. For now, this is just a stub.
+// https://developer.apple.com/library/archive/documentation/GraphicsImaging/Conceptual/OpenGL-MacProgGuide/EnablingOpenGLforHighResolution/EnablingOpenGLforHighResolution.html#//apple_ref/doc/uid/TP40001987-CH1001-SW4
+static int getScalingFactor() {
+ #if defined(__APPLE__)
+ // If we figure out a way to get the "backing scale factor", we can increase
+ // this number to support high-DPI displays.
+ return 1;
+ #else
+ return 1;
+ #endif
+}
+
static void computeBoundingSphereForVertices(const vector& vertices, float& radius, fVec3& center) {
fVec3 lower(vertices[0], vertices[1], vertices[2]);
fVec3 upper = lower;
@@ -269,7 +285,7 @@
void draw(bool setColor = true) {
if (setColor)
glColor3d(color[0], color[1], color[2]);
- glLineWidth(thickness);
+ glLineWidth(getScalingFactor() * thickness);
glBindBuffer(GL_ARRAY_BUFFER, 0);
glVertexPointer(3, GL_FLOAT, 0, &lines[0]);
glDrawArrays(GL_LINES, 0, (GLsizei)(lines.size()/3));
@@ -457,7 +473,7 @@
virtual void execute() = 0;
};
-static int viewWidth, viewHeight;
+static int viewWidth, viewHeight, viewWidthPixels, viewHeightPixels;
static GLfloat fieldOfView = GLfloat(SimTK_PI/4);
static GLfloat nearClip = 1;
static GLfloat farClip = 1000;
@@ -1284,7 +1300,8 @@
groundImage[i*width+j] = float(std::pow(line,.1)*(.35f+noise));
}
}
- glTexImage2D(GL_TEXTURE_2D, 0, 1, width, width, 0, GL_RED, GL_FLOAT, groundImage);
+ glTexImage2D(GL_TEXTURE_2D, 0, 1, width, width, 0, GL_RED, GL_FLOAT,
+ groundImage);
}
// Draw the box to represent the sky.
@@ -1443,7 +1460,7 @@
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
- glViewport(0, 0, viewWidth, viewHeight);
+ glViewport(0, 0, viewWidthPixels, viewHeightPixels);
float sceneRadius;
fVec3 sceneCenter;
// Scene is already locked so OK to call this.
@@ -1480,10 +1497,10 @@
drawGroundAndSky(farClipDistance);
for (int i = 0; i < (int) scene->lines.size(); i++)
scene->lines[i].draw();
- glLineWidth(2);
+ glLineWidth(getScalingFactor() * 2);
for (int i = 0; i < (int) scene->sceneText.size(); i++)
scene->sceneText[i].draw();
- glLineWidth(1);
+ glLineWidth(getScalingFactor() * 1);
glEnableClientState(GL_NORMAL_ARRAY);
for (int i = 0; i < (int) scene->drawnMeshes.size(); i++)
scene->drawnMeshes[i].draw();
@@ -1942,8 +1959,8 @@
};
static void writeImage(const string& filename) {
- int width = ((viewWidth+3)/4)*4; // must be a multiple of 4 pixels
- int height = viewHeight;
+ int width = ((viewWidthPixels+3)/4)*4; // must be a multiple of 4 pixels
+ int height = viewHeightPixels;
// Create offscreen buffers for rendering the image.
@@ -2647,12 +2664,15 @@
static const int DefaultWindowHeight = 600;
-// The glut callback for chaning window size.
+// The glut callback for changing window size.
static void changeSize(int width, int height) {
if (height == 0)
height = 1;
viewWidth = width;
viewHeight = height;
+ viewWidthPixels = getScalingFactor() * width;
+ viewHeightPixels = getScalingFactor() * height;
+
}
diff -Nru simbody-3.6.1+dfsg/Simbody/Visualizer/src/VisualizerProtocol.cpp simbody-3.7+dfsg/Simbody/Visualizer/src/VisualizerProtocol.cpp
--- simbody-3.6.1+dfsg/Simbody/Visualizer/src/VisualizerProtocol.cpp 2018-06-12 01:04:33.000000000 +0000
+++ simbody-3.7+dfsg/Simbody/Visualizer/src/VisualizerProtocol.cpp 2019-12-08 20:33:52.000000000 +0000
@@ -321,6 +321,16 @@
Pathname::addDirectoryOffset(def,
Pathname::addDirectoryOffset("SimTK", SIMBODY_VISUALIZER_REL_INSTALL_DIR)));
+ #if defined(__APPLE__)
+ for (auto& path : actualSearchPath) {
+ #ifndef NDEBUG
+ path += "/simbody-visualizer_d.app/Contents/MacOS/";
+ #else
+ path += "/simbody-visualizer.app/Contents/MacOS/";
+ #endif
+ }
+ #endif
+
// Pipe[0] is the read end, Pipe[1] is the write end.
int sim2vizPipe[2], viz2simPipe[2], status;
diff -Nru simbody-3.6.1+dfsg/SimTKcommon/Simulation/include/SimTKcommon/internal/Measure.h simbody-3.7+dfsg/SimTKcommon/Simulation/include/SimTKcommon/internal/Measure.h
--- simbody-3.6.1+dfsg/SimTKcommon/Simulation/include/SimTKcommon/internal/Measure.h 2018-06-12 01:04:33.000000000 +0000
+++ simbody-3.7+dfsg/SimTKcommon/Simulation/include/SimTKcommon/internal/Measure.h 2019-12-08 20:33:52.000000000 +0000
@@ -228,6 +228,8 @@
/// throw an exception if the Measure is not currently owned by any
/// Subsystem.
const Subsystem& getSubsystem() const;
+ /// Is getSubsystem() the same as the passed-in Subsystem?
+ bool isSameSubsystem(const Subsystem&) const;
/// Return the MeasureIndex by which this Measure is known to the Subsystem
/// that owns it. Will throw an exception if the Measure is not currently
/// owned by any Subsystem.
@@ -600,8 +602,8 @@
: Measure_(sub, new Implementation(left, right),
AbstractMeasure::SetHandle())
{ SimTK_ERRCHK_ALWAYS
- ( this->getSubsystem().isSameSubsystem(left.getSubsystem())
- && this->getSubsystem().isSameSubsystem(right.getSubsystem()),
+ ( this->isSameSubsystem(left.getSubsystem())
+ && this->isSameSubsystem(right.getSubsystem()),
"Measure_::Plus::ctor()",
"Arguments must be in the same Subsystem as this Measure.");
}
@@ -625,8 +627,8 @@
: Measure_(sub, new Implementation(left, right),
AbstractMeasure::SetHandle())
{ SimTK_ERRCHK_ALWAYS
- ( this->getSubsystem().isSameSubsystem(left.getSubsystem())
- && this->getSubsystem().isSameSubsystem(right.getSubsystem()),
+ ( this->isSameSubsystem(left.getSubsystem())
+ && this->isSameSubsystem(right.getSubsystem()),
"Measure_::Minus::ctor()",
"Arguments must be in the same Subsystem as this Measure.");
}
@@ -650,7 +652,7 @@
: Measure_(sub, new Implementation(factor, operand),
AbstractMeasure::SetHandle())
{ SimTK_ERRCHK_ALWAYS
- (this->getSubsystem().isSameSubsystem(operand.getSubsystem()),
+ (this->isSameSubsystem(operand.getSubsystem()),
"Measure_::Scale::ctor()",
"Argument must be in the same Subsystem as this Measure.");
}
diff -Nru simbody-3.6.1+dfsg/SimTKcommon/Simulation/include/SimTKcommon/internal/MeasureImplementation.h simbody-3.7+dfsg/SimTKcommon/Simulation/include/SimTKcommon/internal/MeasureImplementation.h
--- simbody-3.6.1+dfsg/SimTKcommon/Simulation/include/SimTKcommon/internal/MeasureImplementation.h 2018-06-12 01:04:33.000000000 +0000
+++ simbody-3.7+dfsg/SimTKcommon/Simulation/include/SimTKcommon/internal/MeasureImplementation.h 2019-12-08 20:33:52.000000000 +0000
@@ -229,6 +229,10 @@
getSubsystem() const
{ return getImpl().getSubsystem(); }
+inline bool AbstractMeasure::
+isSameSubsystem(const Subsystem& other) const
+{ return getSubsystem().isSameSubsystem(other); }
+
inline MeasureIndex AbstractMeasure::
getSubsystemMeasureIndex() const
{ return getImpl().getSubsystemMeasureIndex();}
diff -Nru simbody-3.6.1+dfsg/SimTKcommon/src/Pathname.cpp simbody-3.7+dfsg/SimTKcommon/src/Pathname.cpp
--- simbody-3.6.1+dfsg/SimTKcommon/src/Pathname.cpp 2018-06-12 01:04:33.000000000 +0000
+++ simbody-3.7+dfsg/SimTKcommon/src/Pathname.cpp 2019-12-08 20:33:52.000000000 +0000
@@ -446,6 +446,10 @@
SimTK_ERRCHK_ALWAYS(status==0,
"Pathname::getThisExecutablePath()",
"2048-byte buffer is not big enough to store executable path.");
+ #elif defined(__FreeBSD__) || defined(__DragonFly__)
+ // This isn't automatically null terminated.
+ const size_t nBytes = readlink("/proc/curproc/file", buf, sizeof(buf));
+ buf[nBytes] = '\0';
#else // Linux
// This isn't automatically null terminated.
const size_t nBytes = readlink("/proc/self/exe", buf, sizeof(buf));
diff -Nru simbody-3.6.1+dfsg/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes.c simbody-3.7+dfsg/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes.c
--- simbody-3.6.1+dfsg/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes.c 2018-06-12 01:04:33.000000000 +0000
+++ simbody-3.7+dfsg/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes.c 2019-12-08 20:33:52.000000000 +0000
@@ -1615,7 +1615,7 @@
cp_mem = (CPodeMem) (*cpode_mem);
- cpFreeVectors(cp_mem);
+ if (cp_mem->cp_MallocDone) cpFreeVectors(cp_mem);
CPodeQuadFree(cp_mem);
diff -Nru simbody-3.6.1+dfsg/SimTKmath/Optimizers/src/lbfgsb.cpp simbody-3.7+dfsg/SimTKmath/Optimizers/src/lbfgsb.cpp
--- simbody-3.6.1+dfsg/SimTKmath/Optimizers/src/lbfgsb.cpp 2018-06-12 01:04:33.000000000 +0000
+++ simbody-3.7+dfsg/SimTKmath/Optimizers/src/lbfgsb.cpp 2019-12-08 20:33:52.000000000 +0000
@@ -4134,42 +4134,42 @@
/* int f_open(), s_wsfe(), do_fio(), e_wsfe(); */
/* Local variables */
- int head;
- Real fold;
- int nact;
- Real ddum;
- int info;
- Real time;
- int nfgv, ifun, iter, nint;
- char word[5];
- Real time1, time2;
- int i__, iback, k;
- Real gdold;
- int nfree;
- bool boxed;
- int itail;
- Real theta;
- Real dnorm;
- int nskip, iword;
- Real xstep, stpmx;
- Real gd, dr, rr;
- int ileave;
- int itfile;
- Real cachyt, epsmch;
- bool updatd;
- Real sbtime;
- bool prjctd;
- int iupdat;
- bool cnstnd;
- Real sbgnrm;
- int nenter;
- Real lnscht;
- int nintol;
- Real dtd;
- int col;
- Real tol;
- bool wrk;
- Real stp, cpu1, cpu2;
+ int head{0};
+ Real fold{0};
+ int nact{0};
+ Real ddum{0};
+ int info{0};
+ Real time{0};
+ int nfgv{0}, ifun{0}, iter{0}, nint{0};
+ char word[5]{0, 0, 0, 0, 0};
+ Real time1{0}, time2{0};
+ int i__{0}, iback{0}, k{0};
+ Real gdold{0};
+ int nfree{0};
+ bool boxed{0};
+ int itail{0};
+ Real theta{0};
+ Real dnorm{0};
+ int nskip{0}, iword{0};
+ Real xstep{0}, stpmx{0};
+ Real gd{0}, dr{0}, rr{0};
+ int ileave{0};
+ int itfile{0};
+ Real cachyt{0}, epsmch{0};
+ bool updatd{0};
+ Real sbtime{0};
+ bool prjctd{0};
+ int iupdat{0};
+ bool cnstnd{0};
+ Real sbgnrm{0};
+ int nenter{0};
+ Real lnscht{0};
+ int nintol{0};
+ Real dtd{0};
+ int col{0};
+ Real tol{0};
+ bool wrk{0};
+ Real stp{0}, cpu1{0}, cpu2{0};
/* Fortran I/O blocks */
/*